projects/microb2009/tests/arm_test/arm_xy.h

   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 #define ARM_L 130UL
  10 #define ARM_H 250UL
  11
  12 #define DIM_COEF  50
  13 #define DIM_D  6  /* distance */
  14 #define DIM_H  10 /* height */
  15
  16 /* coefs to fit in a integer */
  17 #define INT_M_SHOULDER 3
  18 #define INT_M_ELBOW 3
  19
  20 #define sqrtf sqrt
  21 #define acosf acos
  22 #define atan2f atan2
  23
  24 typedef struct arm_pts
  25 {
  26         int16_t p_shoulder;
  27         uint8_t p_elbow;
  28 } arm_pts_t;
  29
  30
  31 typedef struct arm_pos{
  32         int32_t h;
  33         int32_t d;
  34         int32_t w;
  35
  36         int32_t goal_h;
  37         int32_t goal_d;
  38         int32_t goal_w;
  39
  40
  41         volatile int8_t state;
  42
  43         void (*wrist_angle_deg2robot)(double wrist_edg,
  44                                       double *wrist_out);
  45
  46         void (*angle_rad2robot)(double shoulder_deg, double elbow_deg,
  47                                 double *shoulder_out, double *elbow_out);
  48
  49         void (*angle_robot2rad)(double shoulder_robot, double elbow_robot,
  50                                 double *shoulder_rad, double *elbow_rad);
  51
  52         int8_t ELBOW_AX12;
  53         int8_t WRIST_AX12;
  54
  55
  56 }Arm_Pos;
  57
  58
  59
  60 /* process shoulder + elbow angles from height and distance */
  61 int8_t cart2angle(int32_t h, int32_t d, double *alpha, double *beta);
  62
  63 /* process height and distance from shoulder + elbow angles */
  64 void angle2cart(double alpha, double beta, int32_t *h, int32_t *d);
  65
  66 void wrist_angle_deg2robot_r(double wrist_edg,
  67                              double *wrist_out);
  68
  69 void wrist_angle_deg2robot_l(double wrist_edg,
  70                              double *wrist_out);
  71
  72 /* convert an angle in degree into a robot-specific unit
  73  * for shoulder and elbow */
  74 void angle_rad2robot_r(double shoulder_deg, double elbow_deg,
  75                        double *shoulder_out, double *elbow_out);
  76
  77 void angle_rad2robot_l(double shoulder_deg, double elbow_deg,
  78                        double *shoulder_out, double *elbow_out);
  79
  80
  81 /* convert  a robot-specific unit into an angle in radian
  82  * for shoulder and elbow */
  83 void angle_robot2rad_r(double shoulder_robot, double elbow_robot,
  84                        double *shoulder_rad, double *elbow_rad);
  85 void angle_robot2rad_l(double shoulder_robot, double elbow_robot,
  86                        double *shoulder_rad, double *elbow_rad);
  87
  88
  89 /* generate the angle matrix for each (d,h) position */
  90 void init_arm_matrix(void);
  91 void dump_matrix(void);
  92
  93 /* same than cart2angle, but uses a bilinear interpolation */
  94 int8_t coord2ij(int32_t d, int32_t h, int32_t *d_o, int32_t *h_o);
  95
  96 int8_t arm_get_coord(int16_t d,int16_t h, int32_t *d_o, int32_t *h_o);
  97
  98 void test_arm_pos(void);
  99
 100 /*
 101 int8_t arm_do_step(int32_t *arm_h, int32_t *arm_d, int32_t fin_h, int32_t fin_d,
 102                    int32_t *as, int32_t *ae, uint32_t *next_time,
 103                    int32_t *s_quad);
 104 */
 105
 106 int8_t arm_do_step(Arm_Pos *arm_pos,
 107                    int32_t *arm_h, int32_t *arm_d, int32_t fin_h, int32_t fin_d,
 108                    int32_t *as, int32_t *ae, int32_t *aw,
 109                    double *as_fin_rad, double *ae_fin_rad,
 110                    uint32_t *next_time,
 111                    int32_t *s_quad, int32_t *e_quad);
 112
 113 #if 0
 114
 115 int main(void)
 116 {
 117         double a, b;
 118         int8_t ret;
 119         int32_t as, ae;
 120         ret = cart2angle(100, 100, &a, &b);
 121         printf("ret %d %f %f\n", ret, a*180/M_PI, b*180/M_PI);
 122
 123         init_arm_matrix();
 124         dump_matrix();
 125
 126
 127         int32_t px = 100;
 128         int32_t py = -230;
 129         ret = coord2ij(px,py, &as, &ae);
 130         printf("ret %d %ld %ld\n", ret, as, ae);
 131
 132
 133         ret = cart2angle(py, px, &a, &b);
 134         printf("ret %d %f %f\n", ret, a*180/M_PI, b*180/M_PI);
 135
 136         ret = arm_get_coord(px,py, &as, &ae);
 137         printf("ret %d %ld %ld\n", ret, as, ae);
 138
 139         test_arm_pos();
 140         return 0;
 141 }
 142 #endif