Rev 1390 | Rev 1396 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1331 | giacomo | 1 | /* |
2 | * Project: S.Ha.R.K. |
||
3 | * |
||
4 | * Coordinators: Giorgio Buttazzo <giorgio@sssup.it> |
||
5 | * |
||
6 | * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
||
7 | * |
||
8 | * http://www.sssup.it |
||
9 | * http://retis.sssup.it |
||
10 | * http://shark.sssup.it |
||
11 | */ |
||
12 | |||
13 | #include "chimera.h" |
||
14 | |||
15 | //#define DEBUG_SEND /* Print Sent Values */ |
||
16 | #define SERIAL_ON /* Send Data using Serial Port */ |
||
17 | |||
18 | /* Servo Tasks Constants */ |
||
19 | #ifdef DUBUG_SEND |
||
1359 | giacomo | 20 | #define SEND_TASK_WCET 20000 |
1331 | giacomo | 21 | #else |
1359 | giacomo | 22 | #define SEND_TASK_WCET 20000 |
1331 | giacomo | 23 | #endif |
1391 | giacomo | 24 | #define SEND_TASK_MIT 30000 |
1331 | giacomo | 25 | |
26 | #define LEG_A 100.0 |
||
27 | #define LEG_B 66.0 |
||
28 | #define LEG_C 26.0 |
||
29 | #define LEG_D 38.0 |
||
30 | #define LEG_CD_2IPO 92.087 /* 2 * sqrt(LEG_C^2 + LEG_D^2) */ |
||
1338 | giacomo | 31 | #define LEG_CD_ANG 0.600 /* arctg(LEG_C/LEG_D) in radianti */ |
1331 | giacomo | 32 | |
33 | const float c0 = LEG_C * LEG_C; |
||
34 | const float c1 = LEG_B * LEG_B; |
||
35 | const float c2 = LEG_B * LEG_B - LEG_A * LEG_A; |
||
36 | const float todeg = 180.0 / PI; |
||
1336 | giacomo | 37 | const float torad = PI / 180.0; |
1331 | giacomo | 38 | |
39 | HEXAPOD_STATE status; |
||
40 | |||
1332 | giacomo | 41 | extern struct leg_calibration calibration_table[]; |
42 | |||
1331 | giacomo | 43 | void print_status(int n){ |
44 | } |
||
45 | |||
1336 | giacomo | 46 | int leg_to_ang(float px, float py, float pz, int *alfa, int *beta, int *gamma) |
1331 | giacomo | 47 | { |
48 | float px2 = px * px; |
||
49 | float py2 = py * py; |
||
50 | float pz2 = pz * pz; |
||
51 | |||
52 | float pxz2 = px2 + pz2; |
||
53 | |||
1338 | giacomo | 54 | float alfa1,beta1,alfa2,beta2,gamma1,gamma2; |
1331 | giacomo | 55 | float m,dsqrt; |
56 | |||
57 | float delta_xz = pxz2 - c0; |
||
58 | float s,k,k2,y1,delta_xy; |
||
59 | |||
60 | if (delta_xz < 0.0) return -1; |
||
61 | |||
62 | if (pz >= LEG_C) { |
||
1338 | giacomo | 63 | gamma2 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2); |
64 | gamma1 = gamma2 * todeg; |
||
1331 | giacomo | 65 | } else { |
1338 | giacomo | 66 | gamma2 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2); |
67 | gamma1 = gamma2 * todeg; |
||
1331 | giacomo | 68 | } |
69 | |||
1338 | giacomo | 70 | m = pxz2 - LEG_CD_2IPO * (pz * sin(gamma2+LEG_CD_ANG) + px * cos(gamma2+LEG_CD_ANG) - LEG_CD_2IPO / 4.0); |
1331 | giacomo | 71 | |
72 | s = m + py2; |
||
73 | k = c2 + s; |
||
74 | k2 = k * k; |
||
75 | |||
76 | delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1); |
||
77 | |||
78 | if (delta_xy >= 0.0) { |
||
79 | dsqrt = sqrt(delta_xy); |
||
80 | y1 = (py * k + dsqrt) / (2.0 * s); |
||
81 | beta1 = asin(y1/LEG_B) * todeg; |
||
82 | alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1; |
||
83 | y1 = (py * k - dsqrt) / (2.0 * s); |
||
84 | beta2 = asin(y1/LEG_B) * todeg; |
||
85 | alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2; |
||
86 | |||
87 | if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) { |
||
1336 | giacomo | 88 | *alfa = (int)(alfa1 * 3600.0); |
89 | *beta = (int)(beta1 * 3600.0); |
||
90 | *gamma = (int)(gamma1 * 3600.0); |
||
1331 | giacomo | 91 | return 0; |
92 | } else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) { |
||
1336 | giacomo | 93 | *alfa = (int)(alfa2 * 3600.0); |
94 | *beta = (int)(beta2 * 3600.0); |
||
95 | *gamma = (int)(gamma1 * 3600.0); |
||
1331 | giacomo | 96 | return 0; |
97 | } else { |
||
1336 | giacomo | 98 | return -1; |
1331 | giacomo | 99 | } |
100 | } else |
||
1336 | giacomo | 101 | return -1; |
1331 | giacomo | 102 | |
1336 | giacomo | 103 | return -1; |
104 | |||
1331 | giacomo | 105 | } |
106 | |||
1336 | giacomo | 107 | int ang_to_leg(int alfa, int beta, int gamma, float *px, float *py, float *pz) { |
108 | |||
109 | float alfa1 = (float)(alfa)/3600.0 * torad; |
||
110 | float beta1 = (float)(beta)/3600.0 * torad; |
||
111 | float sin_gamma = sin((float)(gamma)/3600.0 * torad); |
||
112 | float cos_gamma = cos((float)(gamma)/3600.0 * torad); |
||
113 | float m; |
||
114 | |||
115 | m = LEG_B * cos(beta1) + LEG_A * cos(alfa1 - beta1); |
||
116 | *py = LEG_B * sin(beta1) - LEG_A * sin(alfa1 - beta1); |
||
117 | |||
118 | *pz = (LEG_D + m) * sin_gamma + LEG_C * cos_gamma; |
||
119 | *px = (LEG_D + m) * cos_gamma - LEG_C * sin_gamma; |
||
120 | |||
121 | return 0; |
||
122 | |||
123 | } |
||
124 | |||
1333 | giacomo | 125 | void update_event_action(void) { |
126 | |||
127 | struct timespec t; |
||
128 | struct action_event *e; |
||
129 | int i; |
||
130 | |||
131 | kern_gettime(&t); |
||
132 | |||
133 | while ((e = get_first_old_event(&t)) != NULL) { |
||
134 | |||
135 | if (e->type == EVT_SET_MASK_LEG_ANGLE) { |
||
136 | |||
137 | for (i=0;i<6;i++) |
||
1359 | giacomo | 138 | if ((e->mask >> i) & 1) { |
1333 | giacomo | 139 | |
1336 | giacomo | 140 | status.ang[i].a = e->ang.a; |
141 | status.ang[i].b = e->ang.b; |
||
142 | status.ang[i].c = e->ang.c; |
||
143 | |||
144 | status.cfg[i].pwm = e->pwm; |
||
145 | |||
1390 | mauro | 146 | //#ifdef DEBUG_SEND |
1334 | giacomo | 147 | printf_xy(3,2,WHITE,"%8d: Update leg %2d angle",(int)kern_gettime(NULL),i); |
1390 | mauro | 148 | //#endif |
1333 | giacomo | 149 | |
150 | |||
151 | } |
||
152 | |||
153 | e->status = EVT_STATUS_DONE; |
||
154 | |||
155 | } |
||
156 | |||
157 | } |
||
158 | |||
159 | } |
||
160 | |||
1331 | giacomo | 161 | TASK servo_send() |
162 | { |
||
163 | HEXAPOD_STATE old_status; |
||
164 | register char changes, new_pos, new_pwm, new_power; |
||
1359 | giacomo | 165 | int res,n; |
1390 | mauro | 166 | struct timespec t; |
1391 | giacomo | 167 | int actual_leg = 0; |
1331 | giacomo | 168 | |
169 | for (n=0; n<6;n++) { |
||
1333 | giacomo | 170 | old_status.ang[n].a = 0; |
171 | old_status.ang[n].b = 0; |
||
172 | old_status.ang[n].c = 0; |
||
173 | old_status.cfg[n].pwm = 0; |
||
1331 | giacomo | 174 | } |
175 | old_status.power = 0; |
||
176 | |||
177 | while (1) { |
||
178 | changes = 0; |
||
1333 | giacomo | 179 | |
180 | update_event_action(); |
||
1331 | giacomo | 181 | |
182 | for (n=0; n<6; n++){ |
||
183 | new_pos = 0; |
||
184 | new_pwm = 0; |
||
185 | new_power = 0; |
||
186 | |||
1333 | giacomo | 187 | if ((status.ang[n].a != old_status.ang[n].a) || |
188 | (status.ang[n].b != old_status.ang[n].b) || |
||
189 | (status.ang[n].c != old_status.ang[n].c)) { |
||
190 | |||
191 | old_status.ang[n].a = status.ang[n].a; |
||
192 | old_status.ang[n].b = status.ang[n].b; |
||
193 | old_status.ang[n].c = status.ang[n].c; |
||
194 | new_pos++; |
||
195 | } |
||
196 | |||
197 | if (status.cfg[n].pwm != old_status.cfg[n].pwm) { |
||
198 | old_status.cfg[n].pwm = status.cfg[n].pwm; |
||
1331 | giacomo | 199 | new_pwm++; |
200 | } |
||
201 | if (status.power != old_status.power) { |
||
202 | old_status.power = status.power; |
||
203 | new_power++; |
||
204 | } |
||
205 | |||
206 | if (new_pos) { |
||
207 | #ifdef SERIAL_ON |
||
1391 | giacomo | 208 | task_nopreempt(); |
1359 | giacomo | 209 | res = servo_set_angle_sec(com(n*3 ), pin(n*3 ), adjust(status.ang[n].a,n,0)); |
210 | if (res != 0) cprintf("Error send data\n"); |
||
211 | res = servo_set_angle_sec(com(n*3+1), pin(n*3+1), adjust(status.ang[n].b,n,1)); |
||
212 | if (res != 0) cprintf("Error send data\n"); |
||
213 | res = servo_set_angle_sec(com(n*3+2), pin(n*3+2), adjust(status.ang[n].c,n,2)); |
||
214 | if (res != 0) cprintf("Error send data\n"); |
||
1391 | giacomo | 215 | task_preempt(); |
1331 | giacomo | 216 | #endif |
1359 | giacomo | 217 | |
1331 | giacomo | 218 | } |
219 | |||
220 | if (new_pwm) { |
||
221 | #ifdef SERIAL_ON |
||
1391 | giacomo | 222 | task_nopreempt(); |
1359 | giacomo | 223 | (old_status.cfg[n].pwm & 1) ? servo_turn_on(com(n*3 ), pin(n*3 )) : servo_turn_off(com(n*3 ), pin(n*3 )); |
224 | (old_status.cfg[n].pwm & 2) ? servo_turn_on(com(n*3+1), pin(n*3+1)) : servo_turn_off(com(n*3+1), pin(n*3+1)); |
||
225 | (old_status.cfg[n].pwm & 4) ? servo_turn_on(com(n*3+2), pin(n*3+2)) : servo_turn_off(com(n*3+2), pin(n*3+2)); |
||
1391 | giacomo | 226 | task_preempt(); |
1331 | giacomo | 227 | #endif |
228 | } |
||
229 | if (new_power) { |
||
230 | #ifdef SERIAL_ON |
||
1391 | giacomo | 231 | task_nopreempt(); |
1331 | giacomo | 232 | if (old_status.power) { |
1359 | giacomo | 233 | servo_set_RC5_switch(COM2, 1); |
1331 | giacomo | 234 | } else { |
1359 | giacomo | 235 | servo_set_RC5_switch(COM2, 0); |
1331 | giacomo | 236 | } |
1391 | giacomo | 237 | task_preempt(); |
1331 | giacomo | 238 | #endif |
239 | } |
||
240 | |||
241 | if (new_pos || new_pwm || new_power) { |
||
242 | changes++; |
||
243 | #ifdef DEBUG_SEND |
||
244 | print_status(n); |
||
245 | #endif |
||
246 | } |
||
247 | } |
||
248 | |||
1391 | giacomo | 249 | if (!new_pos) { |
1331 | giacomo | 250 | |
1391 | giacomo | 251 | task_nopreempt(); |
252 | status.cfg[actual_leg].adc_in = servo_get_analog(COM1, actual_leg); |
||
253 | task_preempt(); |
||
254 | |||
255 | sys_gettime(&t); |
||
256 | printf_xy(1,20,WHITE,"(%d) (%d) (%d) (%d) (%d) (%d) ", |
||
257 | status.cfg[0].adc_in, |
||
258 | status.cfg[1].adc_in, |
||
259 | status.cfg[2].adc_in, |
||
260 | status.cfg[3].adc_in, |
||
261 | status.cfg[4].adc_in, |
||
262 | status.cfg[5].adc_in); |
||
263 | actual_leg = (actual_leg+1)%6; |
||
264 | |||
265 | } |
||
266 | |||
1331 | giacomo | 267 | task_endcycle(); |
268 | } |
||
269 | |||
270 | return 0; |
||
271 | } |
||
272 | |||
273 | int init_serial() |
||
274 | { |
||
275 | int err; |
||
1359 | giacomo | 276 | err = servo_open(COM1, COM_SPEED); |
277 | if (!err) |
||
278 | err = servo_open(COM2, COM_SPEED); |
||
1331 | giacomo | 279 | |
280 | return err; |
||
281 | } |
||
282 | |||
283 | void end_serial() |
||
284 | { |
||
1359 | giacomo | 285 | servo_close(COM1); |
286 | servo_close(COM2); |
||
1331 | giacomo | 287 | } |
288 | |||
289 | void init_send_task() |
||
290 | { |
||
1359 | giacomo | 291 | HARD_TASK_MODEL ms; |
1331 | giacomo | 292 | PID pid; |
293 | |||
1359 | giacomo | 294 | hard_task_default_model(ms); |
295 | hard_task_def_ctrl_jet(ms); |
||
296 | hard_task_def_wcet(ms, SEND_TASK_WCET); |
||
297 | hard_task_def_mit(ms, SEND_TASK_MIT); |
||
298 | hard_task_def_usemath(ms); |
||
1391 | giacomo | 299 | pid = task_create("Servo_Task", servo_send, &ms, NULL); |
1331 | giacomo | 300 | if (pid == NIL) { |
301 | perror("Could not create task <Send_Task>"); |
||
302 | sys_end(); |
||
303 | } else |
||
304 | task_activate(pid); |
||
305 | |||
306 | } |
||
307 | |||
308 | void init_send() |
||
309 | { |
||
310 | int i; |
||
311 | |||
312 | if (init_serial()) { |
||
313 | perror("Could not initialize serial port."); |
||
314 | sys_end(); |
||
315 | } |
||
316 | |||
317 | for (i=0; i<6;i++) { |
||
1333 | giacomo | 318 | status.ang[i].a = 0; |
319 | status.ang[i].b = 0; |
||
320 | status.ang[i].c = 0; |
||
321 | status.cfg[i].pwm = 0; |
||
1331 | giacomo | 322 | } |
323 | status.power = 0; |
||
324 | |||
325 | init_send_task(); |
||
1336 | giacomo | 326 | |
1331 | giacomo | 327 | } |
328 | |||
329 | void end_send() |
||
330 | { |
||
331 | end_serial(); |
||
332 | } |