robotsim first test
[aversive.git] / projects / microb2010 / mechboard / arm_xy.c
1 /*  
2  *  Copyright Droids Corporation (2009)
3  * 
4  *  This program is free software; you can redistribute it and/or modify
5  *  it under the terms of the GNU General Public License as published by
6  *  the Free Software Foundation; either version 2 of the License, or
7  *  (at your option) any later version.
8  *
9  *  This program is distributed in the hope that it will be useful,
10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  *  GNU General Public License for more details.
13  *
14  *  You should have received a copy of the GNU General Public License
15  *  along with this program; if not, write to the Free Software
16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
17  *
18  *  Revision : $Id: arm_xy.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $
19  *
20  * Fabrice DESCLAUX <serpilliere@droids-corp.org>
21  * Olivier MATZ <zer0@droids-corp.org>
22  */
23
24 #include <math.h>
25 #include <string.h>
26
27 #include <aversive.h>
28 #include <aversive/wait.h>
29 #include <aversive/pgmspace.h>
30 #include <aversive/error.h>
31
32 #include <ax12.h>
33 #include <uart.h>
34 #include <spi.h>
35 #include <encoders_spi.h>
36 #include <pwm_ng.h>
37 #include <timer.h>
38 #include <scheduler.h>
39 #include <time.h>
40
41 #include <pid.h>
42 #include <quadramp.h>
43 #include <control_system_manager.h>
44 #include <blocking_detection_manager.h>
45
46 #include <rdline.h>
47
48 #include "main.h"
49 #include "cmdline.h"
50 #include "arm_xy.h"
51 #include "ax12_user.h"
52
53 #define ARM_DEBUG(args...) DEBUG(E_USER_ARM, args)
54 #define ARM_NOTICE(args...) NOTICE(E_USER_ARM, args)
55 #define ARM_ERROR(args...) ERROR(E_USER_ARM, args)
56
57 #define DEG(x) (((double)(x)) * (180.0 / M_PI))
58 #define RAD(x) (((double)(x)) * (M_PI / 180.0))
59 #define M_2PI (2*M_PI)
60
61 /* physical location/dimensions of arm */
62 #define ARM_S_LEN 124.
63 #define ARM_E_LEN 130.
64
65 /* timeout after 1 second if position is not reached */
66 #define ARM_GLOBAL_TIMEOUT 1000000L
67
68 /* timeout 100ms after position is reached if not in window */
69 #define ARM_WINDOW_TIMEOUT 200000L
70
71 /* default (template) period, but real one is variable */
72 #define ARM_PERIOD 50000L
73 #define ARM_MAX_DIST 40L
74
75 /* we pos reached, check arm in window every period */
76 #define ARM_SURVEY_PERIOD 25000UL /* in us */
77
78 /* number of steps/s */
79 #define ARM_AX12_MAX_SPEED (800L)
80
81 /* Maximum number of steps in one ARM_PERIOD */
82 #define ARM_MAX_E (((ARM_AX12_MAX_SPEED*ARM_PERIOD)/1000000L))
83 /* 4000 steps/CS => 800step/ms */
84 #define ARM_MAX_S ((800L*ARM_PERIOD)/1000L)
85
86
87 /* window limits in ax12/cs unit */
88 #define ARM_SHOULDER_WINDOW_POS  250
89 #define ARM_ELBOW_WINDOW_POS     8
90 #define ARM_ELBOW_WINDOW_SPEED   100
91 #define ARM_WRIST_WINDOW_POS     8
92 #define ARM_WRIST_WINDOW_SPEED   100
93
94 /* default and max speeds */
95 #define SHOULDER_DEFAULT_SPEED   800
96 #define ELBOW_DEFAULT_SPEED      0x3ff
97 #define SHOULDER_MAX_SPEED       10000
98 #define ELBOW_MAX_SPEED          0x3ff
99
100 /* window status flags */
101 #define SHOULDER_NOT_IN_WIN 1
102 #define ELBOW_NOT_IN_WIN    2
103 #define WRIST_NOT_IN_WIN    4
104
105 static void wrist_angle_deg2robot_l(double wrist_deg, double *wrist_out);
106 static void angle_rad2robot_l(double shoulder_rad, double elbow_rad,
107                               double *shoulder_robot, double *elbow_robot);
108 static void angle_robot2rad_l(double shoulder_robot, double elbow_robot,
109                               double *shoulder_rad, double *elbow_rad);
110 static void wrist_angle_deg2robot_r(double wrist_deg, double *wrist_out);
111 static void angle_rad2robot_r(double shoulder_rad, double elbow_rad,
112                               double *shoulder_robot, double *elbow_robot);
113 static void angle_robot2rad_r(double shoulder_robot, double elbow_robot,
114                               double *shoulder_rad, double *elbow_rad);
115
116 static void arm_schedule_event(struct arm *arm, uint32_t time);
117
118 struct arm left_arm = {
119         .config = {
120                 .wrist_angle_deg2robot = wrist_angle_deg2robot_l,
121                 .angle_rad2robot = angle_rad2robot_l,
122                 .angle_robot2rad = angle_robot2rad_l,
123                 .elbow_ax12 = L_ELBOW_AX12,
124                 .wrist_ax12 = L_WRIST_AX12,
125         },
126 };
127
128 struct arm right_arm = {
129         .config = {
130                 .wrist_angle_deg2robot = wrist_angle_deg2robot_r,
131                 .angle_rad2robot = angle_rad2robot_r,
132                 .angle_robot2rad = angle_robot2rad_r,
133                 .elbow_ax12 = R_ELBOW_AX12,
134                 .wrist_ax12 = R_WRIST_AX12,
135         },
136 };
137
138 /* process shoulder + elbow angles from height and distance */
139 int8_t cart2angle(int32_t h_int, int32_t d_int, double *alpha, double *beta)
140 {
141         double h, d, l;
142         double elbow, shoulder;
143
144         d = d_int;
145         h = h_int;
146         l = sqrt(d*d + h*h);
147         if  (l > (ARM_S_LEN + ARM_E_LEN))
148                 return -1;
149         
150         elbow = -acos((d*d + h*h - ARM_E_LEN*ARM_E_LEN - 
151                        ARM_S_LEN*ARM_S_LEN) / (2*ARM_S_LEN*ARM_E_LEN));
152         shoulder = atan2(h,d) - atan2(ARM_E_LEN*sin(elbow), 
153                                       ARM_S_LEN+ARM_E_LEN*cos(elbow));
154         
155         *alpha = shoulder;
156         *beta = elbow;
157
158         return 0;
159 }
160
161
162 /* process height and distance from shoulder + elbow angles */
163 void angle2cart(double alpha, double beta, int32_t *h, int32_t *d)
164 {
165         double tmp_a;
166         int32_t tmp_h, tmp_d;
167
168         tmp_h = ARM_S_LEN * sin(alpha);
169         tmp_d = ARM_S_LEN * cos(alpha);
170         
171         tmp_a = alpha+beta;
172         *h = tmp_h + ARM_E_LEN * sin(tmp_a);
173         *d = tmp_d + ARM_E_LEN * cos(tmp_a);
174 }
175
176 /*** left arm */
177
178 #define ARM_LEFT_S_OFFSET -1150.
179 #define ARM_LEFT_E_OFFSET 476.
180 #define ARM_LEFT_W_OFFSET 90.
181
182 static void wrist_angle_deg2robot_l(double wrist_deg, double *wrist_out)
183 {
184         *wrist_out = -wrist_deg * 3.41  + ARM_LEFT_W_OFFSET;
185 }
186
187 /* convert an angle in radian into a robot-specific unit 
188  * for shoulder and elbow for LEFT ARM*/
189 static void angle_rad2robot_l(double shoulder_rad, double elbow_rad,
190                               double *shoulder_robot, double *elbow_robot)
191 {
192         *shoulder_robot = shoulder_rad * 4 * 66 * 512. / (2*M_PI) + ARM_LEFT_S_OFFSET;
193         *elbow_robot = -elbow_rad * 3.41 * 360. / (2*M_PI) + ARM_LEFT_E_OFFSET;
194 }
195
196 /* convert  a robot-specific unit into an angle in radian 
197  * for shoulder and elbow for LEFT ARM */
198 static void angle_robot2rad_l(double shoulder_robot, double elbow_robot,
199                               double *shoulder_rad, double *elbow_rad)
200 {
201         *shoulder_rad = ((shoulder_robot - ARM_LEFT_S_OFFSET) * (2*M_PI))/(4 * 66 * 512.);
202         *elbow_rad = -((elbow_robot - ARM_LEFT_E_OFFSET) * (2*M_PI))/(3.41 * 360.);             
203 }
204
205 /*** right arm */
206
207 #define ARM_RIGHT_S_OFFSET 1150.
208 #define ARM_RIGHT_E_OFFSET 673.
209 #define ARM_RIGHT_W_OFFSET 935.
210
211 static void wrist_angle_deg2robot_r(double wrist_deg, double *wrist_out)
212 {
213         *wrist_out = wrist_deg * 3.41  + ARM_RIGHT_W_OFFSET;
214 }
215
216 /* convert an angle in radian into a robot-specific unit 
217  * for shoulder and elbow */
218 static void angle_rad2robot_r(double shoulder_rad, double elbow_rad,
219                               double *shoulder_robot, double *elbow_robot)
220 {
221         *shoulder_robot = -shoulder_rad * 4 * 66 * 512. / (2*M_PI) + ARM_RIGHT_S_OFFSET;
222         *elbow_robot = elbow_rad * 3.41 * 360. / (2*M_PI) + ARM_RIGHT_E_OFFSET;
223 }
224
225 /* convert  a robot-specific unit into an angle in radian 
226  * for shoulder and elbow */
227 static void angle_robot2rad_r(double shoulder_robot, double elbow_robot,
228                               double *shoulder_rad, double *elbow_rad)
229 {
230         *shoulder_rad = -((shoulder_robot - ARM_RIGHT_S_OFFSET) * (2*M_PI))/(4 * 66 * 512.);
231         *elbow_rad = ((elbow_robot - ARM_RIGHT_E_OFFSET) * (2*M_PI))/(3.41 * 360.);
232 }
233
234
235 /*
236  * Fill the arm_status structure according to request.
237  *
238  * return:
239  *     0  => success
240  *   < 0  => error
241  */
242 static int8_t arm_do_step(struct arm *arm)
243 {
244         const struct arm_config *conf = &arm->config;
245         const struct arm_request *req = &arm->req;
246         struct arm_status *status = &arm->status;
247
248         int8_t ret;
249         int32_t diff_h, diff_d; /* position delta in steps */
250         int32_t next_h, next_d; /* next position in steps */
251         int32_t l; /* distance between cur pos and next pos */
252
253         double as_cur_rad, ae_cur_rad; /* current angle in rad */
254         double as_next_rad, ae_next_rad; /* next angle in rad */
255         double as_cur, ae_cur; /* current angle in angle_steps */
256         double as_next, ae_next; /* next angle in angle_steps */
257
258         int32_t as_diff, ae_diff; /* angle delta in angle_steps */
259         int32_t s_speed, e_speed; /* elbow/shoulder speed in angle_steps */
260         
261         double as_coef, ae_coef;
262         
263         /* process diff between final request and current pos */
264         diff_h = req->h_mm - status->h_mm;
265         diff_d = req->d_mm - status->d_mm;
266         ARM_NOTICE("goal:d=%ld,h=%ld cur:d=%ld,h=%ld diff:d=%ld,h=%ld",
267                   req->d_mm, req->h_mm, status->d_mm, status->h_mm,
268                   diff_d, diff_h);
269
270         /* if distance to next point is too large, saturate it */
271         l = sqrt(diff_h*diff_h + diff_d*diff_d);
272         if (l > ARM_MAX_DIST) {
273                 diff_h = diff_h * ARM_MAX_DIST / l;
274                 diff_d = diff_d * ARM_MAX_DIST / l;
275         }
276         ARM_NOTICE("l=%ld ; after max dist: diff:d=%ld,h=%ld", l, diff_d, diff_h);
277         
278         /* process next position */
279         next_h = status->h_mm + diff_h;
280         next_d = status->d_mm + diff_d;
281         ARM_DEBUG("next:d=%ld,h=%ld", next_d, next_h);
282
283         /* calculate the current angle of arm in radian */
284         ret = cart2angle(status->h_mm, status->d_mm, &as_cur_rad, &ae_cur_rad);
285         if (ret)
286                 return ret;
287         ARM_DEBUG("as_cur_rad=%f ae_cur_rad=%f", as_cur_rad, ae_cur_rad);
288
289         /* calculate the next angle of arm in radian */
290         ret = cart2angle(next_h, next_d, &as_next_rad, &ae_next_rad);
291         if (ret)
292                 return ret;
293         ARM_DEBUG("as_next_rad=%f ae_next_rad=%f", as_next_rad, ae_next_rad);
294
295         /* convert radian in angle_steps */
296         conf->angle_rad2robot(as_cur_rad, ae_cur_rad,
297                              &as_cur, &ae_cur);
298         ARM_DEBUG("as_cur=%f ae_cur=%f", as_cur, ae_cur);
299         conf->angle_rad2robot(as_next_rad, ae_next_rad,
300                              &as_next, &ae_next);
301         ARM_DEBUG("as_next=%f ae_next=%f", as_next, ae_next);
302
303         /* process angle delta in angle_steps */
304         as_diff = as_next - as_cur;
305         ae_diff = ae_next - ae_cur;
306         ARM_DEBUG("as_diff=%ld ae_diff=%ld", as_diff, ae_diff);
307
308         /* update position status */
309         status->h_mm = next_h;
310         status->d_mm = next_d;
311         status->shoulder_angle_steps = as_next;
312         status->elbow_angle_steps = ae_next;
313         status->shoulder_angle_rad = as_next_rad;
314         status->elbow_angle_rad = ae_next_rad;
315
316         /* we reached destination, nothing to do */
317         if (as_diff == 0 && ae_diff == 0) {
318                 status->shoulder_speed = SHOULDER_DEFAULT_SPEED;
319                 status->elbow_speed = ELBOW_DEFAULT_SPEED;
320                 status->next_update_time = 0;
321                 ARM_NOTICE("reaching end");
322                 return 0;
323         }
324
325         /* test if one actuator is already in position */
326         if (as_diff == 0) {
327                 ARM_DEBUG("shoulder reached destination");
328                 ae_coef = (double)ARM_MAX_E / (double)ae_diff;
329                 status->next_update_time = ARM_PERIOD * ABS(ae_coef);
330                 e_speed = ABS(ae_coef) * ABS(ae_diff);
331                 s_speed = ARM_MAX_S;
332         }
333         else if (ae_diff == 0) {
334                 ARM_DEBUG("elbow reached destination");
335                 as_coef = (double)ARM_MAX_S / (double)as_diff;
336                 status->next_update_time = ARM_PERIOD / ABS(as_coef);
337                 e_speed = ARM_MAX_E;
338                 s_speed = ABS(as_coef) * ABS(as_diff);
339         }
340
341         else {
342                 as_coef = (double)ARM_MAX_S / (double)as_diff;
343                 ae_coef = (double)ARM_MAX_E / (double)ae_diff;
344             
345                 ARM_DEBUG("as_coef=%f ae_coef=%f", as_coef, ae_coef);
346             
347                 /* if elbow is limitating */
348                 if (ABS(as_coef) >= ABS(ae_coef)) {
349                         ARM_DEBUG("elbow limit");
350                         status->next_update_time = ARM_PERIOD / ABS(ae_coef);
351                         s_speed = ABS(ae_coef) * ABS(as_diff);
352                         e_speed = ABS(ae_coef) * ABS(ae_diff);
353                 }
354                 /* else, shoulder is limitating */
355                 else {
356                         ARM_DEBUG("shoulder limit");
357                         status->next_update_time = ARM_PERIOD / ABS(as_coef);
358                         s_speed = ABS(as_coef) * ABS(as_diff);
359                         e_speed = ABS(as_coef) * ABS(ae_diff);
360                 }
361         }
362
363         ARM_NOTICE("next update: %ld", status->next_update_time);
364
365         /* convert speed in specific unit */
366         status->shoulder_speed = (s_speed * CS_PERIOD) / ARM_PERIOD;
367         status->elbow_speed = (e_speed * 0x3ff) / ARM_MAX_E;
368
369         ARM_DEBUG("speeds: s=%ld e=%ld", status->shoulder_speed, status->elbow_speed);
370
371         /* avoid limits */
372         if (status->shoulder_speed == 0)
373                 status->shoulder_speed = 1;
374         if (status->elbow_speed == 0)
375                 status->elbow_speed = 1;
376         if (status->shoulder_speed >= SHOULDER_MAX_SPEED)
377                 status->shoulder_speed = SHOULDER_MAX_SPEED;
378         if (status->elbow_speed >= ELBOW_MAX_SPEED)
379                 status->elbow_speed = ELBOW_MAX_SPEED;
380
381         ARM_DEBUG("speeds (sat): s=%ld e=%ld", status->shoulder_speed, status->elbow_speed);
382
383         return 0;
384 }
385
386 static void arm_delete_event(struct arm *arm)
387 {
388         if (arm->status.event == -1)
389                 return;
390         ARM_DEBUG("Delete arm event");
391         scheduler_del_event(arm->status.event);
392         arm->status.event = -1;
393 }
394
395 /* write values to ax12 + cs */
396 static void arm_apply(struct arm *arm)
397 {
398         struct cs_block *csb = arm->config.csb;
399         const struct arm_status *st = &arm->status;
400
401         ARM_DEBUG("arm_apply");
402
403         if (arm->config.simulate)
404                 return;
405
406         /* set speed and pos of shoulder */
407         quadramp_set_1st_order_vars(&csb->qr, 
408                                     st->shoulder_speed,
409                                     st->shoulder_speed);
410         cs_set_consign(&csb->cs, st->shoulder_angle_steps);
411
412         /* set speed and position of elbow */
413         ax12_user_write_int(&gen.ax12, arm->config.elbow_ax12,
414                             AA_MOVING_SPEED_L, st->elbow_speed);
415         ax12_user_write_int(&gen.ax12, arm->config.elbow_ax12,
416                             AA_GOAL_POSITION_L, st->elbow_angle_steps);
417 }
418
419 /* return true if one of the mask condition is true */
420 uint8_t arm_test_traj_end(struct arm *arm, uint8_t mask)
421 {
422         if ((mask & ARM_TRAJ_END) && (arm->status.state & ARM_FLAG_IN_WINDOW))
423                 return ARM_TRAJ_END;
424
425         if ((mask & ARM_TRAJ_NEAR) && (arm->status.state & ARM_FLAG_LAST_STEP))
426                 return ARM_TRAJ_NEAR;
427
428         if ((mask & ARM_TRAJ_TIMEOUT) && (arm->status.state & ARM_FLAG_TIMEOUT))
429                 return ARM_TRAJ_TIMEOUT;
430
431         if ((mask & ARM_TRAJ_ERROR) && (arm->status.state & ARM_FLAG_ERROR))
432                 return ARM_TRAJ_ERROR;
433
434         return 0;
435 }
436
437 uint8_t arm_wait_traj_end(struct arm *arm, uint8_t mask)
438 {
439         uint8_t ret;
440         while(1) {
441                 ret = arm_test_traj_end(arm, mask);
442                 if (ret)
443                         return ret;
444         }
445 }
446
447 /* return true if one of the mask condition is true */
448 uint8_t arm_in_window(struct arm *arm, uint8_t *status)
449 {
450         int8_t err;
451 /*      uint16_t spd; */
452         int16_t pos;
453         int32_t cs_err;
454
455         *status = 0;
456
457         if (arm->config.simulate)
458                 return 1;
459
460         /* shoulder, just check position */
461         cs_err = cs_get_error(&arm->config.csb->cs);
462         if (ABS(cs_err) > ARM_SHOULDER_WINDOW_POS)
463                 *status |= SHOULDER_NOT_IN_WIN;
464
465 #if 0   
466         /* check elbow speed */
467         err = ax12_user_read_int(&gen.ax12, arm->config.elbow_ax12,
468                                  AA_PRESENT_SPEED_L, &spd);
469         if (err)
470                 goto fail;
471         if (spd > ARM_ELBOW_WINDOW_SPEED)
472                 return 0;
473
474         /* check wrist speed */
475         err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12,
476                                  AA_PRESENT_SPEED_L, &spd);
477         if (err)
478                 goto fail;
479         if (spd > ARM_WRIST_WINDOW_SPEED)
480                 return 0;
481 #endif  
482         /* check elbow pos */
483         err = ax12_user_read_int(&gen.ax12, arm->config.elbow_ax12,
484                                  AA_PRESENT_POSITION_L, (uint16_t *)&pos);
485         if (err)
486                 goto fail;
487         if (ABS(arm->status.elbow_angle_steps - pos) > ARM_ELBOW_WINDOW_POS)
488                 *status |= ELBOW_NOT_IN_WIN;
489
490         /* check wrist pos */
491         err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12,
492                                  AA_PRESENT_POSITION_L, (uint16_t *)&pos);
493         if (err)
494                 goto fail;
495         if (ABS(arm->status.wrist_angle_steps - pos) > ARM_WRIST_WINDOW_POS)
496                 *status |= WRIST_NOT_IN_WIN;
497         
498         if (*status)
499                 return 0;
500
501         ARM_NOTICE("arm is in window (%ld us after reach pos)",
502                    time_get_us2() - arm->status.pos_reached_time);
503         return 1; /* ok, we are in window */
504
505  fail:
506         return 0;
507 }
508
509 /* process wrist pos and apply it. it's done only once. */
510 static int8_t arm_set_wrist(struct arm *arm)
511 {
512         int8_t err;
513         int32_t as_deg, ae_deg, aw_deg;
514         uint16_t wrist_out_u16;
515         double wrist_out, as_rad, ae_rad;
516         int16_t pos;
517         uint32_t diff_time;
518
519         /* calculate the destination angle of arm in radian */
520         err = cart2angle(arm->req.h_mm, arm->req.d_mm, 
521                          &as_rad, &ae_rad);
522         if (err)
523                 return -1;
524
525         /* calc angle destination */
526         as_deg = DEG(as_rad);
527         ae_deg = DEG(ae_rad);
528         ARM_DEBUG("as_dest_deg=%d ae_dest_deg=%d", as_deg, ae_deg);
529         aw_deg = as_deg + ae_deg - arm->req.w_deg;
530         arm->config.wrist_angle_deg2robot(aw_deg, &wrist_out);
531         wrist_out_u16 = wrist_out;
532
533         ARM_DEBUG("set wrist to %ld degrees (%d steps)", aw_deg,
534                   wrist_out_u16);
535
536         /* process the theorical reach time for the wrist */
537         if (arm->config.simulate) {
538                 pos = arm->status.wrist_angle_steps;
539         }
540         else {
541                 err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12,
542                                     AA_PRESENT_POSITION_L, (uint16_t *)&pos);
543                 if (err)
544                         pos = arm->status.wrist_angle_steps;
545         }
546         /* 600 is the number of steps/s */
547         diff_time = (ABS((int16_t)wrist_out_u16 - pos) * 1000000L) / 600;
548         arm->status.wrist_reach_time = arm->status.start_time + diff_time;
549         ARM_DEBUG("wrist reach time is %ld (diff=%ld)", 
550                   arm->status.wrist_reach_time, diff_time);
551
552         /* update current position to destination */
553         arm->status.wrist_angle_steps = wrist_out_u16;
554         
555         if (arm->config.simulate)
556                 return 0;
557
558         /* send it to ax12 */
559         ax12_user_write_int(&gen.ax12, arm->config.wrist_ax12, 
560                             AA_GOAL_POSITION_L, wrist_out_u16);
561         return 0;
562 }
563
564 /* event callback */
565 static void arm_do_xy_cb(struct arm *arm)
566 {
567         uint8_t win_status;
568
569         arm->status.event = -1;
570
571         /* if consign haven't reach destination */
572         if ((arm->status.state & ARM_FLAG_LAST_STEP) == 0) {
573                 if (arm_do_step(arm))
574                         arm->status.state |= ARM_FLAG_ERROR;
575
576                 /* it's the first call for the traj */
577                 if (arm->status.state == ARM_STATE_INIT) {
578                         arm->status.state |= ARM_FLAG_MOVING;
579                         if (arm_set_wrist(arm))
580                                 arm->status.state |= ARM_FLAG_ERROR;
581                 }
582
583                 /* we have more steps to do */
584                 if (arm->status.next_update_time == 0) {
585                         arm->status.state &= ~ARM_FLAG_MOVING;
586                         arm->status.state |= ARM_FLAG_LAST_STEP;
587                         arm->status.pos_reached_time = time_get_us2();
588                 }
589                 arm_apply(arm);
590         }
591         /* last step is reached, we can check that arm is in window */
592         else if ((arm->status.state & ARM_FLAG_IN_WINDOW) == 0) {
593                 if (arm_in_window(arm, &win_status))
594                         arm->status.state |= ARM_FLAG_IN_WINDOW;
595                 
596                 /* check for window arm timeout */
597                 else {
598                         microseconds t;
599                         int32_t diff1, diff2;
600                         t = time_get_us2();
601                         diff1 = t - arm->status.pos_reached_time; 
602                         diff2 = t - arm->status.wrist_reach_time; 
603                         if (diff1 > ARM_WINDOW_TIMEOUT &&
604                             diff2 > ARM_WINDOW_TIMEOUT) {
605                                 ARM_NOTICE("win timeout at %ld win_status=%x",
606                                            t, win_status);
607                                 arm->status.state |= ARM_FLAG_TIMEOUT;
608                         }
609                 }
610         }
611
612         /* check for global arm timeout */
613         if ((time_get_us2() - arm->status.start_time) > ARM_GLOBAL_TIMEOUT) {
614                 ARM_NOTICE("global timeout at %ld", time_get_us2());
615                 arm->status.state |= ARM_FLAG_TIMEOUT;
616         }
617         
618         /* reload event if needed */
619         if ((arm->status.state & ARM_FLAG_FINISHED) == ARM_FLAG_FINISHED) {
620                 ARM_NOTICE("arm traj finished");
621                 return; /* no more event, position reached */
622         }
623         if (arm->status.state & (ARM_FLAG_ERROR|ARM_FLAG_TIMEOUT)) {
624                 ARM_NOTICE("error or timeout");
625                 return; /* no more event */
626         }
627         else if (arm->status.state & ARM_FLAG_LAST_STEP) {
628                 /* theorical position is reached, but reload an event
629                  * for position survey (window), every 25ms */
630                 arm_schedule_event(arm, ARM_SURVEY_PERIOD);
631         }
632         else {
633                 /* reload event for next position step */
634                 arm_schedule_event(arm, arm->status.next_update_time);
635         }
636 }
637
638 /* schedule a single event for this arm */
639 static void arm_schedule_event(struct arm *arm, uint32_t time)
640 {
641         uint8_t flags;
642         int8_t ret;
643
644         arm_delete_event(arm);
645         if (time < SCHEDULER_UNIT)
646                 time = SCHEDULER_UNIT;
647         IRQ_LOCK(flags);
648         ret = scheduler_add_event(SCHEDULER_SINGLE,
649                                   (void *)arm_do_xy_cb,
650                                   arm, time/SCHEDULER_UNIT, ARM_PRIO);
651         if (ret == -1) {
652                 IRQ_UNLOCK(flags);
653                 ARM_ERROR("Cannot load arm event");
654                 return;
655         }
656         arm->status.event = ret;
657         IRQ_UNLOCK(flags);
658 }
659
660 int8_t arm_do_xy(struct arm *arm, int16_t d_mm, int16_t h_mm, int16_t w_deg)
661 {
662         ARM_NOTICE("arm_do_xy: d_mm=%d h_mm=%d w_deg=%d", d_mm, h_mm, w_deg);
663
664         /* remove previous event if any */
665         arm_delete_event(arm);
666
667         /* init mandatory params */
668         arm->req.d_mm = d_mm; 
669         arm->req.h_mm = h_mm; 
670         arm->req.w_deg = w_deg; 
671         arm->status.start_time = time_get_us2();
672         arm->status.state = ARM_STATE_INIT;
673
674         /* all the job will be done asynchronously now */
675         arm_schedule_event(arm, 0);
676         return 0;
677 }
678
679 void arm_dump(struct arm *arm)
680 {
681         printf_P(PSTR("config: simulate=%d\r\n"),
682                  arm->config.simulate);
683         printf_P(PSTR("req: d_mm=%ld h_mm=%ld w_deg=%ld\r\n"),
684                  arm->req.d_mm, arm->req.h_mm, arm->req.w_deg);
685         printf_P(PSTR("status: "));
686         if (arm->status.state == ARM_STATE_INIT)
687                 printf_P(PSTR("ARM_STATE_INIT "));
688         if (arm->status.state & ARM_FLAG_MOVING)
689                 printf_P(PSTR("ARM_FLAG_MOVING "));
690         if (arm->status.state & ARM_FLAG_LAST_STEP)
691                 printf_P(PSTR("ARM_FLAG_LAST_STEP "));
692         if (arm->status.state & ARM_FLAG_IN_WINDOW)
693                 printf_P(PSTR("ARM_FLAG_IN_WINDOW "));
694         if (arm->status.state & ARM_FLAG_ERROR)
695                 printf_P(PSTR("ARM_FLAG_ERROR "));
696         if (arm->status.state & ARM_FLAG_TIMEOUT)
697                 printf_P(PSTR("ARM_FLAG_TIMEOUT "));
698         printf_P(PSTR("\r\n"));
699
700         printf_P(PSTR("   d_mm=%ld h_mm=%ld goal_w_steps=%d\r\n"),
701                  arm->status.d_mm, arm->status.h_mm, arm->status.wrist_angle_steps);
702         printf_P(PSTR("   cur_shl_steps=%ld cur_elb_steps=%ld\r\n"),
703                  arm->status.shoulder_angle_steps, arm->status.elbow_angle_steps);
704         printf_P(PSTR("   cur_shl_rad=%f cur_elb_rad=%f\r\n"),
705                  arm->status.shoulder_angle_rad, arm->status.elbow_angle_rad);
706         printf_P(PSTR("   cur_shl_deg=%f cur_elb_deg=%f\r\n"),
707                  DEG(arm->status.shoulder_angle_rad), DEG(arm->status.elbow_angle_rad));
708         printf_P(PSTR("   event=%d next_update_time=%ld\r\n"),
709                  arm->status.event, arm->status.next_update_time);
710         printf_P(PSTR("   start_time=%ld pos_reached_time=%ld wrist_reach_time=%ld\r\n"),
711                  arm->status.start_time, arm->status.pos_reached_time,
712                  arm->status.wrist_reach_time);
713 }
714
715 #define CALIB_ANGLE (RAD(-93.))
716
717 void arm_calibrate(void)
718 {
719         double shoulder, elbow;
720
721         pwm_ng_set(LEFT_ARM_PWM, 500);
722         pwm_ng_set(RIGHT_ARM_PWM, -500);
723         wait_ms(200);
724
725         pwm_ng_set(LEFT_ARM_PWM, 300);
726         pwm_ng_set(RIGHT_ARM_PWM, -300);
727         wait_ms(700);
728
729         printf_P(PSTR("Init arm, please wait..."));
730         ax12_user_write_int(&gen.ax12, AX12_BROADCAST_ID, AA_TORQUE_ENABLE, 0x1);
731         ax12_user_write_int(&gen.ax12, AX12_BROADCAST_ID, AA_ALARM_SHUTDOWN, 0x04);
732
733         angle_rad2robot_r(0, CALIB_ANGLE, &shoulder, &elbow);
734         ax12_user_write_int(&gen.ax12, R_ELBOW_AX12, AA_GOAL_POSITION_L, elbow);
735         ax12_user_write_int(&gen.ax12, R_WRIST_AX12, AA_GOAL_POSITION_L, 628);
736
737         angle_rad2robot_l(0, CALIB_ANGLE, &shoulder, &elbow);
738         ax12_user_write_int(&gen.ax12, L_ELBOW_AX12, AA_GOAL_POSITION_L, elbow);
739         ax12_user_write_int(&gen.ax12, L_WRIST_AX12, AA_GOAL_POSITION_L, 394);
740         pwm_ng_set(LEFT_ARM_PWM, -100);
741         pwm_ng_set(RIGHT_ARM_PWM, 100);
742
743         wait_ms(2000);
744
745         cs_set_consign(&mechboard.left_arm.cs, 0);
746         cs_set_consign(&mechboard.right_arm.cs, 0);
747         encoders_spi_set_value(LEFT_ARM_ENCODER, 0);
748         encoders_spi_set_value(RIGHT_ARM_ENCODER, 0);
749
750         printf_P(PSTR("ok\r\n"));
751 }
752
753 /* init arm config */
754 void arm_init(void)
755 {
756         uint32_t shoulder_robot;
757         uint16_t elbow_robot, wrist_robot;
758         double shoulder_rad, elbow_rad;
759         int32_t h, d;
760         uint8_t err = 0;
761
762         memset(&left_arm.status, 0, sizeof(left_arm.status));
763         memset(&right_arm.status, 0, sizeof(right_arm.status));
764         left_arm.status.event = -1;
765         right_arm.status.event = -1;
766
767         arm_calibrate();
768
769         /* set des slopes XXX */
770
771         /* set maximum moving speeds */
772         err |= ax12_user_write_int(&gen.ax12, L_ELBOW_AX12, AA_MOVING_SPEED_L, 0x3ff);
773         err |= ax12_user_write_int(&gen.ax12, L_WRIST_AX12, AA_MOVING_SPEED_L, 0x3ff);
774         err |= ax12_user_write_int(&gen.ax12, R_ELBOW_AX12, AA_MOVING_SPEED_L, 0x3ff);
775         err |= ax12_user_write_int(&gen.ax12, R_WRIST_AX12, AA_MOVING_SPEED_L, 0x3ff);
776
777         /* left arm init */
778         shoulder_robot = encoders_spi_get_value(LEFT_ARM_ENCODER);
779         err |= ax12_user_read_int(&gen.ax12, L_ELBOW_AX12, AA_PRESENT_POSITION_L, &elbow_robot);
780         err |= ax12_user_read_int(&gen.ax12, L_WRIST_AX12, AA_PRESENT_POSITION_L, &wrist_robot);
781         
782         angle_robot2rad_l(shoulder_robot, elbow_robot,
783                           &shoulder_rad, &elbow_rad);
784         angle2cart(shoulder_rad, elbow_rad, &h, &d);
785         printf_P(PSTR("left arm: h:%ld d:%ld w:%d\r\n"), h, d, wrist_robot);
786         left_arm.status.h_mm = h;
787         left_arm.status.d_mm = d;
788         left_arm.status.wrist_angle_steps = wrist_robot;
789         left_arm.status.state = ARM_FLAG_FINISHED;
790         left_arm.config.csb = &mechboard.left_arm;
791
792         /* left arm init */
793         shoulder_robot = encoders_spi_get_value(RIGHT_ARM_ENCODER);
794         err |= ax12_user_read_int(&gen.ax12, R_ELBOW_AX12, AA_PRESENT_POSITION_L, &elbow_robot);
795         err |= ax12_user_read_int(&gen.ax12, R_WRIST_AX12, AA_PRESENT_POSITION_L, &wrist_robot);
796         
797         angle_robot2rad_r(shoulder_robot, elbow_robot,
798                           &shoulder_rad, &elbow_rad);
799         angle2cart(shoulder_rad, elbow_rad, &h, &d);
800         printf_P(PSTR("right arm: h:%ld d:%ld w:%d\r\n"), h, d, wrist_robot);
801         right_arm.status.h_mm = h;
802         right_arm.status.d_mm = d;
803         right_arm.status.wrist_angle_steps = wrist_robot;
804         right_arm.status.state = ARM_FLAG_FINISHED;
805         right_arm.config.csb = &mechboard.right_arm;
806
807         if (err)
808                 ARM_ERROR("ARM INIT ERROR");
809 }