X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmechboard%2Farm_xy.c;fp=projects%2Fmicrob2010%2Fmechboard%2Farm_xy.c;h=2690844fa7caa56a90135e6c452e44d3c2c52de5;hp=0000000000000000000000000000000000000000;hb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae;hpb=9d2d9100592e18fed985730298215884127fc568 diff --git a/projects/microb2010/mechboard/arm_xy.c b/projects/microb2010/mechboard/arm_xy.c new file mode 100644 index 0000000..2690844 --- /dev/null +++ b/projects/microb2010/mechboard/arm_xy.c @@ -0,0 +1,809 @@ +/* + * Copyright Droids Corporation (2009) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: arm_xy.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $ + * + * Fabrice DESCLAUX + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "main.h" +#include "cmdline.h" +#include "arm_xy.h" +#include "ax12_user.h" + +#define ARM_DEBUG(args...) DEBUG(E_USER_ARM, args) +#define ARM_NOTICE(args...) NOTICE(E_USER_ARM, args) +#define ARM_ERROR(args...) ERROR(E_USER_ARM, args) + +#define DEG(x) (((double)(x)) * (180.0 / M_PI)) +#define RAD(x) (((double)(x)) * (M_PI / 180.0)) +#define M_2PI (2*M_PI) + +/* physical location/dimensions of arm */ +#define ARM_S_LEN 124. +#define ARM_E_LEN 130. + +/* timeout after 1 second if position is not reached */ +#define ARM_GLOBAL_TIMEOUT 1000000L + +/* timeout 100ms after position is reached if not in window */ +#define ARM_WINDOW_TIMEOUT 200000L + +/* default (template) period, but real one is variable */ +#define ARM_PERIOD 50000L +#define ARM_MAX_DIST 40L + +/* we pos reached, check arm in window every period */ +#define ARM_SURVEY_PERIOD 25000UL /* in us */ + +/* number of steps/s */ +#define ARM_AX12_MAX_SPEED (800L) + +/* Maximum number of steps in one ARM_PERIOD */ +#define ARM_MAX_E (((ARM_AX12_MAX_SPEED*ARM_PERIOD)/1000000L)) +/* 4000 steps/CS => 800step/ms */ +#define ARM_MAX_S ((800L*ARM_PERIOD)/1000L) + + +/* window limits in ax12/cs unit */ +#define ARM_SHOULDER_WINDOW_POS 250 +#define ARM_ELBOW_WINDOW_POS 8 +#define ARM_ELBOW_WINDOW_SPEED 100 +#define ARM_WRIST_WINDOW_POS 8 +#define ARM_WRIST_WINDOW_SPEED 100 + +/* default and max speeds */ +#define SHOULDER_DEFAULT_SPEED 800 +#define ELBOW_DEFAULT_SPEED 0x3ff +#define SHOULDER_MAX_SPEED 10000 +#define ELBOW_MAX_SPEED 0x3ff + +/* window status flags */ +#define SHOULDER_NOT_IN_WIN 1 +#define ELBOW_NOT_IN_WIN 2 +#define WRIST_NOT_IN_WIN 4 + +static void wrist_angle_deg2robot_l(double wrist_deg, double *wrist_out); +static void angle_rad2robot_l(double shoulder_rad, double elbow_rad, + double *shoulder_robot, double *elbow_robot); +static void angle_robot2rad_l(double shoulder_robot, double elbow_robot, + double *shoulder_rad, double *elbow_rad); +static void wrist_angle_deg2robot_r(double wrist_deg, double *wrist_out); +static void angle_rad2robot_r(double shoulder_rad, double elbow_rad, + double *shoulder_robot, double *elbow_robot); +static void angle_robot2rad_r(double shoulder_robot, double elbow_robot, + double *shoulder_rad, double *elbow_rad); + +static void arm_schedule_event(struct arm *arm, uint32_t time); + +struct arm left_arm = { + .config = { + .wrist_angle_deg2robot = wrist_angle_deg2robot_l, + .angle_rad2robot = angle_rad2robot_l, + .angle_robot2rad = angle_robot2rad_l, + .elbow_ax12 = L_ELBOW_AX12, + .wrist_ax12 = L_WRIST_AX12, + }, +}; + +struct arm right_arm = { + .config = { + .wrist_angle_deg2robot = wrist_angle_deg2robot_r, + .angle_rad2robot = angle_rad2robot_r, + .angle_robot2rad = angle_robot2rad_r, + .elbow_ax12 = R_ELBOW_AX12, + .wrist_ax12 = R_WRIST_AX12, + }, +}; + +/* process shoulder + elbow angles from height and distance */ +int8_t cart2angle(int32_t h_int, int32_t d_int, double *alpha, double *beta) +{ + double h, d, l; + double elbow, shoulder; + + d = d_int; + h = h_int; + l = sqrt(d*d + h*h); + if (l > (ARM_S_LEN + ARM_E_LEN)) + return -1; + + elbow = -acos((d*d + h*h - ARM_E_LEN*ARM_E_LEN - + ARM_S_LEN*ARM_S_LEN) / (2*ARM_S_LEN*ARM_E_LEN)); + shoulder = atan2(h,d) - atan2(ARM_E_LEN*sin(elbow), + ARM_S_LEN+ARM_E_LEN*cos(elbow)); + + *alpha = shoulder; + *beta = elbow; + + return 0; +} + + +/* process height and distance from shoulder + elbow angles */ +void angle2cart(double alpha, double beta, int32_t *h, int32_t *d) +{ + double tmp_a; + int32_t tmp_h, tmp_d; + + tmp_h = ARM_S_LEN * sin(alpha); + tmp_d = ARM_S_LEN * cos(alpha); + + tmp_a = alpha+beta; + *h = tmp_h + ARM_E_LEN * sin(tmp_a); + *d = tmp_d + ARM_E_LEN * cos(tmp_a); +} + +/*** left arm */ + +#define ARM_LEFT_S_OFFSET -1150. +#define ARM_LEFT_E_OFFSET 476. +#define ARM_LEFT_W_OFFSET 90. + +static void wrist_angle_deg2robot_l(double wrist_deg, double *wrist_out) +{ + *wrist_out = -wrist_deg * 3.41 + ARM_LEFT_W_OFFSET; +} + +/* convert an angle in radian into a robot-specific unit + * for shoulder and elbow for LEFT ARM*/ +static void angle_rad2robot_l(double shoulder_rad, double elbow_rad, + double *shoulder_robot, double *elbow_robot) +{ + *shoulder_robot = shoulder_rad * 4 * 66 * 512. / (2*M_PI) + ARM_LEFT_S_OFFSET; + *elbow_robot = -elbow_rad * 3.41 * 360. / (2*M_PI) + ARM_LEFT_E_OFFSET; +} + +/* convert a robot-specific unit into an angle in radian + * for shoulder and elbow for LEFT ARM */ +static void angle_robot2rad_l(double shoulder_robot, double elbow_robot, + double *shoulder_rad, double *elbow_rad) +{ + *shoulder_rad = ((shoulder_robot - ARM_LEFT_S_OFFSET) * (2*M_PI))/(4 * 66 * 512.); + *elbow_rad = -((elbow_robot - ARM_LEFT_E_OFFSET) * (2*M_PI))/(3.41 * 360.); +} + +/*** right arm */ + +#define ARM_RIGHT_S_OFFSET 1150. +#define ARM_RIGHT_E_OFFSET 673. +#define ARM_RIGHT_W_OFFSET 935. + +static void wrist_angle_deg2robot_r(double wrist_deg, double *wrist_out) +{ + *wrist_out = wrist_deg * 3.41 + ARM_RIGHT_W_OFFSET; +} + +/* convert an angle in radian into a robot-specific unit + * for shoulder and elbow */ +static void angle_rad2robot_r(double shoulder_rad, double elbow_rad, + double *shoulder_robot, double *elbow_robot) +{ + *shoulder_robot = -shoulder_rad * 4 * 66 * 512. / (2*M_PI) + ARM_RIGHT_S_OFFSET; + *elbow_robot = elbow_rad * 3.41 * 360. / (2*M_PI) + ARM_RIGHT_E_OFFSET; +} + +/* convert a robot-specific unit into an angle in radian + * for shoulder and elbow */ +static void angle_robot2rad_r(double shoulder_robot, double elbow_robot, + double *shoulder_rad, double *elbow_rad) +{ + *shoulder_rad = -((shoulder_robot - ARM_RIGHT_S_OFFSET) * (2*M_PI))/(4 * 66 * 512.); + *elbow_rad = ((elbow_robot - ARM_RIGHT_E_OFFSET) * (2*M_PI))/(3.41 * 360.); +} + + +/* + * Fill the arm_status structure according to request. + * + * return: + * 0 => success + * < 0 => error + */ +static int8_t arm_do_step(struct arm *arm) +{ + const struct arm_config *conf = &arm->config; + const struct arm_request *req = &arm->req; + struct arm_status *status = &arm->status; + + int8_t ret; + int32_t diff_h, diff_d; /* position delta in steps */ + int32_t next_h, next_d; /* next position in steps */ + int32_t l; /* distance between cur pos and next pos */ + + double as_cur_rad, ae_cur_rad; /* current angle in rad */ + double as_next_rad, ae_next_rad; /* next angle in rad */ + double as_cur, ae_cur; /* current angle in angle_steps */ + double as_next, ae_next; /* next angle in angle_steps */ + + int32_t as_diff, ae_diff; /* angle delta in angle_steps */ + int32_t s_speed, e_speed; /* elbow/shoulder speed in angle_steps */ + + double as_coef, ae_coef; + + /* process diff between final request and current pos */ + diff_h = req->h_mm - status->h_mm; + diff_d = req->d_mm - status->d_mm; + ARM_NOTICE("goal:d=%ld,h=%ld cur:d=%ld,h=%ld diff:d=%ld,h=%ld", + req->d_mm, req->h_mm, status->d_mm, status->h_mm, + diff_d, diff_h); + + /* if distance to next point is too large, saturate it */ + l = sqrt(diff_h*diff_h + diff_d*diff_d); + if (l > ARM_MAX_DIST) { + diff_h = diff_h * ARM_MAX_DIST / l; + diff_d = diff_d * ARM_MAX_DIST / l; + } + ARM_NOTICE("l=%ld ; after max dist: diff:d=%ld,h=%ld", l, diff_d, diff_h); + + /* process next position */ + next_h = status->h_mm + diff_h; + next_d = status->d_mm + diff_d; + ARM_DEBUG("next:d=%ld,h=%ld", next_d, next_h); + + /* calculate the current angle of arm in radian */ + ret = cart2angle(status->h_mm, status->d_mm, &as_cur_rad, &ae_cur_rad); + if (ret) + return ret; + ARM_DEBUG("as_cur_rad=%f ae_cur_rad=%f", as_cur_rad, ae_cur_rad); + + /* calculate the next angle of arm in radian */ + ret = cart2angle(next_h, next_d, &as_next_rad, &ae_next_rad); + if (ret) + return ret; + ARM_DEBUG("as_next_rad=%f ae_next_rad=%f", as_next_rad, ae_next_rad); + + /* convert radian in angle_steps */ + conf->angle_rad2robot(as_cur_rad, ae_cur_rad, + &as_cur, &ae_cur); + ARM_DEBUG("as_cur=%f ae_cur=%f", as_cur, ae_cur); + conf->angle_rad2robot(as_next_rad, ae_next_rad, + &as_next, &ae_next); + ARM_DEBUG("as_next=%f ae_next=%f", as_next, ae_next); + + /* process angle delta in angle_steps */ + as_diff = as_next - as_cur; + ae_diff = ae_next - ae_cur; + ARM_DEBUG("as_diff=%ld ae_diff=%ld", as_diff, ae_diff); + + /* update position status */ + status->h_mm = next_h; + status->d_mm = next_d; + status->shoulder_angle_steps = as_next; + status->elbow_angle_steps = ae_next; + status->shoulder_angle_rad = as_next_rad; + status->elbow_angle_rad = ae_next_rad; + + /* we reached destination, nothing to do */ + if (as_diff == 0 && ae_diff == 0) { + status->shoulder_speed = SHOULDER_DEFAULT_SPEED; + status->elbow_speed = ELBOW_DEFAULT_SPEED; + status->next_update_time = 0; + ARM_NOTICE("reaching end"); + return 0; + } + + /* test if one actuator is already in position */ + if (as_diff == 0) { + ARM_DEBUG("shoulder reached destination"); + ae_coef = (double)ARM_MAX_E / (double)ae_diff; + status->next_update_time = ARM_PERIOD * ABS(ae_coef); + e_speed = ABS(ae_coef) * ABS(ae_diff); + s_speed = ARM_MAX_S; + } + else if (ae_diff == 0) { + ARM_DEBUG("elbow reached destination"); + as_coef = (double)ARM_MAX_S / (double)as_diff; + status->next_update_time = ARM_PERIOD / ABS(as_coef); + e_speed = ARM_MAX_E; + s_speed = ABS(as_coef) * ABS(as_diff); + } + + else { + as_coef = (double)ARM_MAX_S / (double)as_diff; + ae_coef = (double)ARM_MAX_E / (double)ae_diff; + + ARM_DEBUG("as_coef=%f ae_coef=%f", as_coef, ae_coef); + + /* if elbow is limitating */ + if (ABS(as_coef) >= ABS(ae_coef)) { + ARM_DEBUG("elbow limit"); + status->next_update_time = ARM_PERIOD / ABS(ae_coef); + s_speed = ABS(ae_coef) * ABS(as_diff); + e_speed = ABS(ae_coef) * ABS(ae_diff); + } + /* else, shoulder is limitating */ + else { + ARM_DEBUG("shoulder limit"); + status->next_update_time = ARM_PERIOD / ABS(as_coef); + s_speed = ABS(as_coef) * ABS(as_diff); + e_speed = ABS(as_coef) * ABS(ae_diff); + } + } + + ARM_NOTICE("next update: %ld", status->next_update_time); + + /* convert speed in specific unit */ + status->shoulder_speed = (s_speed * CS_PERIOD) / ARM_PERIOD; + status->elbow_speed = (e_speed * 0x3ff) / ARM_MAX_E; + + ARM_DEBUG("speeds: s=%ld e=%ld", status->shoulder_speed, status->elbow_speed); + + /* avoid limits */ + if (status->shoulder_speed == 0) + status->shoulder_speed = 1; + if (status->elbow_speed == 0) + status->elbow_speed = 1; + if (status->shoulder_speed >= SHOULDER_MAX_SPEED) + status->shoulder_speed = SHOULDER_MAX_SPEED; + if (status->elbow_speed >= ELBOW_MAX_SPEED) + status->elbow_speed = ELBOW_MAX_SPEED; + + ARM_DEBUG("speeds (sat): s=%ld e=%ld", status->shoulder_speed, status->elbow_speed); + + return 0; +} + +static void arm_delete_event(struct arm *arm) +{ + if (arm->status.event == -1) + return; + ARM_DEBUG("Delete arm event"); + scheduler_del_event(arm->status.event); + arm->status.event = -1; +} + +/* write values to ax12 + cs */ +static void arm_apply(struct arm *arm) +{ + struct cs_block *csb = arm->config.csb; + const struct arm_status *st = &arm->status; + + ARM_DEBUG("arm_apply"); + + if (arm->config.simulate) + return; + + /* set speed and pos of shoulder */ + quadramp_set_1st_order_vars(&csb->qr, + st->shoulder_speed, + st->shoulder_speed); + cs_set_consign(&csb->cs, st->shoulder_angle_steps); + + /* set speed and position of elbow */ + ax12_user_write_int(&gen.ax12, arm->config.elbow_ax12, + AA_MOVING_SPEED_L, st->elbow_speed); + ax12_user_write_int(&gen.ax12, arm->config.elbow_ax12, + AA_GOAL_POSITION_L, st->elbow_angle_steps); +} + +/* return true if one of the mask condition is true */ +uint8_t arm_test_traj_end(struct arm *arm, uint8_t mask) +{ + if ((mask & ARM_TRAJ_END) && (arm->status.state & ARM_FLAG_IN_WINDOW)) + return ARM_TRAJ_END; + + if ((mask & ARM_TRAJ_NEAR) && (arm->status.state & ARM_FLAG_LAST_STEP)) + return ARM_TRAJ_NEAR; + + if ((mask & ARM_TRAJ_TIMEOUT) && (arm->status.state & ARM_FLAG_TIMEOUT)) + return ARM_TRAJ_TIMEOUT; + + if ((mask & ARM_TRAJ_ERROR) && (arm->status.state & ARM_FLAG_ERROR)) + return ARM_TRAJ_ERROR; + + return 0; +} + +uint8_t arm_wait_traj_end(struct arm *arm, uint8_t mask) +{ + uint8_t ret; + while(1) { + ret = arm_test_traj_end(arm, mask); + if (ret) + return ret; + } +} + +/* return true if one of the mask condition is true */ +uint8_t arm_in_window(struct arm *arm, uint8_t *status) +{ + int8_t err; +/* uint16_t spd; */ + int16_t pos; + int32_t cs_err; + + *status = 0; + + if (arm->config.simulate) + return 1; + + /* shoulder, just check position */ + cs_err = cs_get_error(&arm->config.csb->cs); + if (ABS(cs_err) > ARM_SHOULDER_WINDOW_POS) + *status |= SHOULDER_NOT_IN_WIN; + +#if 0 + /* check elbow speed */ + err = ax12_user_read_int(&gen.ax12, arm->config.elbow_ax12, + AA_PRESENT_SPEED_L, &spd); + if (err) + goto fail; + if (spd > ARM_ELBOW_WINDOW_SPEED) + return 0; + + /* check wrist speed */ + err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12, + AA_PRESENT_SPEED_L, &spd); + if (err) + goto fail; + if (spd > ARM_WRIST_WINDOW_SPEED) + return 0; +#endif + /* check elbow pos */ + err = ax12_user_read_int(&gen.ax12, arm->config.elbow_ax12, + AA_PRESENT_POSITION_L, (uint16_t *)&pos); + if (err) + goto fail; + if (ABS(arm->status.elbow_angle_steps - pos) > ARM_ELBOW_WINDOW_POS) + *status |= ELBOW_NOT_IN_WIN; + + /* check wrist pos */ + err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12, + AA_PRESENT_POSITION_L, (uint16_t *)&pos); + if (err) + goto fail; + if (ABS(arm->status.wrist_angle_steps - pos) > ARM_WRIST_WINDOW_POS) + *status |= WRIST_NOT_IN_WIN; + + if (*status) + return 0; + + ARM_NOTICE("arm is in window (%ld us after reach pos)", + time_get_us2() - arm->status.pos_reached_time); + return 1; /* ok, we are in window */ + + fail: + return 0; +} + +/* process wrist pos and apply it. it's done only once. */ +static int8_t arm_set_wrist(struct arm *arm) +{ + int8_t err; + int32_t as_deg, ae_deg, aw_deg; + uint16_t wrist_out_u16; + double wrist_out, as_rad, ae_rad; + int16_t pos; + uint32_t diff_time; + + /* calculate the destination angle of arm in radian */ + err = cart2angle(arm->req.h_mm, arm->req.d_mm, + &as_rad, &ae_rad); + if (err) + return -1; + + /* calc angle destination */ + as_deg = DEG(as_rad); + ae_deg = DEG(ae_rad); + ARM_DEBUG("as_dest_deg=%d ae_dest_deg=%d", as_deg, ae_deg); + aw_deg = as_deg + ae_deg - arm->req.w_deg; + arm->config.wrist_angle_deg2robot(aw_deg, &wrist_out); + wrist_out_u16 = wrist_out; + + ARM_DEBUG("set wrist to %ld degrees (%d steps)", aw_deg, + wrist_out_u16); + + /* process the theorical reach time for the wrist */ + if (arm->config.simulate) { + pos = arm->status.wrist_angle_steps; + } + else { + err = ax12_user_read_int(&gen.ax12, arm->config.wrist_ax12, + AA_PRESENT_POSITION_L, (uint16_t *)&pos); + if (err) + pos = arm->status.wrist_angle_steps; + } + /* 600 is the number of steps/s */ + diff_time = (ABS((int16_t)wrist_out_u16 - pos) * 1000000L) / 600; + arm->status.wrist_reach_time = arm->status.start_time + diff_time; + ARM_DEBUG("wrist reach time is %ld (diff=%ld)", + arm->status.wrist_reach_time, diff_time); + + /* update current position to destination */ + arm->status.wrist_angle_steps = wrist_out_u16; + + if (arm->config.simulate) + return 0; + + /* send it to ax12 */ + ax12_user_write_int(&gen.ax12, arm->config.wrist_ax12, + AA_GOAL_POSITION_L, wrist_out_u16); + return 0; +} + +/* event callback */ +static void arm_do_xy_cb(struct arm *arm) +{ + uint8_t win_status; + + arm->status.event = -1; + + /* if consign haven't reach destination */ + if ((arm->status.state & ARM_FLAG_LAST_STEP) == 0) { + if (arm_do_step(arm)) + arm->status.state |= ARM_FLAG_ERROR; + + /* it's the first call for the traj */ + if (arm->status.state == ARM_STATE_INIT) { + arm->status.state |= ARM_FLAG_MOVING; + if (arm_set_wrist(arm)) + arm->status.state |= ARM_FLAG_ERROR; + } + + /* we have more steps to do */ + if (arm->status.next_update_time == 0) { + arm->status.state &= ~ARM_FLAG_MOVING; + arm->status.state |= ARM_FLAG_LAST_STEP; + arm->status.pos_reached_time = time_get_us2(); + } + arm_apply(arm); + } + /* last step is reached, we can check that arm is in window */ + else if ((arm->status.state & ARM_FLAG_IN_WINDOW) == 0) { + if (arm_in_window(arm, &win_status)) + arm->status.state |= ARM_FLAG_IN_WINDOW; + + /* check for window arm timeout */ + else { + microseconds t; + int32_t diff1, diff2; + t = time_get_us2(); + diff1 = t - arm->status.pos_reached_time; + diff2 = t - arm->status.wrist_reach_time; + if (diff1 > ARM_WINDOW_TIMEOUT && + diff2 > ARM_WINDOW_TIMEOUT) { + ARM_NOTICE("win timeout at %ld win_status=%x", + t, win_status); + arm->status.state |= ARM_FLAG_TIMEOUT; + } + } + } + + /* check for global arm timeout */ + if ((time_get_us2() - arm->status.start_time) > ARM_GLOBAL_TIMEOUT) { + ARM_NOTICE("global timeout at %ld", time_get_us2()); + arm->status.state |= ARM_FLAG_TIMEOUT; + } + + /* reload event if needed */ + if ((arm->status.state & ARM_FLAG_FINISHED) == ARM_FLAG_FINISHED) { + ARM_NOTICE("arm traj finished"); + return; /* no more event, position reached */ + } + if (arm->status.state & (ARM_FLAG_ERROR|ARM_FLAG_TIMEOUT)) { + ARM_NOTICE("error or timeout"); + return; /* no more event */ + } + else if (arm->status.state & ARM_FLAG_LAST_STEP) { + /* theorical position is reached, but reload an event + * for position survey (window), every 25ms */ + arm_schedule_event(arm, ARM_SURVEY_PERIOD); + } + else { + /* reload event for next position step */ + arm_schedule_event(arm, arm->status.next_update_time); + } +} + +/* schedule a single event for this arm */ +static void arm_schedule_event(struct arm *arm, uint32_t time) +{ + uint8_t flags; + int8_t ret; + + arm_delete_event(arm); + if (time < SCHEDULER_UNIT) + time = SCHEDULER_UNIT; + IRQ_LOCK(flags); + ret = scheduler_add_event(SCHEDULER_SINGLE, + (void *)arm_do_xy_cb, + arm, time/SCHEDULER_UNIT, ARM_PRIO); + if (ret == -1) { + IRQ_UNLOCK(flags); + ARM_ERROR("Cannot load arm event"); + return; + } + arm->status.event = ret; + IRQ_UNLOCK(flags); +} + +int8_t arm_do_xy(struct arm *arm, int16_t d_mm, int16_t h_mm, int16_t w_deg) +{ + ARM_NOTICE("arm_do_xy: d_mm=%d h_mm=%d w_deg=%d", d_mm, h_mm, w_deg); + + /* remove previous event if any */ + arm_delete_event(arm); + + /* init mandatory params */ + arm->req.d_mm = d_mm; + arm->req.h_mm = h_mm; + arm->req.w_deg = w_deg; + arm->status.start_time = time_get_us2(); + arm->status.state = ARM_STATE_INIT; + + /* all the job will be done asynchronously now */ + arm_schedule_event(arm, 0); + return 0; +} + +void arm_dump(struct arm *arm) +{ + printf_P(PSTR("config: simulate=%d\r\n"), + arm->config.simulate); + printf_P(PSTR("req: d_mm=%ld h_mm=%ld w_deg=%ld\r\n"), + arm->req.d_mm, arm->req.h_mm, arm->req.w_deg); + printf_P(PSTR("status: ")); + if (arm->status.state == ARM_STATE_INIT) + printf_P(PSTR("ARM_STATE_INIT ")); + if (arm->status.state & ARM_FLAG_MOVING) + printf_P(PSTR("ARM_FLAG_MOVING ")); + if (arm->status.state & ARM_FLAG_LAST_STEP) + printf_P(PSTR("ARM_FLAG_LAST_STEP ")); + if (arm->status.state & ARM_FLAG_IN_WINDOW) + printf_P(PSTR("ARM_FLAG_IN_WINDOW ")); + if (arm->status.state & ARM_FLAG_ERROR) + printf_P(PSTR("ARM_FLAG_ERROR ")); + if (arm->status.state & ARM_FLAG_TIMEOUT) + printf_P(PSTR("ARM_FLAG_TIMEOUT ")); + printf_P(PSTR("\r\n")); + + printf_P(PSTR(" d_mm=%ld h_mm=%ld goal_w_steps=%d\r\n"), + arm->status.d_mm, arm->status.h_mm, arm->status.wrist_angle_steps); + printf_P(PSTR(" cur_shl_steps=%ld cur_elb_steps=%ld\r\n"), + arm->status.shoulder_angle_steps, arm->status.elbow_angle_steps); + printf_P(PSTR(" cur_shl_rad=%f cur_elb_rad=%f\r\n"), + arm->status.shoulder_angle_rad, arm->status.elbow_angle_rad); + printf_P(PSTR(" cur_shl_deg=%f cur_elb_deg=%f\r\n"), + DEG(arm->status.shoulder_angle_rad), DEG(arm->status.elbow_angle_rad)); + printf_P(PSTR(" event=%d next_update_time=%ld\r\n"), + arm->status.event, arm->status.next_update_time); + printf_P(PSTR(" start_time=%ld pos_reached_time=%ld wrist_reach_time=%ld\r\n"), + arm->status.start_time, arm->status.pos_reached_time, + arm->status.wrist_reach_time); +} + +#define CALIB_ANGLE (RAD(-93.)) + +void arm_calibrate(void) +{ + double shoulder, elbow; + + pwm_ng_set(LEFT_ARM_PWM, 500); + pwm_ng_set(RIGHT_ARM_PWM, -500); + wait_ms(200); + + pwm_ng_set(LEFT_ARM_PWM, 300); + pwm_ng_set(RIGHT_ARM_PWM, -300); + wait_ms(700); + + printf_P(PSTR("Init arm, please wait...")); + ax12_user_write_int(&gen.ax12, AX12_BROADCAST_ID, AA_TORQUE_ENABLE, 0x1); + ax12_user_write_int(&gen.ax12, AX12_BROADCAST_ID, AA_ALARM_SHUTDOWN, 0x04); + + angle_rad2robot_r(0, CALIB_ANGLE, &shoulder, &elbow); + ax12_user_write_int(&gen.ax12, R_ELBOW_AX12, AA_GOAL_POSITION_L, elbow); + ax12_user_write_int(&gen.ax12, R_WRIST_AX12, AA_GOAL_POSITION_L, 628); + + angle_rad2robot_l(0, CALIB_ANGLE, &shoulder, &elbow); + ax12_user_write_int(&gen.ax12, L_ELBOW_AX12, AA_GOAL_POSITION_L, elbow); + ax12_user_write_int(&gen.ax12, L_WRIST_AX12, AA_GOAL_POSITION_L, 394); + pwm_ng_set(LEFT_ARM_PWM, -100); + pwm_ng_set(RIGHT_ARM_PWM, 100); + + wait_ms(2000); + + cs_set_consign(&mechboard.left_arm.cs, 0); + cs_set_consign(&mechboard.right_arm.cs, 0); + encoders_spi_set_value(LEFT_ARM_ENCODER, 0); + encoders_spi_set_value(RIGHT_ARM_ENCODER, 0); + + printf_P(PSTR("ok\r\n")); +} + +/* init arm config */ +void arm_init(void) +{ + uint32_t shoulder_robot; + uint16_t elbow_robot, wrist_robot; + double shoulder_rad, elbow_rad; + int32_t h, d; + uint8_t err = 0; + + memset(&left_arm.status, 0, sizeof(left_arm.status)); + memset(&right_arm.status, 0, sizeof(right_arm.status)); + left_arm.status.event = -1; + right_arm.status.event = -1; + + arm_calibrate(); + + /* set des slopes XXX */ + + /* set maximum moving speeds */ + err |= ax12_user_write_int(&gen.ax12, L_ELBOW_AX12, AA_MOVING_SPEED_L, 0x3ff); + err |= ax12_user_write_int(&gen.ax12, L_WRIST_AX12, AA_MOVING_SPEED_L, 0x3ff); + err |= ax12_user_write_int(&gen.ax12, R_ELBOW_AX12, AA_MOVING_SPEED_L, 0x3ff); + err |= ax12_user_write_int(&gen.ax12, R_WRIST_AX12, AA_MOVING_SPEED_L, 0x3ff); + + /* left arm init */ + shoulder_robot = encoders_spi_get_value(LEFT_ARM_ENCODER); + err |= ax12_user_read_int(&gen.ax12, L_ELBOW_AX12, AA_PRESENT_POSITION_L, &elbow_robot); + err |= ax12_user_read_int(&gen.ax12, L_WRIST_AX12, AA_PRESENT_POSITION_L, &wrist_robot); + + angle_robot2rad_l(shoulder_robot, elbow_robot, + &shoulder_rad, &elbow_rad); + angle2cart(shoulder_rad, elbow_rad, &h, &d); + printf_P(PSTR("left arm: h:%ld d:%ld w:%d\r\n"), h, d, wrist_robot); + left_arm.status.h_mm = h; + left_arm.status.d_mm = d; + left_arm.status.wrist_angle_steps = wrist_robot; + left_arm.status.state = ARM_FLAG_FINISHED; + left_arm.config.csb = &mechboard.left_arm; + + /* left arm init */ + shoulder_robot = encoders_spi_get_value(RIGHT_ARM_ENCODER); + err |= ax12_user_read_int(&gen.ax12, R_ELBOW_AX12, AA_PRESENT_POSITION_L, &elbow_robot); + err |= ax12_user_read_int(&gen.ax12, R_WRIST_AX12, AA_PRESENT_POSITION_L, &wrist_robot); + + angle_robot2rad_r(shoulder_robot, elbow_robot, + &shoulder_rad, &elbow_rad); + angle2cart(shoulder_rad, elbow_rad, &h, &d); + printf_P(PSTR("right arm: h:%ld d:%ld w:%d\r\n"), h, d, wrist_robot); + right_arm.status.h_mm = h; + right_arm.status.d_mm = d; + right_arm.status.wrist_angle_steps = wrist_robot; + right_arm.status.state = ARM_FLAG_FINISHED; + right_arm.config.csb = &mechboard.right_arm; + + if (err) + ARM_ERROR("ARM INIT ERROR"); +}