13 #define DIM_D 6 /* distance */
14 #define DIM_H 10 /* height */
16 /* coefs to fit in a integer */
17 #define INT_M_SHOULDER 3
24 typedef struct arm_pts
31 typedef struct arm_pos{
41 volatile int8_t state;
43 void (*wrist_angle_deg2robot)(double wrist_edg,
46 void (*angle_rad2robot)(double shoulder_deg, double elbow_deg,
47 double *shoulder_out, double *elbow_out);
49 void (*angle_robot2rad)(double shoulder_robot, double elbow_robot,
50 double *shoulder_rad, double *elbow_rad);
60 /* process shoulder + elbow angles from height and distance */
61 int8_t cart2angle(int32_t h, int32_t d, double *alpha, double *beta);
63 /* process height and distance from shoulder + elbow angles */
64 void angle2cart(double alpha, double beta, int32_t *h, int32_t *d);
66 void wrist_angle_deg2robot_r(double wrist_edg,
69 void wrist_angle_deg2robot_l(double wrist_edg,
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);
77 void angle_rad2robot_l(double shoulder_deg, double elbow_deg,
78 double *shoulder_out, double *elbow_out);
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);
89 /* generate the angle matrix for each (d,h) position */
90 void init_arm_matrix(void);
91 void dump_matrix(void);
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);
96 int8_t arm_get_coord(int16_t d,int16_t h, int32_t *d_o, int32_t *h_o);
98 void test_arm_pos(void);
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,
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,
111 int32_t *s_quad, int32_t *e_quad);
120 ret = cart2angle(100, 100, &a, &b);
121 printf("ret %d %f %f\n", ret, a*180/M_PI, b*180/M_PI);
129 ret = coord2ij(px,py, &as, &ae);
130 printf("ret %d %ld %ld\n", ret, as, ae);
133 ret = cart2angle(py, px, &a, &b);
134 printf("ret %d %f %f\n", ret, a*180/M_PI, b*180/M_PI);
136 ret = arm_get_coord(px,py, &as, &ae);
137 printf("ret %d %ld %ld\n", ret, as, ae);