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=0000000000000000000000000000000000000000;hp=2690844fa7caa56a90135e6c452e44d3c2c52de5;hb=8d6a47e9e21a9a31f4bc12d32fb3d11091a4b305;hpb=821f753c0f88aff895d9feae59c442a6c446f96b diff --git a/projects/microb2010/mechboard/arm_xy.c b/projects/microb2010/mechboard/arm_xy.c deleted file mode 100644 index 2690844..0000000 --- a/projects/microb2010/mechboard/arm_xy.c +++ /dev/null @@ -1,809 +0,0 @@ -/* - * 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"); -}