9 #include <aversive/wait.h>
18 #include <encoders_microb.h>
20 #include <scheduler.h>
24 #include <control_system_manager.h>
32 arm_pts_t mat_angle[DIM_D][DIM_H];
36 /* process shoulder + elbow angles from height and distance */
37 int8_t cart2angle(int32_t h, int32_t d, double *alpha, double *beta)
41 //l = SquareRootDouble((double)(d*d + h*h));
42 l = sqrt((double)(d*d + h*h));
45 theta = atan2f((double)h, (double)d);
46 phi = acosf(l/(2*ARM_L));
55 /* process height and distance from shoulder + elbow angles */
56 void angle2cart(double alpha, double beta, int32_t *h, int32_t *d)
61 tmp_h = ARM_L * sin(alpha);
62 tmp_d = ARM_L * cos(alpha);
65 *h = tmp_h + ARM_L * sin(tmp_a);
66 *d = tmp_d + ARM_L * cos(tmp_a);
70 void wrist_angle_deg2robot_r(double wrist_deg,
73 *wrist_out = wrist_deg * 3.41 + 875;
77 void wrist_angle_deg2robot_l(double wrist_deg,
80 *wrist_out = wrist_deg * 3.41 + 40;
84 #define ARM_S_OFFSET -16000
85 #define ARM_E_OFFSET 660
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)
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;
97 #define ARM_LEFT_S_OFFSET -16000
98 #define ARM_LEFT_E_OFFSET 470
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)
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;
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)
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.);
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)
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.);
131 /* generate the angle matrix for each (d,h) position */
132 void init_arm_matrix(void)
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);
141 mat_angle[d][h].p_shoulder = 0;
142 mat_angle[d][h].p_elbow = 0;
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;
153 void dump_matrix(void)
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);
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)
169 int32_t a00, a10, a11, a01;
170 uint16_t b00, b10, b11, b01;
172 int32_t poids1, poids2;
173 int32_t atmp1, atmp2, btmp1, btmp2;
175 if (d*d + h*h >(ARM_L*2)*(ARM_L*2))
179 j_o = (h+ARM_H) / DIM_COEF;
181 if (i_o >= DIM_D || j_o >= DIM_H || i_o < 0 || j_o < 0)
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;
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;
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;
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;
203 poids1 = d - DIM_COEF * i_o;
204 poids2 = DIM_COEF * (i_o+1) - d;
206 atmp1 = (poids1*a10 + poids2*a00)/DIM_COEF;
207 btmp1 = (poids1*b10 + poids2*b00)/DIM_COEF;
209 atmp2 = (poids1*a11 + poids2*a01)/DIM_COEF;
210 btmp2 = (poids1*b11 + poids2*b01)/DIM_COEF;
212 poids1 = (h+ARM_H) - DIM_COEF*j_o;
213 poids2 = DIM_COEF*(j_o+1) - (h+ARM_H);
215 *d_o = (poids1*atmp2 + poids2*atmp1) / DIM_COEF;
216 *h_o = (poids1*btmp2 + poids2*btmp1) / DIM_COEF;
222 int8_t arm_get_coord(int16_t d,int16_t h, int32_t *d_o, int32_t *h_o)
227 ret = coord2ij(d,h, d_o,h_o);
231 ret = cart2angle(h, d, &a, &b);
235 angle_rad2robot_r(a, b, &a, &b);
242 void test_arm_pos(void)
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);
253 printf("%7ld %4ld ", as, ae);
259 #define ARM_PERIOD 50000
261 #define ARM_MAX_DIST 80
262 #define ARM_MAX_E 10000
263 #define ARM_MAX_S 30000
266 //100000 us for max shoulder / max elbow
268 #define ARM_PERIOD 50000L
270 #define ARM_MAX_DIST 40L
272 //#define ARM_MAX_E ((2331L*ARM_PERIOD)/1000000L)
274 // 800step/CS => 160step/ms
275 #define ARM_MAX_S ((160L*ARM_PERIOD)/1000L)
277 // 4000step/CS => 800step/ms
278 //#define ARM_MAX_S ((800L*ARM_PERIOD)/1000L)
283 #define ARM_MAX_E (((1500L*ARM_PERIOD)/1000000L))
284 #define ARM_MAX_S (((1500L*ARM_PERIOD)/1000L))
292 <0 => unreachable pos
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,
299 int32_t *s_quad, int32_t *e_quad)
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;
308 diff_h = fin_h-*arm_h;
309 diff_d = fin_d-*arm_d;
311 //printf("1 dh %ld dd %ld\r\n", diff_h, diff_d);
313 l = sqrt(diff_h*diff_h + diff_d*diff_d);
316 diff_h = diff_h*ARM_MAX_DIST/l;
317 diff_d = diff_d*ARM_MAX_DIST/l;
320 //printf("2 l: %ld dh %ld dd %ld\r\n", l, diff_h, diff_d);
323 fin_h = *arm_h+diff_h;
324 fin_d = *arm_d+diff_d;
326 //printf("2 fh %ld fd %ld\r\n", fin_h, fin_d);
328 /* calc for current angle */
329 ret = cart2angle(*arm_h, *arm_d, &as_cur_rad, &ae_cur_rad);
333 /* calc for next step */
334 ret = cart2angle(fin_h, fin_d, as_fin_rad, ae_fin_rad);
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);
344 arm_pos->angle_rad2robot(as_cur_rad, ae_cur_rad,
346 arm_pos->angle_rad2robot(*as_fin_rad, *ae_fin_rad,
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);
354 as_diff = as_fin-as_cur;
355 ae_diff = ae_fin-ae_cur;
357 //printf("7 asdiff %ld aediff %ld \r\n", as_diff, ae_diff);
363 /* XXX we are at pos, set default speed */
367 if (!as_diff && !ae_diff){
368 /* printf("reaching end\r\n"); */
377 /* test if one activator is in position */
379 //printf("as reached\r\n");
380 ae_coef = (double)ARM_MAX_E/(double)ae_diff;
381 *next_time = ARM_PERIOD*ABS(ae_coef);
383 *e_quad = ABS(ae_coef)*ABS(ae_diff);
386 *ae = ae_cur+ae_diff;
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);
396 *s_quad = ABS(as_coef)*ABS(as_diff);
398 *as = as_cur+as_diff;
403 as_coef = (double)ARM_MAX_S/(double)as_diff;
404 ae_coef = (double)ARM_MAX_E/(double)ae_diff;
406 //printf("asc %f aec %f\r\n", as_coef, ae_coef);
408 *as = as_cur+as_diff;
409 *ae = ae_cur+ae_diff;
412 if (ABS(as_coef)>=ABS(ae_coef)){
413 /* elbow is limitating */
415 //printf("e limit %ld %ld \r\n", as_diff, ae_diff);
417 *next_time = ARM_PERIOD/ABS(ae_coef);
418 //*next_time = ARM_PERIOD*ae_diff/ARM_MAX_E;
421 *s_quad = ABS(ae_coef)*ABS(as_diff);
422 *e_quad = ABS(ae_coef)*ABS(ae_diff);
427 /* shoulder is limitating */
429 //printf("s limit %ld %ld \r\n", as_diff, ae_diff);
431 *next_time = ARM_PERIOD/ABS(as_coef);
433 *s_quad = ABS(as_coef)*ABS(as_diff);
434 *e_quad = ABS(as_coef)*ABS(ae_diff);
440 *s_quad = (*s_quad*CS_PERIOD)/ARM_PERIOD;
441 *e_quad = (0x3ff*(*e_quad))/ARM_MAX_E;
443 //printf("max e %ld\r\n", *e_quad);
451 //printf("8 sq %ld eq %ld \r\n", *s_quad, *e_quad);
465 ret = cart2angle(100, 100, &a, &b);
466 printf("ret %d %f %f\n", ret, a*180/M_PI, b*180/M_PI);
474 ret = coord2ij(px,py, &as, &ae);
475 printf("ret %d %ld %ld\n", ret, as, ae);
478 ret = cart2angle(py, px, &a, &b);
479 printf("ret %d %f %f\n", ret, a*180/M_PI, b*180/M_PI);
481 ret = arm_get_coord(px,py, &as, &ae);
482 printf("ret %d %ld %ld\n", ret, as, ae);