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 }