projects/microb2009/tests/arm_test/arm_xy.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <math.h>
   4 #include <stdint.h>
   5 #include <inttypes.h>
   6
   7 #include <aversive.h>
   8
   9 #include <aversive/wait.h>
  10
  11
  12 #include <uart.h>
  13 #include <i2c.h>
  14 #include <ax12.h>
  15 #include <parse.h>
  16 #include <rdline.h>
  17 #include <pwm_ng.h>
  18 #include <encoders_microb.h>
  19 #include <timer.h>
  20 #include <scheduler.h>
  21 #include <pid.h>
  22 #include <time.h>
  23 #include <quadramp.h>
  24 #include <control_system_manager.h>
  25 #include <adc.h>
  26 #include <spi.h>
  27
  28 #include "main.h"
  29
  30 #include "arm_xy.h"
  31
  32 arm_pts_t mat_angle[DIM_D][DIM_H];
  33
  34
  35
  36 /* process shoulder + elbow angles from height and distance */
  37 int8_t cart2angle(int32_t h, int32_t d, double *alpha, double *beta)
  38 {
  39         double theta, l, phi;
  40
  41         //l = SquareRootDouble((double)(d*d + h*h));
  42         l = sqrt((double)(d*d + h*h));
  43         if  (l>2*ARM_L)
  44                 return -1;
  45         theta = atan2f((double)h, (double)d);
  46         phi = acosf(l/(2*ARM_L));
  47
  48         *alpha = theta+phi;
  49         *beta = -2*phi;
  50
  51         return 0;
  52 }
  53
  54
  55 /* process height and distance from shoulder + elbow angles */
  56 void angle2cart(double alpha, double beta, int32_t *h, int32_t *d)
  57 {
  58         double tmp_a;
  59         int32_t tmp_h, tmp_d;
  60
  61         tmp_h = ARM_L * sin(alpha);
  62         tmp_d = ARM_L * cos(alpha);
  63
  64         tmp_a = alpha+beta;
  65         *h = tmp_h + ARM_L * sin(tmp_a);
  66         *d = tmp_d + ARM_L * cos(tmp_a);
  67
  68 }
  69
  70 void wrist_angle_deg2robot_r(double wrist_deg,
  71                      double *wrist_out)
  72 {
  73         *wrist_out = wrist_deg * 3.41  + 875;
  74 }
  75
  76
  77 void wrist_angle_deg2robot_l(double wrist_deg,
  78                      double *wrist_out)
  79 {
  80         *wrist_out = wrist_deg * 3.41  + 40;
  81 }
  82
  83
  84 #define ARM_S_OFFSET -16000
  85 #define ARM_E_OFFSET 660
  86
  87 /* convert an angle in radian into a robot-specific unit
  88  * for shoulder and elbow */
  89 void angle_rad2robot_r(double shoulder_rad, double elbow_rad,
  90                      double *shoulder_robot, double *elbow_robot)
  91 {
  92         *shoulder_robot = -shoulder_rad * 4 * 66 * 512. / (2*M_PI) + ARM_S_OFFSET;
  93         *elbow_robot = elbow_rad * 3.41 * 360. / (2*M_PI) + ARM_E_OFFSET;
  94 }
  95
  96
  97 #define ARM_LEFT_S_OFFSET -16000
  98 #define ARM_LEFT_E_OFFSET 470
  99
 100
 101 /* convert an angle in radian into a robot-specific unit
 102  * for shoulder and elbow for LEFT ARM*/
 103 void angle_rad2robot_l(double shoulder_rad, double elbow_rad,
 104                        double *shoulder_robot, double *elbow_robot)
 105 {
 106         *shoulder_robot = -shoulder_rad * 4 * 66 * 512. / (2*M_PI) + ARM_LEFT_S_OFFSET;
 107         *elbow_robot = elbow_rad * 3.41 * 360. / (2*M_PI) + ARM_LEFT_E_OFFSET;
 108 }
 109
 110
 111
 112 /* convert  a robot-specific unit into an angle in radian
 113  * for shoulder and elbow */
 114 void angle_robot2rad_r(double shoulder_robot, double elbow_robot,
 115                      double *shoulder_rad, double *elbow_rad)
 116 {
 117         *shoulder_rad = -((shoulder_robot - ARM_S_OFFSET)*(2*M_PI))/(4 * 66 * 512.);
 118         *elbow_rad = ((elbow_robot - ARM_E_OFFSET) * (2*M_PI))/(3.41 * 360.);
 119 }
 120
 121 /* convert  a robot-specific unit into an angle in radian
 122  * for shoulder and elbow for LEFT ARM*/
 123 void angle_robot2rad_l(double shoulder_robot, double elbow_robot,
 124                      double *shoulder_rad, double *elbow_rad)
 125 {
 126         *shoulder_rad = -((shoulder_robot - ARM_LEFT_S_OFFSET)*(2*M_PI))/(4 * 66 * 512.);
 127         *elbow_rad = ((elbow_robot - ARM_LEFT_E_OFFSET) * (2*M_PI))/(3.41 * 360.);
 128 }
 129
 130
 131 /* generate the angle matrix for each (d,h) position */
 132 void init_arm_matrix(void)
 133 {
 134         int8_t d, h, ret;
 135         double a, b;
 136
 137         for (d = 0; d < DIM_D; d++){
 138                 for (h = 0; h < DIM_H; h++){
 139                         ret = cart2angle((h * DIM_COEF - ARM_H), d * DIM_COEF, &a, &b);
 140                         if (ret) {
 141                                 mat_angle[d][h].p_shoulder = 0;
 142                                 mat_angle[d][h].p_elbow = 0;
 143                         }
 144                         else {
 145                                 angle_rad2robot_r(a, b, &a, &b);
 146                                 mat_angle[d][h].p_shoulder = a / INT_M_SHOULDER;
 147                                 mat_angle[d][h].p_elbow = b / INT_M_ELBOW;
 148                         }
 149                 }
 150         }
 151 }
 152
 153 void dump_matrix(void)
 154 {
 155         int8_t d, h;
 156         for (h = 0; h < DIM_H; h++) {
 157                 for (d = 0; d < DIM_D; d++) {
 158                         printf("%6d %4d ", mat_angle[d][h].p_shoulder*INT_M_SHOULDER, mat_angle[d][h].p_elbow*INT_M_ELBOW);
 159                 }
 160                 printf("\n");
 161         }
 162 }
 163
 164 /* same than cart2angle, but uses a bilinear interpolation */
 165 int8_t coord2ij(int32_t d, int32_t h, int32_t *d_o, int32_t *h_o)
 166 {
 167         int8_t i_o, j_o;
 168
 169         int32_t a00, a10, a11, a01;
 170         uint16_t b00, b10, b11, b01;
 171
 172         int32_t poids1, poids2;
 173         int32_t atmp1, atmp2, btmp1, btmp2;
 174
 175         if (d*d + h*h >(ARM_L*2)*(ARM_L*2))
 176                 return -6;
 177
 178         i_o = d / DIM_COEF;
 179         j_o = (h+ARM_H) / DIM_COEF;
 180
 181         if (i_o >= DIM_D || j_o >= DIM_H || i_o < 0 || j_o < 0)
 182                 return -1;
 183
 184         a00 = mat_angle[i_o][j_o].p_shoulder * INT_M_SHOULDER;
 185         b00 = mat_angle[i_o][j_o].p_elbow * INT_M_ELBOW;
 186         if (!(a00 || b00))
 187                 return -2;
 188         a10 = mat_angle[i_o+1][j_o].p_shoulder * INT_M_SHOULDER;
 189         b10 = mat_angle[i_o+1][j_o].p_elbow * INT_M_ELBOW;
 190         if (!(a10 || b10))
 191                 return -3;
 192
 193         a11 = mat_angle[i_o+1][j_o+1].p_shoulder * INT_M_SHOULDER;
 194         b11 = mat_angle[i_o+1][j_o+1].p_elbow * INT_M_ELBOW;
 195         if (!(a11 || b11))
 196                 return -4;
 197
 198         a01 = mat_angle[i_o][j_o+1].p_shoulder * INT_M_SHOULDER;
 199         b01 = mat_angle[i_o][j_o+1].p_elbow * INT_M_ELBOW;
 200         if (!(a01 || b01))
 201                 return -5;
 202
 203         poids1 = d - DIM_COEF * i_o;
 204         poids2 = DIM_COEF * (i_o+1) - d;
 205
 206         atmp1 = (poids1*a10 + poids2*a00)/DIM_COEF;
 207         btmp1 = (poids1*b10 + poids2*b00)/DIM_COEF;
 208
 209         atmp2 = (poids1*a11 + poids2*a01)/DIM_COEF;
 210         btmp2 = (poids1*b11 + poids2*b01)/DIM_COEF;
 211
 212         poids1 = (h+ARM_H) - DIM_COEF*j_o;
 213         poids2 = DIM_COEF*(j_o+1) - (h+ARM_H);
 214
 215         *d_o = (poids1*atmp2 + poids2*atmp1) / DIM_COEF;
 216         *h_o = (poids1*btmp2 + poids2*btmp1) / DIM_COEF;
 217
 218         return 0;
 219 }
 220
 221
 222 int8_t arm_get_coord(int16_t d,int16_t h, int32_t *d_o, int32_t *h_o)
 223 {
 224         int8_t ret;
 225         double a, b;
 226
 227         ret = coord2ij(d,h, d_o,h_o);
 228         if (!ret)
 229                 return ret;
 230
 231         ret = cart2angle(h, d, &a, &b);
 232         if (ret<0)
 233                 return ret;
 234
 235         angle_rad2robot_r(a, b, &a, &b);
 236         *d_o = a;
 237         *h_o = b;
 238         return ret;
 239
 240 }
 241
 242 void test_arm_pos(void)
 243 {
 244         int16_t d, h;
 245         int8_t ret;
 246         int32_t as, ae;
 247
 248         for (d=-300; d<300; d+=25){
 249                 for (h=-300; h<300; h+=25){
 250                         ret = arm_get_coord(d,h, &as, &ae);
 251                         if (ret < 0)
 252                                 as = ae = 0;
 253                         printf("%7ld %4ld ", as, ae);
 254                 }
 255                 printf("\n");
 256         }
 257 }
 258 /*
 259 #define ARM_PERIOD 50000
 260
 261 #define ARM_MAX_DIST 80
 262 #define ARM_MAX_E 10000
 263 #define ARM_MAX_S 30000
 264 */
 265
 266 //100000 us for max shoulder / max elbow
 267 //en us
 268 #define ARM_PERIOD 50000L
 269
 270 #define ARM_MAX_DIST 40L
 271 // 2331 step/s =>
 272 //#define ARM_MAX_E ((2331L*ARM_PERIOD)/1000000L)
 273 /*
 274 // 800step/CS => 160step/ms
 275 #define ARM_MAX_S ((160L*ARM_PERIOD)/1000L)
 276 */
 277 // 4000step/CS => 800step/ms
 278 //#define ARM_MAX_S ((800L*ARM_PERIOD)/1000L)
 279
 280
 281
 282 /* TEST TEST XXX */
 283 #define ARM_MAX_E (((1500L*ARM_PERIOD)/1000000L))
 284 #define ARM_MAX_S (((1500L*ARM_PERIOD)/1000L))
 285
 286
 287
 288
 289 /* return:
 290    0 => at last pos
 291    >0 => need more step
 292    <0 => unreachable pos
 293 */
 294 int8_t arm_do_step(Arm_Pos *arm_pos,
 295                    int32_t *arm_h, int32_t *arm_d, int32_t fin_h, int32_t fin_d,
 296                    int32_t *as, int32_t *ae, int32_t *aw,
 297                    double *as_fin_rad, double *ae_fin_rad,
 298                    uint32_t *next_time,
 299                    int32_t *s_quad, int32_t *e_quad)
 300 {
 301         int8_t ret;
 302         int32_t diff_h, diff_d, l;
 303         double as_fin, ae_fin, as_cur, ae_cur, as_cur_rad, ae_cur_rad;//, as_fin_rad, ae_fin_rad;
 304         double as_coef, ae_coef;
 305         int32_t as_diff, ae_diff;
 306
 307
 308         diff_h = fin_h-*arm_h;
 309         diff_d = fin_d-*arm_d;
 310
 311         //printf("1 dh %ld dd %ld\r\n", diff_h, diff_d);
 312
 313         l = sqrt(diff_h*diff_h + diff_d*diff_d);
 314
 315         if (l>ARM_MAX_DIST){
 316                 diff_h = diff_h*ARM_MAX_DIST/l;
 317                 diff_d = diff_d*ARM_MAX_DIST/l;
 318         }
 319
 320         //printf("2 l: %ld dh %ld dd %ld\r\n", l, diff_h, diff_d);
 321
 322
 323         fin_h = *arm_h+diff_h;
 324         fin_d = *arm_d+diff_d;
 325
 326         //printf("2 fh %ld fd %ld\r\n", fin_h, fin_d);
 327
 328         /* calc for current angle */
 329         ret = cart2angle(*arm_h, *arm_d, &as_cur_rad, &ae_cur_rad);
 330         if (ret)
 331                 return ret;
 332
 333         /* calc for next step */
 334         ret = cart2angle(fin_h, fin_d, as_fin_rad, ae_fin_rad);
 335         if (ret)
 336                 return ret;
 337
 338
 339
 340         //printf("3 as_cur %f ae_cur %f \r\n", as_cur_rad, ae_cur_rad);
 341         //printf("4 as_fin %f ae_fin %f \r\n", *as_fin_rad, *ae_fin_rad);
 342
 343
 344         arm_pos->angle_rad2robot(as_cur_rad, ae_cur_rad,
 345                                  &as_cur, &ae_cur);
 346         arm_pos->angle_rad2robot(*as_fin_rad, *ae_fin_rad,
 347                                  &as_fin, &ae_fin);
 348
 349
 350         //printf("5 as_cur %f ae_cur %f \r\n", as_cur, ae_cur);
 351         //printf("6 as_fin %f ae_fin %f \r\n", as_fin, ae_fin);
 352
 353
 354         as_diff = as_fin-as_cur;
 355         ae_diff = ae_fin-ae_cur;
 356
 357         //printf("7 asdiff %ld aediff %ld \r\n", as_diff, ae_diff);
 358
 359
 360         *arm_h = fin_h;
 361         *arm_d = fin_d;
 362
 363         /* XXX we are at pos, set default speed */
 364         *s_quad = 800;
 365         *e_quad = 0x3ff;
 366
 367         if (!as_diff && !ae_diff){
 368                 /* printf("reaching end\r\n"); */
 369                 *as = as_fin;
 370                 *ae = ae_fin;
 371                 *next_time = 0;
 372
 373
 374                 return 0;
 375         }
 376
 377         /* test if one activator is in position */
 378         if (as_diff == 0){
 379                 //printf("as reached\r\n");
 380                 ae_coef = (double)ARM_MAX_E/(double)ae_diff;
 381                 *next_time = ARM_PERIOD*ABS(ae_coef);
 382
 383                 *e_quad = ABS(ae_coef)*ABS(ae_diff);
 384
 385                 *as = as_cur;
 386                 *ae = ae_cur+ae_diff;
 387
 388
 389         }
 390         else if (ae_diff == 0){
 391                 //printf("ae reached\r\n");
 392                 as_coef = (double)ARM_MAX_S/(double)as_diff;
 393                 *next_time = ARM_PERIOD/ABS(as_coef);
 394
 395
 396                 *s_quad = ABS(as_coef)*ABS(as_diff);
 397
 398                 *as = as_cur+as_diff;
 399                 *ae = ae_cur;
 400         }
 401
 402         else{
 403             as_coef = (double)ARM_MAX_S/(double)as_diff;
 404             ae_coef = (double)ARM_MAX_E/(double)ae_diff;
 405
 406             //printf("asc %f aec %f\r\n", as_coef, ae_coef);
 407
 408             *as = as_cur+as_diff;
 409             *ae = ae_cur+ae_diff;
 410
 411
 412             if (ABS(as_coef)>=ABS(ae_coef)){
 413                 /* elbow is limitating */
 414
 415                 //printf("e limit %ld %ld \r\n", as_diff, ae_diff);
 416
 417                 *next_time = ARM_PERIOD/ABS(ae_coef);
 418                 //*next_time = ARM_PERIOD*ae_diff/ARM_MAX_E;
 419
 420
 421                 *s_quad = ABS(ae_coef)*ABS(as_diff);
 422                 *e_quad = ABS(ae_coef)*ABS(ae_diff);
 423
 424
 425             }
 426             else{
 427                 /* shoulder is limitating */
 428
 429                 //printf("s limit %ld %ld \r\n", as_diff, ae_diff);
 430
 431                 *next_time = ARM_PERIOD/ABS(as_coef);
 432
 433                 *s_quad = ABS(as_coef)*ABS(as_diff);
 434                 *e_quad = ABS(as_coef)*ABS(ae_diff);
 435
 436
 437             }
 438         }
 439
 440         *s_quad = (*s_quad*CS_PERIOD)/ARM_PERIOD;
 441         *e_quad = (0x3ff*(*e_quad))/ARM_MAX_E;
 442
 443         //printf("max e %ld\r\n", *e_quad);
 444         /*avoid limits*/
 445         if (!*s_quad)
 446                 *s_quad = 1;
 447         if (!*e_quad)
 448                 *e_quad = 1;
 449
 450
 451         //printf("8 sq %ld eq %ld \r\n", *s_quad, *e_quad);
 452
 453         return 1;
 454
 455 }
 456
 457
 458 #if 0
 459
 460 int main(void)
 461 {
 462         double a, b;
 463         int8_t ret;
 464         int32_t as, ae;
 465         ret = cart2angle(100, 100, &a, &b);
 466         printf("ret %d %f %f\n", ret, a*180/M_PI, b*180/M_PI);
 467
 468         init_arm_matrix();
 469         dump_matrix();
 470
 471
 472         int32_t px = 100;
 473         int32_t py = -230;
 474         ret = coord2ij(px,py, &as, &ae);
 475         printf("ret %d %ld %ld\n", ret, as, ae);
 476
 477
 478         ret = cart2angle(py, px, &a, &b);
 479         printf("ret %d %f %f\n", ret, a*180/M_PI, b*180/M_PI);
 480
 481         ret = arm_get_coord(px,py, &as, &ae);
 482         printf("ret %d %ld %ld\n", ret, as, ae);
 483
 484         test_arm_pos();
 485         return 0;
 486 }
 487 #endif