Rev 1408 | 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" |
||
1396 | giacomo | 14 | #include "tracer.h" |
1408 | giacomo | 15 | #include "unistd.h" |
1331 | giacomo | 16 | |
17 | //#define DEBUG_SEND /* Print Sent Values */ |
||
18 | #define SERIAL_ON /* Send Data using Serial Port */ |
||
19 | |||
20 | /* Servo Tasks Constants */ |
||
21 | #ifdef DUBUG_SEND |
||
1359 | giacomo | 22 | #define SEND_TASK_WCET 20000 |
1331 | giacomo | 23 | #else |
1359 | giacomo | 24 | #define SEND_TASK_WCET 20000 |
1331 | giacomo | 25 | #endif |
1391 | giacomo | 26 | #define SEND_TASK_MIT 30000 |
1331 | giacomo | 27 | |
28 | #define LEG_A 100.0 |
||
29 | #define LEG_B 66.0 |
||
30 | #define LEG_C 26.0 |
||
31 | #define LEG_D 38.0 |
||
32 | #define LEG_CD_2IPO 92.087 /* 2 * sqrt(LEG_C^2 + LEG_D^2) */ |
||
1338 | giacomo | 33 | #define LEG_CD_ANG 0.600 /* arctg(LEG_C/LEG_D) in radianti */ |
1331 | giacomo | 34 | |
35 | const float c0 = LEG_C * LEG_C; |
||
36 | const float c1 = LEG_B * LEG_B; |
||
37 | const float c2 = LEG_B * LEG_B - LEG_A * LEG_A; |
||
38 | const float todeg = 180.0 / PI; |
||
1336 | giacomo | 39 | const float torad = PI / 180.0; |
1331 | giacomo | 40 | |
41 | HEXAPOD_STATE status; |
||
42 | |||
1408 | giacomo | 43 | void *start_tracer = NULL; |
44 | void *end_tracer = NULL; |
||
45 | void *trace_pointer = NULL; |
||
1407 | giacomo | 46 | |
1332 | giacomo | 47 | extern struct leg_calibration calibration_table[]; |
48 | |||
1331 | giacomo | 49 | void print_status(int n){ |
50 | } |
||
51 | |||
1336 | giacomo | 52 | int leg_to_ang(float px, float py, float pz, int *alfa, int *beta, int *gamma) |
1331 | giacomo | 53 | { |
54 | float px2 = px * px; |
||
55 | float py2 = py * py; |
||
56 | float pz2 = pz * pz; |
||
57 | |||
58 | float pxz2 = px2 + pz2; |
||
59 | |||
1338 | giacomo | 60 | float alfa1,beta1,alfa2,beta2,gamma1,gamma2; |
1331 | giacomo | 61 | float m,dsqrt; |
62 | |||
63 | float delta_xz = pxz2 - c0; |
||
64 | float s,k,k2,y1,delta_xy; |
||
65 | |||
66 | if (delta_xz < 0.0) return -1; |
||
67 | |||
68 | if (pz >= LEG_C) { |
||
1338 | giacomo | 69 | gamma2 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2); |
70 | gamma1 = gamma2 * todeg; |
||
1331 | giacomo | 71 | } else { |
1338 | giacomo | 72 | gamma2 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2); |
73 | gamma1 = gamma2 * todeg; |
||
1331 | giacomo | 74 | } |
75 | |||
1338 | giacomo | 76 | m = pxz2 - LEG_CD_2IPO * (pz * sin(gamma2+LEG_CD_ANG) + px * cos(gamma2+LEG_CD_ANG) - LEG_CD_2IPO / 4.0); |
1331 | giacomo | 77 | |
78 | s = m + py2; |
||
79 | k = c2 + s; |
||
80 | k2 = k * k; |
||
81 | |||
82 | delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1); |
||
83 | |||
84 | if (delta_xy >= 0.0) { |
||
85 | dsqrt = sqrt(delta_xy); |
||
86 | y1 = (py * k + dsqrt) / (2.0 * s); |
||
87 | beta1 = asin(y1/LEG_B) * todeg; |
||
88 | alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1; |
||
89 | y1 = (py * k - dsqrt) / (2.0 * s); |
||
90 | beta2 = asin(y1/LEG_B) * todeg; |
||
91 | alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2; |
||
92 | |||
93 | if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) { |
||
1336 | giacomo | 94 | *alfa = (int)(alfa1 * 3600.0); |
95 | *beta = (int)(beta1 * 3600.0); |
||
96 | *gamma = (int)(gamma1 * 3600.0); |
||
1331 | giacomo | 97 | return 0; |
98 | } else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) { |
||
1336 | giacomo | 99 | *alfa = (int)(alfa2 * 3600.0); |
100 | *beta = (int)(beta2 * 3600.0); |
||
101 | *gamma = (int)(gamma1 * 3600.0); |
||
1331 | giacomo | 102 | return 0; |
103 | } else { |
||
1336 | giacomo | 104 | return -1; |
1331 | giacomo | 105 | } |
106 | } else |
||
1336 | giacomo | 107 | return -1; |
1331 | giacomo | 108 | |
1336 | giacomo | 109 | return -1; |
110 | |||
1331 | giacomo | 111 | } |
112 | |||
1336 | giacomo | 113 | int ang_to_leg(int alfa, int beta, int gamma, float *px, float *py, float *pz) { |
114 | |||
115 | float alfa1 = (float)(alfa)/3600.0 * torad; |
||
116 | float beta1 = (float)(beta)/3600.0 * torad; |
||
117 | float sin_gamma = sin((float)(gamma)/3600.0 * torad); |
||
118 | float cos_gamma = cos((float)(gamma)/3600.0 * torad); |
||
119 | float m; |
||
120 | |||
121 | m = LEG_B * cos(beta1) + LEG_A * cos(alfa1 - beta1); |
||
122 | *py = LEG_B * sin(beta1) - LEG_A * sin(alfa1 - beta1); |
||
123 | |||
124 | *pz = (LEG_D + m) * sin_gamma + LEG_C * cos_gamma; |
||
125 | *px = (LEG_D + m) * cos_gamma - LEG_C * sin_gamma; |
||
126 | |||
127 | return 0; |
||
128 | |||
129 | } |
||
130 | |||
1333 | giacomo | 131 | void update_event_action(void) { |
132 | |||
133 | struct timespec t; |
||
134 | struct action_event *e; |
||
135 | int i; |
||
136 | |||
137 | kern_gettime(&t); |
||
138 | |||
139 | while ((e = get_first_old_event(&t)) != NULL) { |
||
140 | |||
141 | if (e->type == EVT_SET_MASK_LEG_ANGLE) { |
||
142 | |||
143 | for (i=0;i<6;i++) |
||
1359 | giacomo | 144 | if ((e->mask >> i) & 1) { |
1333 | giacomo | 145 | |
1336 | giacomo | 146 | status.ang[i].a = e->ang.a; |
147 | status.ang[i].b = e->ang.b; |
||
148 | status.ang[i].c = e->ang.c; |
||
149 | |||
150 | status.cfg[i].pwm = e->pwm; |
||
151 | |||
1396 | giacomo | 152 | #ifdef DEBUG_SEND |
1334 | giacomo | 153 | printf_xy(3,2,WHITE,"%8d: Update leg %2d angle",(int)kern_gettime(NULL),i); |
1396 | giacomo | 154 | #endif |
1333 | giacomo | 155 | |
156 | |||
157 | } |
||
158 | |||
159 | e->status = EVT_STATUS_DONE; |
||
160 | |||
161 | } |
||
162 | |||
163 | } |
||
164 | |||
165 | } |
||
166 | |||
1407 | giacomo | 167 | int trace_init(int buffer_size) |
168 | { |
||
169 | |||
170 | start_tracer = trace_pointer = (void *)malloc(buffer_size); |
||
171 | end_tracer = start_tracer + buffer_size; |
||
172 | memset(trace_pointer,0,buffer_size); |
||
173 | |||
174 | if (trace_pointer == NULL) return -1; |
||
175 | |||
176 | return 0; |
||
177 | } |
||
178 | |||
1408 | giacomo | 179 | /* Init the network stack */ |
180 | int init_network(char *local_ip) |
||
181 | { |
||
182 | |||
183 | struct net_model m = net_base; |
||
184 | |||
185 | net_setudpip(m, local_ip, "255.255.255.255"); |
||
186 | |||
187 | if (net_init(&m) != 1) { |
||
188 | cprintf("Network: Init Error.\n"); |
||
189 | return -1; |
||
190 | } |
||
191 | |||
192 | return 0; |
||
193 | |||
194 | } |
||
195 | |||
1407 | giacomo | 196 | int trace_consumption(int sensor, int value) |
197 | { |
||
198 | |||
199 | struct timespec t; |
||
200 | SYS_FLAGS f; |
||
201 | |||
202 | if (trace_pointer == NULL) return -1; |
||
203 | |||
204 | if (trace_pointer >= (end_tracer-16)) return -1; |
||
205 | |||
206 | f = kern_fsave(); |
||
207 | |||
208 | sys_gettime(&t); |
||
209 | |||
210 | *(unsigned int *)trace_pointer = (sensor & 0xFF) | 0xAABBFF00; |
||
211 | *(unsigned int *)(trace_pointer + 4) = value; |
||
212 | *(unsigned int *)(trace_pointer + 8) = t.tv_sec; |
||
213 | *(unsigned int *)(trace_pointer + 12) = t.tv_nsec; |
||
214 | |||
215 | trace_pointer += 16; |
||
216 | |||
217 | kern_frestore(f); |
||
218 | |||
219 | return 0; |
||
220 | |||
221 | } |
||
222 | |||
1408 | giacomo | 223 | int trace_send() { |
1407 | giacomo | 224 | |
225 | static char pkg_buffer[1100]; |
||
226 | int actual = 0; |
||
227 | |||
1408 | giacomo | 228 | UDP_ADDR target, local; |
229 | char local_ip[20], target_ip[20]; |
||
230 | int socket; |
||
231 | IP_ADDR bindlist[5]; |
||
232 | |||
233 | strcpy(local_ip, "192.168.1.10"); |
||
234 | strcpy(target_ip, "192.168.1.1"); |
||
235 | |||
236 | if (init_network(local_ip)) sys_end(); |
||
237 | |||
238 | /* local IP string to addr */ |
||
239 | ip_str2addr(local_ip,&(local.s_addr)); |
||
240 | /* set the source port */ |
||
241 | local.s_port = 20000; |
||
242 | |||
243 | /* target IP string to addr */ |
||
244 | ip_str2addr(target_ip,&(bindlist[0])); |
||
245 | memset(&(bindlist[1]), 0, sizeof(IP_ADDR)); |
||
246 | /* bind */ |
||
247 | socket = udp_bind(&local, NULL); |
||
248 | |||
249 | /* target IP string to addr */ |
||
250 | ip_str2addr(target_ip,&(target.s_addr)); |
||
251 | /* target port */ |
||
252 | target.s_port = 20000; |
||
253 | |||
1407 | giacomo | 254 | trace_pointer = start_tracer; |
1408 | giacomo | 255 | actual = 0; |
1407 | giacomo | 256 | |
1408 | giacomo | 257 | while(trace_pointer < end_tracer) { |
1407 | giacomo | 258 | |
1408 | giacomo | 259 | if (((*(int *)(trace_pointer) >> 8) & 0xAABBFF) == 0xAABBFF) { |
1407 | giacomo | 260 | |
1410 | giacomo | 261 | cprintf("."); |
262 | |||
1408 | giacomo | 263 | memcpy(&(pkg_buffer[actual]),trace_pointer,16); |
1407 | giacomo | 264 | |
1408 | giacomo | 265 | actual += 16; |
266 | |||
267 | if (actual > 1000) { |
||
268 | pkg_buffer[actual] = 0; |
||
1410 | giacomo | 269 | cprintf("X"); |
270 | udp_sendto(socket, pkg_buffer, actual+1, &target); |
||
1408 | giacomo | 271 | usleep(10000); |
272 | actual = 0; |
||
273 | } |
||
274 | |||
1407 | giacomo | 275 | } |
276 | |||
1408 | giacomo | 277 | trace_pointer += 16; |
278 | |||
1407 | giacomo | 279 | } |
280 | |||
1410 | giacomo | 281 | pkg_buffer[actual] = 0; |
282 | cprintf("X"); |
||
283 | udp_sendto(socket, pkg_buffer, actual+1, &target); |
||
284 | usleep(10000); |
||
285 | actual = 0; |
||
286 | |||
1408 | giacomo | 287 | return 0; |
288 | |||
1407 | giacomo | 289 | } |
290 | |||
1331 | giacomo | 291 | TASK servo_send() |
292 | { |
||
293 | HEXAPOD_STATE old_status; |
||
294 | register char changes, new_pos, new_pwm, new_power; |
||
1359 | giacomo | 295 | int res,n; |
1390 | mauro | 296 | struct timespec t; |
1391 | giacomo | 297 | int actual_leg = 0; |
1331 | giacomo | 298 | |
299 | for (n=0; n<6;n++) { |
||
1333 | giacomo | 300 | old_status.ang[n].a = 0; |
301 | old_status.ang[n].b = 0; |
||
302 | old_status.ang[n].c = 0; |
||
303 | old_status.cfg[n].pwm = 0; |
||
1331 | giacomo | 304 | } |
305 | old_status.power = 0; |
||
306 | |||
307 | while (1) { |
||
308 | changes = 0; |
||
1333 | giacomo | 309 | |
310 | update_event_action(); |
||
1331 | giacomo | 311 | |
312 | for (n=0; n<6; n++){ |
||
313 | new_pos = 0; |
||
314 | new_pwm = 0; |
||
315 | new_power = 0; |
||
316 | |||
1333 | giacomo | 317 | if ((status.ang[n].a != old_status.ang[n].a) || |
318 | (status.ang[n].b != old_status.ang[n].b) || |
||
319 | (status.ang[n].c != old_status.ang[n].c)) { |
||
320 | |||
321 | old_status.ang[n].a = status.ang[n].a; |
||
322 | old_status.ang[n].b = status.ang[n].b; |
||
323 | old_status.ang[n].c = status.ang[n].c; |
||
324 | new_pos++; |
||
325 | } |
||
326 | |||
327 | if (status.cfg[n].pwm != old_status.cfg[n].pwm) { |
||
328 | old_status.cfg[n].pwm = status.cfg[n].pwm; |
||
1331 | giacomo | 329 | new_pwm++; |
330 | } |
||
331 | if (status.power != old_status.power) { |
||
332 | old_status.power = status.power; |
||
333 | new_power++; |
||
334 | } |
||
335 | |||
336 | if (new_pos) { |
||
337 | #ifdef SERIAL_ON |
||
1391 | giacomo | 338 | task_nopreempt(); |
1359 | giacomo | 339 | res = servo_set_angle_sec(com(n*3 ), pin(n*3 ), adjust(status.ang[n].a,n,0)); |
340 | if (res != 0) cprintf("Error send data\n"); |
||
341 | res = servo_set_angle_sec(com(n*3+1), pin(n*3+1), adjust(status.ang[n].b,n,1)); |
||
342 | if (res != 0) cprintf("Error send data\n"); |
||
343 | res = servo_set_angle_sec(com(n*3+2), pin(n*3+2), adjust(status.ang[n].c,n,2)); |
||
344 | if (res != 0) cprintf("Error send data\n"); |
||
1391 | giacomo | 345 | task_preempt(); |
1331 | giacomo | 346 | #endif |
1359 | giacomo | 347 | |
1331 | giacomo | 348 | } |
349 | |||
350 | if (new_pwm) { |
||
351 | #ifdef SERIAL_ON |
||
1391 | giacomo | 352 | task_nopreempt(); |
1359 | giacomo | 353 | (old_status.cfg[n].pwm & 1) ? servo_turn_on(com(n*3 ), pin(n*3 )) : servo_turn_off(com(n*3 ), pin(n*3 )); |
354 | (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)); |
||
355 | (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 | 356 | task_preempt(); |
1331 | giacomo | 357 | #endif |
358 | } |
||
359 | if (new_power) { |
||
360 | #ifdef SERIAL_ON |
||
1391 | giacomo | 361 | task_nopreempt(); |
1331 | giacomo | 362 | if (old_status.power) { |
1359 | giacomo | 363 | servo_set_RC5_switch(COM2, 1); |
1331 | giacomo | 364 | } else { |
1359 | giacomo | 365 | servo_set_RC5_switch(COM2, 0); |
1331 | giacomo | 366 | } |
1391 | giacomo | 367 | task_preempt(); |
1331 | giacomo | 368 | #endif |
369 | } |
||
370 | |||
371 | if (new_pos || new_pwm || new_power) { |
||
372 | changes++; |
||
373 | } |
||
374 | } |
||
375 | |||
1391 | giacomo | 376 | if (!new_pos) { |
1331 | giacomo | 377 | |
1397 | giacomo | 378 | |
379 | #ifdef SERIAL_ON |
||
1391 | giacomo | 380 | task_nopreempt(); |
381 | status.cfg[actual_leg].adc_in = servo_get_analog(COM1, actual_leg); |
||
1407 | giacomo | 382 | trace_consumption(actual_leg,status.cfg[actual_leg].adc_in); |
1391 | giacomo | 383 | task_preempt(); |
1397 | giacomo | 384 | #endif |
1391 | giacomo | 385 | |
386 | sys_gettime(&t); |
||
387 | printf_xy(1,20,WHITE,"(%d) (%d) (%d) (%d) (%d) (%d) ", |
||
388 | status.cfg[0].adc_in, |
||
389 | status.cfg[1].adc_in, |
||
390 | status.cfg[2].adc_in, |
||
391 | status.cfg[3].adc_in, |
||
392 | status.cfg[4].adc_in, |
||
393 | status.cfg[5].adc_in); |
||
394 | actual_leg = (actual_leg+1)%6; |
||
395 | |||
396 | } |
||
397 | |||
1331 | giacomo | 398 | task_endcycle(); |
399 | } |
||
400 | |||
401 | return 0; |
||
402 | } |
||
403 | |||
404 | int init_serial() |
||
405 | { |
||
406 | int err; |
||
1359 | giacomo | 407 | err = servo_open(COM1, COM_SPEED); |
408 | if (!err) |
||
409 | err = servo_open(COM2, COM_SPEED); |
||
1331 | giacomo | 410 | |
411 | return err; |
||
412 | } |
||
413 | |||
414 | void end_serial() |
||
415 | { |
||
1359 | giacomo | 416 | servo_close(COM1); |
417 | servo_close(COM2); |
||
1331 | giacomo | 418 | } |
419 | |||
420 | void init_send_task() |
||
421 | { |
||
1359 | giacomo | 422 | HARD_TASK_MODEL ms; |
1331 | giacomo | 423 | PID pid; |
424 | |||
1359 | giacomo | 425 | hard_task_default_model(ms); |
426 | hard_task_def_ctrl_jet(ms); |
||
427 | hard_task_def_wcet(ms, SEND_TASK_WCET); |
||
428 | hard_task_def_mit(ms, SEND_TASK_MIT); |
||
429 | hard_task_def_usemath(ms); |
||
1391 | giacomo | 430 | pid = task_create("Servo_Task", servo_send, &ms, NULL); |
1331 | giacomo | 431 | if (pid == NIL) { |
432 | perror("Could not create task <Send_Task>"); |
||
433 | sys_end(); |
||
434 | } else |
||
435 | task_activate(pid); |
||
436 | |||
437 | } |
||
438 | |||
439 | void init_send() |
||
440 | { |
||
441 | int i; |
||
442 | |||
443 | if (init_serial()) { |
||
444 | perror("Could not initialize serial port."); |
||
445 | sys_end(); |
||
446 | } |
||
447 | |||
448 | for (i=0; i<6;i++) { |
||
1333 | giacomo | 449 | status.ang[i].a = 0; |
450 | status.ang[i].b = 0; |
||
451 | status.ang[i].c = 0; |
||
452 | status.cfg[i].pwm = 0; |
||
1331 | giacomo | 453 | } |
454 | status.power = 0; |
||
455 | |||
456 | init_send_task(); |
||
1336 | giacomo | 457 | |
1331 | giacomo | 458 | } |
459 | |||
460 | void end_send() |
||
461 | { |
||
462 | end_serial(); |
||
463 | } |