root / branches / autonomous_recharging / code / projects / colonet / utilities / robot_slave / servo.c @ 1390
History | View | Annotate | Download (5.44 KB)
1 |
#include "servo.h" |
---|---|
2 |
|
3 |
|
4 |
#define SERVO_COUNT 16 //please dont change this, or bad things might happen |
5 |
|
6 |
|
7 |
|
8 |
struct SERVO_NODE {
|
9 |
uint8_t num; |
10 |
uint16_t angle; |
11 |
}; |
12 |
|
13 |
struct SERVO_CHANGE {
|
14 |
uint16_t port_val; |
15 |
uint16_t split_time_period; |
16 |
}; |
17 |
|
18 |
|
19 |
struct SERVO_STATUS_STRUCT {
|
20 |
struct SERVO_NODE servos[SERVO_COUNT];
|
21 |
uint16_t servo_angles[SERVO_COUNT]; |
22 |
struct SERVO_CHANGE changes[SERVO_COUNT+1]; |
23 |
uint8_t change_count; |
24 |
uint16_t ms; |
25 |
uint8_t new_angles; |
26 |
uint8_t in_pulse; |
27 |
uint8_t current_servo; |
28 |
|
29 |
} SERVO_STATUS; |
30 |
|
31 |
|
32 |
void servo_sort(void); |
33 |
void servo_setup_pulse(void); |
34 |
void servo_set_next_interrupt(uint16_t ticks, struct SERVO_CHANGE *sn); |
35 |
|
36 |
|
37 |
|
38 |
|
39 |
SIGNAL (SIG_OUTPUT_COMPARE3B){ |
40 |
|
41 |
if (SERVO_STATUS.in_pulse) {
|
42 |
|
43 |
//pull the correct ones down
|
44 |
SERVOPORT1 &= (~SERVOMASK1)|SERVO_STATUS.changes[SERVO_STATUS.current_servo].port_val; |
45 |
SERVOPORT2 &= (~SERVOMASK2)|(SERVO_STATUS.changes[SERVO_STATUS.current_servo].port_val >> 8);
|
46 |
|
47 |
++SERVO_STATUS.current_servo; //now look at next servo
|
48 |
|
49 |
if (SERVO_STATUS.current_servo < SERVO_STATUS.change_count) { //if it isnt the end... |
50 |
|
51 |
//setup timer for next pull down
|
52 |
OCR3B = SERVO_STATUS.changes[SERVO_STATUS.current_servo].split_time_period; |
53 |
TCNT3 = 0;
|
54 |
} |
55 |
else { //we are done with these pulses |
56 |
// SERVO_STATUS.in_pulse = 0;
|
57 |
// SERVO_STATUS.ms = 0;
|
58 |
// OCR3B += SERVO_MS; //NOTE--this might mean the negative width is a function of max pos width
|
59 |
// TCNT3 = 0;
|
60 |
servo_setup_pulse(); |
61 |
} |
62 |
|
63 |
} |
64 |
else { //we are between pulses |
65 |
++SERVO_STATUS.ms; //increment ms counter
|
66 |
|
67 |
if (SERVO_STATUS.ms >= SERVO_PERIOD) { //see how many ms have passed |
68 |
servo_setup_pulse(); //Seems like we could do this while not on the clock, but who knows
|
69 |
} |
70 |
else {
|
71 |
OCR3B = SERVO_MS; //go another ms
|
72 |
TCNT3 = 0;
|
73 |
|
74 |
} |
75 |
} |
76 |
} |
77 |
|
78 |
|
79 |
|
80 |
void servo_init(void) { |
81 |
int i;
|
82 |
uint8_t mysreg; |
83 |
|
84 |
SERVODDR1 |= SERVOMASK1; //all outputs
|
85 |
SERVODDR2 |= SERVOMASK2; |
86 |
|
87 |
mysreg=SREG; |
88 |
cli(); //turn off interrupts for now
|
89 |
|
90 |
//init everything
|
91 |
|
92 |
for (i = 0; i < SERVO_COUNT; ++i) { |
93 |
SERVO_STATUS.servos[i].num = i; |
94 |
SERVO_STATUS.servos[i].angle = 127;
|
95 |
SERVO_STATUS.servo_angles[i] = 127;
|
96 |
} |
97 |
|
98 |
SERVO_STATUS.ms = 0;
|
99 |
SERVO_STATUS.new_angles = 1;
|
100 |
SERVO_STATUS.in_pulse = 0;
|
101 |
SERVO_STATUS.change_count = 0;
|
102 |
|
103 |
//init timer3
|
104 |
TCCR3A = 0;
|
105 |
TCCR3B = _BV(CS31); //prescale = 8
|
106 |
TCCR3C=0;
|
107 |
TCNT3=0;
|
108 |
ETIMSK |= _BV(OCIE3B); |
109 |
OCR3B = SERVO_MS; |
110 |
|
111 |
SREG=mysreg; |
112 |
} |
113 |
|
114 |
void servo_set_angle(int servo, int angle) { |
115 |
uint8_t mysreg; |
116 |
if(servo>=8){ //2nd port flipped (on avrxbee at least) |
117 |
servo= 23-servo;
|
118 |
} |
119 |
if (SERVO_STATUS.servo_angles[servo] != angle) {
|
120 |
mysreg=SREG; |
121 |
cli(); |
122 |
SERVO_STATUS.servo_angles[servo] = angle; |
123 |
SERVO_STATUS.new_angles = 1;
|
124 |
SREG=mysreg; |
125 |
} |
126 |
} |
127 |
|
128 |
|
129 |
void servo_sort(void) { |
130 |
int done = 0, i; |
131 |
|
132 |
while (! done) {
|
133 |
done = 1;
|
134 |
|
135 |
for (i = 0; i < SERVO_COUNT - 1; ++i) { //loop through all |
136 |
|
137 |
//if they are out of order, swap them
|
138 |
if (SERVO_STATUS.servos[i].angle > SERVO_STATUS.servos[i+1].angle) { |
139 |
SERVO_STATUS.servos[i].angle ^= SERVO_STATUS.servos[i+1].angle;
|
140 |
SERVO_STATUS.servos[i+1].angle ^= SERVO_STATUS.servos[i].angle;
|
141 |
SERVO_STATUS.servos[i].angle ^= SERVO_STATUS.servos[i+1].angle;
|
142 |
|
143 |
SERVO_STATUS.servos[i].num ^= SERVO_STATUS.servos[i+1].num;
|
144 |
SERVO_STATUS.servos[i+1].num ^= SERVO_STATUS.servos[i].num;
|
145 |
SERVO_STATUS.servos[i].num ^= SERVO_STATUS.servos[i+1].num;
|
146 |
|
147 |
done = 0;
|
148 |
} |
149 |
} |
150 |
} |
151 |
} |
152 |
|
153 |
void servo_setup_pulse(void) { |
154 |
int i;
|
155 |
uint16_t my_port; |
156 |
uint16_t sum = 0;
|
157 |
uint16_t split_time; |
158 |
|
159 |
my_port = 0xffff; //all on |
160 |
|
161 |
if (SERVO_STATUS.new_angles) {
|
162 |
|
163 |
SERVO_STATUS.change_count = 0;
|
164 |
|
165 |
for (i = 0; i < SERVO_COUNT; ++i) { |
166 |
SERVO_STATUS.servos[i].angle = SERVO_STATUS.servo_angles[SERVO_STATUS.servos[i].num]; |
167 |
} |
168 |
|
169 |
servo_sort(); |
170 |
SERVO_STATUS.new_angles = 0;
|
171 |
|
172 |
for (i = 0; i < SERVO_COUNT; ++i) { |
173 |
split_time = /*SERVO_MIN + ((((((long int)(SERVO_MAX-SERVO_MIN)) >> 3) ) *
|
174 |
SERVO_STATUS.servos[i].angle) >> 5) */SERVO_STATUS.servos[i].angle - sum;
|
175 |
my_port &= ~_BV(SERVO_STATUS.servos[i].num); |
176 |
for (; i < SERVO_COUNT - 1 && SERVO_STATUS.servos[i].angle == SERVO_STATUS.servos[i+1].angle; ++i) { |
177 |
my_port &= ~_BV(SERVO_STATUS.servos[i+1].num);
|
178 |
} |
179 |
|
180 |
SERVO_STATUS.changes[SERVO_STATUS.change_count].port_val = my_port; |
181 |
servo_set_next_interrupt(split_time, &SERVO_STATUS.changes[SERVO_STATUS.change_count]); |
182 |
|
183 |
++SERVO_STATUS.change_count; |
184 |
|
185 |
sum += split_time; |
186 |
} |
187 |
|
188 |
SERVO_STATUS.changes[SERVO_STATUS.change_count].port_val = my_port; |
189 |
servo_set_next_interrupt(SERVO_RESET - split_time, &SERVO_STATUS.changes[SERVO_STATUS.change_count]); |
190 |
|
191 |
++SERVO_STATUS.change_count; |
192 |
|
193 |
} |
194 |
|
195 |
|
196 |
|
197 |
SERVO_STATUS.current_servo = 0;
|
198 |
SERVO_STATUS.in_pulse = 1;
|
199 |
|
200 |
|
201 |
SERVOPORT1 |= SERVOMASK1; |
202 |
SERVOPORT2 |= SERVOMASK2; |
203 |
|
204 |
OCR3B = SERVO_STATUS.changes[0].split_time_period;
|
205 |
TCNT3=0;
|
206 |
} |
207 |
|
208 |
void servo_set_next_interrupt(uint16_t ticks, struct SERVO_CHANGE *sn) { |
209 |
//this function is a holdover from when we used a worse clock.
|
210 |
|
211 |
(*sn).split_time_period = ticks; |
212 |
|
213 |
} |