1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126 | // a set of subroutines that generates PWM signals to control RC/hobby servos.
/* ----- revision history:
this file has been modified and tailored for Superbot - calibration, etc.
we use timer3 interrupt to start the pulse,
and timer1 for timing the pulse duration and ending the pulse.
port C is where all the servo lines are output.
2006-08-05, Jimmy Sastra: Today I started this revision history.
- turned off sending PM for servo position.
2006-08-07, Mike Park:
- turned PM for servo back on. User can modulate frequency in ConfigTool or GaitComposer
*/
#use FAST_IO(C)
// FAST_IO prevents the compiler from inserting extra instructions to set TRISC.
// to avoid slow EEPROM reads, keep a copy of these values in RAM:
// to avoid needless computation, calculate these values on reset only once.
float commandScaleFactor;
sint16 commandCenter, tmpAmplitude, tmps;
uint16 tmp;
uint8 s;
void RCservo_init() {
/* Ignoring all servo communications
uint16 D16,P;
uint8 D,PL,PH;
PL=HSR_ReadMemory(_HSR_MEM_P1L);
PH=HSR_ReadMemory(_HSR_MEM_P1H);
P=make16(PH,PL);
D=HSR_ReadMemory(_HSR_MEM_P1D);
D16=make16(0,D);
RBReadEEPROM(&tmp,SERVO_KP,2);
if ( tmp!=0 )
{HSR_WriteMemory(_HSR_MEM_P1H,make8(tmp,1));
HSR_WriteMemory(_HSR_MEM_P1L,make8(tmp,0));
HSR_SelectControl(1);
}
else
{RBSetEEPROM(&P,SERVO_KP, 2);
HSR_SelectControl(3);}
RBReadEEPROM(&tmp,SERVO_KD,2);
if ( tmp!=0 )
{HSR_WriteMemory(_HSR_MEM_P1D,make8(tmp,0));
HSR_SelectControl(1);
}
else
{RBSetEEPROM(&D16,SERVO_KD, 2);
HSR_SelectControl(3);}
*/
calMode = 0; // set normal mode
RBReadEEPROM(&tmp,SERVO_SPEED,2);
s=make8(tmp,0);
ServoSpeed=s;
RBReadEEPROM(&commandCenter, SERVO_CAL_CMD_CENTER, 2);
RBReadEEPROM(&tmpAmplitude, SERVO_CAL_CMD_AMPLITUDE, 2);
commandScaleFactor = ((float) tmpAmplitude) / ((float) 9000);
RBReadEEPROM(&tmps,SERVO_START_POS, 2);
servoPosCommand=tmps;
RBReadEEPROM(&feed_freq,FEEDBACK, 2);
if ( feed_freq>0 )
feed_int = URB_HEARTBEAT_INTERVAL/feed_freq;
}
static sint16 TAngle = 0; //Target Position -9000 -- 9000
static sint16 CAngle = 0; //Callibration Target Position -9000 -- 9000
sint16 TargetPos;
uint8 TargetPosH;
uint8 TargetPosL;
sint16 TargetAngle;
uint16 Timer_100 = ((int)URB_HEARTBEAT_INTERVAL)/100; // Timer count for 60 Hz
void initiate_pulse() {
if (calMode)
{
if (CAngle!=servoPosCommand)
{CAngle=servoPosCommand;
if ( !( (servoPosCommand > 9000)||(servoPosCommand < -9000)) )
{
TargetAngle=servoPosCommand;
TargetPos = TargetAngle+14500;
TargetPos = TargetPos/10;
TargetPosH = make8(TargetPos,1);
TargetPosL = make8(TargetPos,0);
HSR_WriteMemory_v2(_HSR_MEM_Speed,ServoSpeed);
HSR_WriteMemory_v2(_HSR_MEM_CurSpeed, ServoSpeed);
HSR_SetPosition(TargetPosH,TargetPosL);
}
}
if(get_timer0() >= Timer_100*feed_mult)
{ raw_adc = HSR_ReadPosition();
output_toggle(PIN_D5);
feed_mult++;
}
}
else
{
if (TAngle!=servoPosCommand)
{TAngle=servoPosCommand;
if( (servoPosCommand > 9000)||(servoPosCommand < -9000)) {
HSR_Release_v2(); //Go Limp
delay_ms(20); //WARNING: Removing this will make the command unreliable
}
else {
TargetAngle = commandCenter + (sint16)(((float) servoPosCommand) * commandScaleFactor);
TargetPos = TargetAngle+14500;
TargetPos = TargetPos/10;
TargetPosH = make8(TargetPos,1);
TargetPosL = make8(TargetPos,0);
HSR_WriteMemory_v2(_HSR_MEM_Speed,ServoSpeed);
HSR_WriteMemory_v2(_HSR_MEM_CurSpeed, ServoSpeed);
HSR_SetPosition(TargetPosH,TargetPosL);
output_toggle(Pin_D7);
}
}
}
}
|