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