--- /dev/null
+/*
+ * 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 <serpilliere@droids-corp.org>
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <math.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/wait.h>
+#include <aversive/pgmspace.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <spi.h>
+#include <encoders_spi.h>
+#include <pwm_ng.h>
+#include <timer.h>
+#include <scheduler.h>
+#include <time.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+
+#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");
+}
git.droids-corp.org / aversive.git / blobdiff