Rev 1333 | Rev 1335 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1331 | giacomo | 1 | |
2 | /* |
||
3 | * Project: S.Ha.R.K. |
||
4 | * |
||
5 | * Coordinators: Giorgio Buttazzo <giorgio@sssup.it> |
||
6 | * |
||
7 | * Authors : Paolo Gai <pj@hartik.sssup.it> |
||
8 | * (see authors.txt for full list of hartik's authors) |
||
9 | * |
||
10 | * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
||
11 | * |
||
12 | * http://www.sssup.it |
||
13 | * http://retis.sssup.it |
||
14 | * http://shark.sssup.it |
||
15 | */ |
||
16 | |||
17 | /* |
||
18 | * Copyright (C) 2000 Paolo Gai |
||
19 | * |
||
20 | * This program is free software; you can redistribute it and/or modify |
||
21 | * it under the terms of the GNU General Public License as published by |
||
22 | * the Free Software Foundation; either version 2 of the License, or |
||
23 | * (at your option) any later version. |
||
24 | * |
||
25 | * This program is distributed in the hope that it will be useful, |
||
26 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
27 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
28 | * GNU General Public License for more details. |
||
29 | * |
||
30 | * You should have received a copy of the GNU General Public License |
||
31 | * along with this program; if not, write to the Free Software |
||
32 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
||
33 | * |
||
34 | */ |
||
35 | |||
36 | #include "chimera.h" |
||
37 | |||
38 | //#define DEBUG_SEND /* Print Sent Values */ |
||
39 | #define SERIAL_ON /* Send Data using Serial Port */ |
||
40 | |||
41 | /* Servo Tasks Constants */ |
||
42 | #ifdef DUBUG_SEND |
||
43 | #define SEND_TASK_WCET 6000 |
||
44 | #else |
||
1332 | giacomo | 45 | #define SEND_TASK_WCET 6000 |
1331 | giacomo | 46 | #endif |
47 | #define SEND_TASK_MIT 20000 |
||
48 | |||
49 | #ifdef DUBUG_SEND |
||
1332 | giacomo | 50 | #define GET_TASK_WCET 6000 |
1331 | giacomo | 51 | #else |
1332 | giacomo | 52 | #define GET_TASK_WCET 6000 |
1331 | giacomo | 53 | #endif |
54 | #define GET_TASK_MIT 20000 |
||
55 | |||
56 | #define LEG_A 100.0 |
||
57 | #define LEG_B 66.0 |
||
58 | #define LEG_C 26.0 |
||
59 | #define LEG_D 38.0 |
||
60 | #define LEG_CD_2IPO 92.087 /* 2 * sqrt(LEG_C^2 + LEG_D^2) */ |
||
61 | #define LEG_CD_ANG 34.380 /* arctg(LEG_C/LEG_D) in gradi */ |
||
62 | |||
63 | const float c0 = LEG_C * LEG_C; |
||
64 | const float c1 = LEG_B * LEG_B; |
||
65 | const float c2 = LEG_B * LEG_B - LEG_A * LEG_A; |
||
66 | const float todeg = 180.0 / PI; |
||
67 | |||
68 | sem_t mx_status, mx_servo; |
||
69 | HEXAPOD_STATE status; |
||
70 | ANGLES_STATE angles[6]; |
||
71 | |||
1332 | giacomo | 72 | extern struct leg_calibration calibration_table[]; |
73 | |||
1331 | giacomo | 74 | void print_status(int n){ |
75 | int i; |
||
76 | |||
77 | if (n>=6) { |
||
78 | for (i=0; i<6;i++) |
||
1333 | giacomo | 79 | cprintf("Leg %1d: ( %3d | %3d | %3d ) (%d)\n", i, status.leg[i].x, status.leg[i].y, status.leg[i].z, status.cfg[i].pwm ); |
1331 | giacomo | 80 | cprintf("Power: %d", status.power); |
81 | cprintf("\n"); |
||
82 | } else { |
||
1333 | giacomo | 83 | cprintf("Leg %1d: ( %3d | %3d | %3d ) (%1d-%1d)\n", n, status.leg[n].x,status.leg[n].y,status.leg[n].z, status.cfg[n].pwm, status.power); |
1331 | giacomo | 84 | } |
85 | } |
||
86 | |||
87 | int set_leg_position(int n, float px, float py, float pz) |
||
88 | { |
||
89 | float px2 = px * px; |
||
90 | float py2 = py * py; |
||
91 | float pz2 = pz * pz; |
||
92 | |||
93 | float pxz2 = px2 + pz2; |
||
94 | |||
95 | float alfa1,beta1,alfa2,beta2,gamma1; |
||
96 | float m,dsqrt; |
||
97 | |||
98 | float delta_xz = pxz2 - c0; |
||
99 | float s,k,k2,y1,delta_xy; |
||
100 | |||
101 | if (delta_xz < 0.0) return -1; |
||
102 | |||
103 | if (pz >= LEG_C) { |
||
104 | gamma1 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg; |
||
105 | } else { |
||
106 | gamma1 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg; |
||
107 | } |
||
108 | |||
109 | if (gamma1 < -90.0 || gamma1 > 90.0) return -1; |
||
110 | |||
111 | m = pxz2 - LEG_CD_2IPO * (sin(gamma1+LEG_CD_ANG)+cos(gamma1+LEG_CD_ANG)); |
||
112 | |||
113 | s = m + py2; |
||
114 | k = c2 + s; |
||
115 | k2 = k * k; |
||
116 | |||
117 | delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1); |
||
118 | |||
119 | if (delta_xy >= 0.0) { |
||
120 | dsqrt = sqrt(delta_xy); |
||
121 | y1 = (py * k + dsqrt) / (2.0 * s); |
||
122 | beta1 = asin(y1/LEG_B) * todeg; |
||
123 | alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1; |
||
124 | y1 = (py * k - dsqrt) / (2.0 * s); |
||
125 | beta2 = asin(y1/LEG_B) * todeg; |
||
126 | alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2; |
||
127 | |||
128 | if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) { |
||
129 | angles[n].a = (int)(alfa1 * 3600.0); |
||
130 | angles[n].b = (int)(beta1 * 3600.0); |
||
131 | angles[n].c = (int)(gamma1 * 3600.0); |
||
132 | #ifdef DEBUG_SEND |
||
133 | cprintf("Leg %d: ( %3d | %3d | %3d )\n", n, angles[n].a, angles[n].b, angles[n].c); |
||
134 | #endif |
||
135 | return 0; |
||
136 | } else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) { |
||
137 | angles[n].a = (int)(alfa2 * 3600.0); |
||
138 | angles[n].b = (int)(beta2 * 3600.0); |
||
139 | angles[n].c = (int)(gamma1 * 3600.0); |
||
140 | #ifdef DEBUG_SEND |
||
141 | cprintf("Leg %d: ( %3d | %3d | %3d )\n", n, angles[n].a, angles[n].b, angles[n].c); |
||
142 | #endif |
||
143 | return 0; |
||
144 | } else { |
||
145 | #ifdef DEBUG_SEND |
||
146 | cprintf("No possible !\n"); |
||
147 | #endif |
||
148 | return 1; |
||
149 | } |
||
150 | } else |
||
151 | return 1; |
||
152 | |||
153 | return 0; |
||
154 | } |
||
155 | |||
1333 | giacomo | 156 | void update_event_action(void) { |
157 | |||
158 | struct timespec t; |
||
159 | struct action_event *e; |
||
160 | int i; |
||
161 | |||
162 | kern_gettime(&t); |
||
163 | |||
164 | while ((e = get_first_old_event(&t)) != NULL) { |
||
165 | |||
166 | if (e->type == EVT_SET_MASK_LEG_POS) { |
||
167 | |||
168 | e->status = EVT_STATUS_DONE; |
||
169 | |||
170 | } |
||
171 | |||
172 | if (e->type == EVT_SET_MASK_LEG_ANGLE) { |
||
173 | |||
174 | for (i=0;i<6;i++) |
||
175 | if ((1 << i) & e->mask) { |
||
176 | |||
177 | status.ang[i].a = angles[i].a = *(int *)(e->data); |
||
178 | status.ang[i].b = angles[i].b = *(int *)(e->data+4); |
||
179 | status.ang[i].c = angles[i].c = *(int *)(e->data+8); |
||
1334 | giacomo | 180 | status.cfg[i].pwm = *(unsigned char *)(e->data+12); |
1333 | giacomo | 181 | |
182 | //#ifdef DEBUG_SEND |
||
1334 | giacomo | 183 | printf_xy(3,2,WHITE,"%8d: Update leg %2d angle",(int)kern_gettime(NULL),i); |
1333 | giacomo | 184 | //#endif |
185 | |||
186 | |||
187 | } |
||
188 | |||
189 | e->status = EVT_STATUS_DONE; |
||
190 | |||
191 | } |
||
192 | |||
193 | } |
||
194 | |||
195 | } |
||
196 | |||
1331 | giacomo | 197 | TASK servo_send() |
198 | { |
||
199 | HEXAPOD_STATE old_status; |
||
200 | register char changes, new_pos, new_pwm, new_power; |
||
201 | int n; |
||
202 | |||
203 | for (n=0; n<6;n++) { |
||
204 | old_status.leg[n].x = 150; |
||
1333 | giacomo | 205 | old_status.leg[n].y = 0; |
206 | old_status.leg[n].z = 0; |
||
207 | old_status.ang[n].a = 0; |
||
208 | old_status.ang[n].b = 0; |
||
209 | old_status.ang[n].c = 0; |
||
210 | old_status.cfg[n].pwm = 0; |
||
1331 | giacomo | 211 | } |
212 | old_status.power = 0; |
||
213 | |||
214 | while (1) { |
||
215 | changes = 0; |
||
1333 | giacomo | 216 | |
217 | update_event_action(); |
||
1331 | giacomo | 218 | |
219 | for (n=0; n<6; n++){ |
||
220 | new_pos = 0; |
||
221 | new_pwm = 0; |
||
222 | new_power = 0; |
||
223 | |||
1333 | giacomo | 224 | sem_wait(&mx_status); |
1331 | giacomo | 225 | if ((status.leg[n].x != old_status.leg[n].x) || (status.leg[n].y != old_status.leg[n].y) || (status.leg[n].z != old_status.leg[n].z)) { |
226 | if (set_leg_position(n, status.leg[n].x, status.leg[n].y, status.leg[n].z)==0) { |
||
227 | old_status.leg[n].x = status.leg[n].x; |
||
228 | old_status.leg[n].y = status.leg[n].y; |
||
229 | old_status.leg[n].z = status.leg[n].z; |
||
230 | new_pos++; |
||
231 | } else { |
||
232 | #ifdef DEBUG_SEND |
||
233 | cprintf("Position not reachable.\n"); |
||
234 | #endif |
||
235 | status.leg[n].x = old_status.leg[n].x; |
||
236 | status.leg[n].y = old_status.leg[n].y; |
||
237 | status.leg[n].z = old_status.leg[n].z; |
||
238 | } |
||
239 | } |
||
1333 | giacomo | 240 | |
241 | if ((status.ang[n].a != old_status.ang[n].a) || |
||
242 | (status.ang[n].b != old_status.ang[n].b) || |
||
243 | (status.ang[n].c != old_status.ang[n].c)) { |
||
244 | |||
245 | old_status.ang[n].a = status.ang[n].a; |
||
246 | old_status.ang[n].b = status.ang[n].b; |
||
247 | old_status.ang[n].c = status.ang[n].c; |
||
248 | new_pos++; |
||
249 | } |
||
250 | |||
251 | if (status.cfg[n].pwm != old_status.cfg[n].pwm) { |
||
252 | old_status.cfg[n].pwm = status.cfg[n].pwm; |
||
1331 | giacomo | 253 | new_pwm++; |
254 | } |
||
255 | if (status.power != old_status.power) { |
||
256 | old_status.power = status.power; |
||
257 | new_power++; |
||
258 | } |
||
1333 | giacomo | 259 | sem_post(&mx_status); |
1331 | giacomo | 260 | |
261 | if (new_pos) { |
||
262 | #ifdef SERIAL_ON |
||
263 | sem_wait(&mx_servo); |
||
1332 | giacomo | 264 | servo_set_angle_sec(COM_PORT, n*3+2, adjust(angles[n].a,n,0)); |
265 | servo_set_angle_sec(COM_PORT, n*3+1, adjust(angles[n].b,n,1)); |
||
266 | servo_set_angle_sec(COM_PORT, n*3 , adjust(angles[n].c,n,2)); |
||
1331 | giacomo | 267 | sem_post(&mx_servo); |
1332 | giacomo | 268 | |
269 | printf_xy(3,3,WHITE,"PX = %05d PY = %05d PZ = %05d",(int)(status.leg[n].x) |
||
270 | ,(int)(status.leg[n].y) |
||
271 | ,(int)(status.leg[n].z)); |
||
272 | |||
1333 | giacomo | 273 | printf_xy(3,4,WHITE,"ALFA = %07d BETA = %07d GAMMA = %07d",(int)(angles[n].a/3600) |
274 | ,(int)(angles[n].b/3600) |
||
275 | ,(int)(angles[n].c/3600)); |
||
1332 | giacomo | 276 | |
1333 | giacomo | 277 | printf_xy(3,5,WHITE,"ADJ_ALFA = %07d ADJ_BETA = %07d ADJ_GAMMA = %07d",adjust(angles[n].a,n,0)/3600,adjust(angles[n].b,n,1)/3600,adjust(angles[n].c,n,2)/3600); |
1332 | giacomo | 278 | |
1331 | giacomo | 279 | #endif |
280 | } |
||
281 | |||
282 | if (new_pwm) { |
||
283 | #ifdef SERIAL_ON |
||
284 | sem_wait(&mx_servo); |
||
1333 | giacomo | 285 | (old_status.cfg[n].pwm & 1) ? servo_turn_on(COM_PORT, n*3+2) : servo_turn_off(COM_PORT, n*3+2); |
286 | (old_status.cfg[n].pwm & 2) ? servo_turn_on(COM_PORT, n*3+1) : servo_turn_off(COM_PORT, n*3+1); |
||
287 | (old_status.cfg[n].pwm & 4) ? servo_turn_on(COM_PORT, n*3 ) : servo_turn_off(COM_PORT, n*3 ); |
||
1331 | giacomo | 288 | sem_post(&mx_servo); |
289 | #endif |
||
290 | } |
||
291 | if (new_power) { |
||
292 | #ifdef SERIAL_ON |
||
293 | if (old_status.power) { |
||
294 | sem_wait(&mx_servo); |
||
295 | servo_set_RC5_switch(COM_PORT, 1); |
||
296 | sem_post(&mx_servo); |
||
297 | } else { |
||
298 | sem_wait(&mx_servo); |
||
299 | servo_set_RC5_switch(COM_PORT, 0); |
||
300 | sem_post(&mx_servo); |
||
301 | } |
||
302 | #endif |
||
303 | } |
||
304 | |||
305 | if (new_pos || new_pwm || new_power) { |
||
306 | changes++; |
||
307 | #ifdef DEBUG_SEND |
||
308 | print_status(n); |
||
309 | #endif |
||
310 | } |
||
311 | } |
||
312 | task_endcycle(); |
||
313 | } |
||
314 | |||
315 | return 0; |
||
316 | } |
||
317 | |||
318 | TASK servo_get() |
||
319 | { |
||
320 | int i = 0; |
||
321 | |||
322 | while (1) { |
||
323 | #ifdef SERIAL_ON |
||
324 | //sem_wait(&mx_status); |
||
325 | sem_wait(&mx_servo); |
||
1333 | giacomo | 326 | status.cfg[i ].adc_in = servo_get_analog(COM_PORT, i ); |
327 | status.cfg[i+1].adc_in = servo_get_analog(COM_PORT, i+1); |
||
1331 | giacomo | 328 | sem_post(&mx_servo); |
329 | #ifdef DEBUG_SEND |
||
330 | cprintf("Leg %1d-%1d: (%4d) (%4d)\n", i, i+1, status.leg[i].adc_in, status.leg[i+1].adc_in); |
||
331 | #endif |
||
332 | i = (i+2)%6; |
||
333 | //sem_post(&mx_status); |
||
334 | #endif |
||
335 | task_endcycle(); |
||
336 | } |
||
337 | |||
338 | return 0; |
||
339 | } |
||
340 | |||
341 | int init_serial() |
||
342 | { |
||
343 | int err; |
||
1332 | giacomo | 344 | err = servo_open(COM_PORT, 19200); |
1331 | giacomo | 345 | |
346 | return err; |
||
347 | } |
||
348 | |||
349 | void end_serial() |
||
350 | { |
||
351 | servo_close(COM_PORT); |
||
352 | } |
||
353 | |||
354 | void init_send_task() |
||
355 | { |
||
1332 | giacomo | 356 | SOFT_TASK_MODEL ms; |
1331 | giacomo | 357 | PID pid; |
358 | |||
1332 | giacomo | 359 | soft_task_default_model(ms); |
360 | soft_task_def_ctrl_jet(ms); |
||
361 | soft_task_def_met(ms, SEND_TASK_WCET); |
||
362 | soft_task_def_period(ms, SEND_TASK_MIT); |
||
363 | soft_task_def_usemath(ms); |
||
1331 | giacomo | 364 | pid = task_create("Send_Task", servo_send, &ms, NULL); |
365 | if (pid == NIL) { |
||
366 | perror("Could not create task <Send_Task>"); |
||
367 | sys_end(); |
||
368 | } else |
||
369 | task_activate(pid); |
||
370 | |||
1332 | giacomo | 371 | soft_task_default_model(ms); |
372 | soft_task_def_ctrl_jet(ms); |
||
373 | soft_task_def_met(ms, GET_TASK_WCET); |
||
374 | soft_task_def_period(ms, GET_TASK_MIT); |
||
375 | soft_task_def_usemath(ms); |
||
1331 | giacomo | 376 | pid = task_create("Get_Task", servo_get, &ms, NULL); |
377 | if (pid == NIL) { |
||
378 | perror("Could not create task <Get_Task>"); |
||
379 | sys_end(); |
||
380 | } else |
||
381 | task_activate(pid); |
||
382 | } |
||
383 | |||
384 | void init_send() |
||
385 | { |
||
386 | int i; |
||
387 | |||
388 | if (init_serial()) { |
||
389 | perror("Could not initialize serial port."); |
||
390 | sys_end(); |
||
391 | } |
||
392 | |||
1333 | giacomo | 393 | sem_init(&mx_status,0,1); |
1331 | giacomo | 394 | sem_init(&mx_servo,0,1); |
395 | |||
396 | for (i=0; i<6;i++) { |
||
1333 | giacomo | 397 | status.ang[i].a = 0; |
398 | status.ang[i].b = 0; |
||
399 | status.ang[i].c = 0; |
||
1331 | giacomo | 400 | status.leg[i].x = 150; |
401 | status.leg[i].y = 0; |
||
402 | status.leg[i].z = 0; |
||
1333 | giacomo | 403 | status.cfg[i].pwm = 0; |
1331 | giacomo | 404 | } |
405 | status.power = 0; |
||
406 | |||
407 | init_send_task(); |
||
408 | } |
||
409 | |||
410 | void end_send() |
||
411 | { |
||
412 | end_serial(); |
||
413 | } |