2 * Copyright Droids Corporation (2009)
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.
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.
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
18 * Revision : $Id: arm_xy.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $
20 * Fabrice DESCLAUX <serpilliere@droids-corp.org>
21 * Olivier MATZ <zer0@droids-corp.org>
28 #include <aversive/wait.h>
29 #include <aversive/pgmspace.h>
30 #include <aversive/error.h>
35 #include <encoders_spi.h>
38 #include <scheduler.h>
43 #include <control_system_manager.h>
44 #include <blocking_detection_manager.h>
51 #include "ax12_user.h"
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)
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)
61 /* physical location/dimensions of arm */
62 #define ARM_S_LEN 124.
63 #define ARM_E_LEN 130.
65 /* timeout after 1 second if position is not reached */
66 #define ARM_GLOBAL_TIMEOUT 1000000L
68 /* timeout 100ms after position is reached if not in window */
69 #define ARM_WINDOW_TIMEOUT 200000L
71 /* default (template) period, but real one is variable */
72 #define ARM_PERIOD 50000L
73 #define ARM_MAX_DIST 40L
75 /* we pos reached, check arm in window every period */
76 #define ARM_SURVEY_PERIOD 25000UL /* in us */
78 /* number of steps/s */
79 #define ARM_AX12_MAX_SPEED (800L)
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)
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
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
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
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);
116 static void arm_schedule_event(struct arm *arm, uint32_t time);
118 struct arm left_arm = {
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,
128 struct arm right_arm = {
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,
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)
142 double elbow, shoulder;
147 if (l > (ARM_S_LEN + ARM_E_LEN))
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));
162 /* process height and distance from shoulder + elbow angles */
163 void angle2cart(double alpha, double beta, int32_t *h, int32_t *d)
166 int32_t tmp_h, tmp_d;
168 tmp_h = ARM_S_LEN * sin(alpha);
169 tmp_d = ARM_S_LEN * cos(alpha);
172 *h = tmp_h + ARM_E_LEN * sin(tmp_a);
173 *d = tmp_d + ARM_E_LEN * cos(tmp_a);
178 #define ARM_LEFT_S_OFFSET -1150.
179 #define ARM_LEFT_E_OFFSET 476.
180 #define ARM_LEFT_W_OFFSET 90.
182 static void wrist_angle_deg2robot_l(double wrist_deg, double *wrist_out)
184 *wrist_out = -wrist_deg * 3.41 + ARM_LEFT_W_OFFSET;
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)
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;
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)
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.);
207 #define ARM_RIGHT_S_OFFSET 1150.
208 #define ARM_RIGHT_E_OFFSET 673.
209 #define ARM_RIGHT_W_OFFSET 935.
211 static void wrist_angle_deg2robot_r(double wrist_deg, double *wrist_out)
213 *wrist_out = wrist_deg * 3.41 + ARM_RIGHT_W_OFFSET;
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)
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;
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)
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.);
236 * Fill the arm_status structure according to request.
242 static int8_t arm_do_step(struct arm *arm)
244 const struct arm_config *conf = &arm->config;
245 const struct arm_request *req = &arm->req;
246 struct arm_status *status = &arm->status;
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 */
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 */
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 */
261 double as_coef, ae_coef;
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,
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;
276 ARM_NOTICE("l=%ld ; after max dist: diff:d=%ld,h=%ld", l, diff_d, diff_h);
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);
283 /* calculate the current angle of arm in radian */
284 ret = cart2angle(status->h_mm, status->d_mm, &as_cur_rad, &ae_cur_rad);
287 ARM_DEBUG("as_cur_rad=%f ae_cur_rad=%f", as_cur_rad, ae_cur_rad);
289 /* calculate the next angle of arm in radian */
290 ret = cart2angle(next_h, next_d, &as_next_rad, &ae_next_rad);
293 ARM_DEBUG("as_next_rad=%f ae_next_rad=%f", as_next_rad, ae_next_rad);
295 /* convert radian in angle_steps */
296 conf->angle_rad2robot(as_cur_rad, ae_cur_rad,
298 ARM_DEBUG("as_cur=%f ae_cur=%f", as_cur, ae_cur);
299 conf->angle_rad2robot(as_next_rad, ae_next_rad,
301 ARM_DEBUG("as_next=%f ae_next=%f", as_next, ae_next);
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);
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;
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");
325 /* test if one actuator is already in position */
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);
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);
338 s_speed = ABS(as_coef) * ABS(as_diff);
342 as_coef = (double)ARM_MAX_S / (double)as_diff;
343 ae_coef = (double)ARM_MAX_E / (double)ae_diff;
345 ARM_DEBUG("as_coef=%f ae_coef=%f", as_coef, ae_coef);
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);
354 /* else, shoulder is limitating */
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);
363 ARM_NOTICE("next update: %ld", status->next_update_time);
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;
369 ARM_DEBUG("speeds: s=%ld e=%ld", status->shoulder_speed, status->elbow_speed);
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;
381 ARM_DEBUG("speeds (sat): s=%ld e=%ld", status->shoulder_speed, status->elbow_speed);
386 static void arm_delete_event(struct arm *arm)
388 if (arm->status.event == -1)
390 ARM_DEBUG("Delete arm event");
391 scheduler_del_event(arm->status.event);
392 arm->status.event = -1;
395 /* write values to ax12 + cs */
396 static void arm_apply(struct arm *arm)
398 struct cs_block *csb = arm->config.csb;
399 const struct arm_status *st = &arm->status;
401 ARM_DEBUG("arm_apply");
403 if (arm->config.simulate)
406 /* set speed and pos of shoulder */
407 quadramp_set_1st_order_vars(&csb->qr,
410 cs_set_consign(&csb->cs, st->shoulder_angle_steps);
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);
419 /* return true if one of the mask condition is true */
420 uint8_t arm_test_traj_end(struct arm *arm, uint8_t mask)
422 if ((mask & ARM_TRAJ_END) && (arm->status.state & ARM_FLAG_IN_WINDOW))
425 if ((mask & ARM_TRAJ_NEAR) && (arm->status.state & ARM_FLAG_LAST_STEP))
426 return ARM_TRAJ_NEAR;
428 if ((mask & ARM_TRAJ_TIMEOUT) && (arm->status.state & ARM_FLAG_TIMEOUT))
429 return ARM_TRAJ_TIMEOUT;
431 if ((mask & ARM_TRAJ_ERROR) && (arm->status.state & ARM_FLAG_ERROR))
432 return ARM_TRAJ_ERROR;
437 uint8_t arm_wait_traj_end(struct arm *arm, uint8_t mask)
441 ret = arm_test_traj_end(arm, mask);
447 /* return true if one of the mask condition is true */
448 uint8_t arm_in_window(struct arm *arm, uint8_t *status)
457 if (arm->config.simulate)
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;
466 /* check elbow speed */
467 err = ax12_user_read_int(&gen.ax12, arm->config.elbow_ax12,
468 AA_PRESENT_SPEED_L, &spd);
471 if (spd > ARM_ELBOW_WINDOW_SPEED)
474 /* check wrist speed */
475 err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12,
476 AA_PRESENT_SPEED_L, &spd);
479 if (spd > ARM_WRIST_WINDOW_SPEED)
482 /* check elbow pos */
483 err = ax12_user_read_int(&gen.ax12, arm->config.elbow_ax12,
484 AA_PRESENT_POSITION_L, (uint16_t *)&pos);
487 if (ABS(arm->status.elbow_angle_steps - pos) > ARM_ELBOW_WINDOW_POS)
488 *status |= ELBOW_NOT_IN_WIN;
490 /* check wrist pos */
491 err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12,
492 AA_PRESENT_POSITION_L, (uint16_t *)&pos);
495 if (ABS(arm->status.wrist_angle_steps - pos) > ARM_WRIST_WINDOW_POS)
496 *status |= WRIST_NOT_IN_WIN;
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 */
509 /* process wrist pos and apply it. it's done only once. */
510 static int8_t arm_set_wrist(struct arm *arm)
513 int32_t as_deg, ae_deg, aw_deg;
514 uint16_t wrist_out_u16;
515 double wrist_out, as_rad, ae_rad;
519 /* calculate the destination angle of arm in radian */
520 err = cart2angle(arm->req.h_mm, arm->req.d_mm,
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;
533 ARM_DEBUG("set wrist to %ld degrees (%d steps)", aw_deg,
536 /* process the theorical reach time for the wrist */
537 if (arm->config.simulate) {
538 pos = arm->status.wrist_angle_steps;
541 err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12,
542 AA_PRESENT_POSITION_L, (uint16_t *)&pos);
544 pos = arm->status.wrist_angle_steps;
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);
552 /* update current position to destination */
553 arm->status.wrist_angle_steps = wrist_out_u16;
555 if (arm->config.simulate)
558 /* send it to ax12 */
559 ax12_user_write_int(&gen.ax12, arm->config.wrist_ax12,
560 AA_GOAL_POSITION_L, wrist_out_u16);
565 static void arm_do_xy_cb(struct arm *arm)
569 arm->status.event = -1;
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;
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;
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();
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;
596 /* check for window arm timeout */
599 int32_t diff1, diff2;
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",
607 arm->status.state |= ARM_FLAG_TIMEOUT;
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;
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 */
623 if (arm->status.state & (ARM_FLAG_ERROR|ARM_FLAG_TIMEOUT)) {
624 ARM_NOTICE("error or timeout");
625 return; /* no more event */
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);
633 /* reload event for next position step */
634 arm_schedule_event(arm, arm->status.next_update_time);
638 /* schedule a single event for this arm */
639 static void arm_schedule_event(struct arm *arm, uint32_t time)
644 arm_delete_event(arm);
645 if (time < SCHEDULER_UNIT)
646 time = SCHEDULER_UNIT;
648 ret = scheduler_add_event(SCHEDULER_SINGLE,
649 (void *)arm_do_xy_cb,
650 arm, time/SCHEDULER_UNIT, ARM_PRIO);
653 ARM_ERROR("Cannot load arm event");
656 arm->status.event = ret;
660 int8_t arm_do_xy(struct arm *arm, int16_t d_mm, int16_t h_mm, int16_t w_deg)
662 ARM_NOTICE("arm_do_xy: d_mm=%d h_mm=%d w_deg=%d", d_mm, h_mm, w_deg);
664 /* remove previous event if any */
665 arm_delete_event(arm);
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;
674 /* all the job will be done asynchronously now */
675 arm_schedule_event(arm, 0);
679 void arm_dump(struct arm *arm)
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"));
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);
715 #define CALIB_ANGLE (RAD(-93.))
717 void arm_calibrate(void)
719 double shoulder, elbow;
721 pwm_ng_set(LEFT_ARM_PWM, 500);
722 pwm_ng_set(RIGHT_ARM_PWM, -500);
725 pwm_ng_set(LEFT_ARM_PWM, 300);
726 pwm_ng_set(RIGHT_ARM_PWM, -300);
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);
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);
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);
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);
750 printf_P(PSTR("ok\r\n"));
753 /* init arm config */
756 uint32_t shoulder_robot;
757 uint16_t elbow_robot, wrist_robot;
758 double shoulder_rad, elbow_rad;
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;
769 /* set des slopes XXX */
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);
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);
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;
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);
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;
808 ARM_ERROR("ARM INIT ERROR");