projects/microb2009/mechboard/arm_xy.h

   1 /*
   2  *  Copyright Droids Corporation (2009)
   3  *
   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.
   8  *
   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.
  13  *
  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
  17  *
  18  *  Revision : $Id: arm_xy.h,v 1.4 2009-11-08 17:25:00 zer0 Exp $
  19  *
  20  * Fabrice DESCLAUX <serpilliere@droids-corp.org>
  21  * Olivier MATZ <zer0@droids-corp.org>
  22  */
  23
  24 /* all static configuration for an arm */
  25 struct arm_config {
  26         void (*wrist_angle_deg2robot)(double wrist_edg,
  27                                       double *wrist_out);
  28
  29         void (*angle_rad2robot)(double shoulder_deg, double elbow_deg,
  30                                 double *shoulder_out, double *elbow_out);
  31
  32         void (*angle_robot2rad)(double shoulder_robot, double elbow_robot,
  33                                 double *shoulder_rad, double *elbow_rad);
  34
  35         /* ax12 identifiers */
  36         int8_t elbow_ax12;
  37         int8_t wrist_ax12;
  38
  39         /* related control system */
  40         struct cs_block *csb;
  41
  42         /* if true, don't apply to ax12 / cs */
  43         uint8_t simulate;
  44 };
  45
  46 /* request for a final position, in mm */
  47 struct arm_request {
  48         int32_t h_mm;
  49         int32_t d_mm;
  50         int32_t w_deg;
  51 };
  52
  53 /* returned by arm_test_traj_end() */
  54 #define ARM_TRAJ_END         0x01
  55 #define ARM_TRAJ_NEAR        0x02
  56 #define ARM_TRAJ_TIMEOUT     0x04
  57 #define ARM_TRAJ_ERROR       0x08
  58
  59 #define ARM_TRAJ_ALL (ARM_TRAJ_END|ARM_TRAJ_TIMEOUT|ARM_TRAJ_ERROR)
  60 #define ARM_TRAJ_ALL_NEAR (ARM_TRAJ_END|ARM_TRAJ_NEAR|ARM_TRAJ_TIMEOUT|ARM_TRAJ_ERROR)
  61 #define ARM_TRAJ_END_NEAR (ARM_TRAJ_END|ARM_TRAJ_NEAR)
  62
  63 #define ARM_STATE_INIT       0
  64 #define ARM_FLAG_MOVING      0x01 /* arm is currently moving */
  65 #define ARM_FLAG_LAST_STEP   0x02 /* no more step is needed */
  66 #define ARM_FLAG_IN_WINDOW   0x04 /* arm speed and pos are ok */
  67 #define ARM_FLAG_TIMEOUT     0x08 /* too much time too reach pos */
  68 #define ARM_FLAG_ERROR       0x10 /* error */
  69 #define ARM_FLAG_FINISHED    (ARM_FLAG_LAST_STEP | ARM_FLAG_IN_WINDOW)
  70
  71 /* Describes the current position of the arm. Mainly filled by
  72  * arm_do_step(), arm_set_wrist(), arm_do_xy_cb(), ... */
  73 struct arm_status {
  74         /* current position */
  75         int32_t h_mm;
  76         int32_t d_mm;
  77
  78         /* wrist goal position (set once at init) */
  79         int16_t wrist_angle_steps;
  80
  81         /* current angles in steps */
  82         int32_t elbow_angle_steps;
  83         int32_t shoulder_angle_steps;
  84
  85         /* current angles in radian */
  86         double elbow_angle_rad;
  87         double shoulder_angle_rad;
  88
  89         /* time before next update */
  90         uint32_t next_update_time;
  91
  92         /* what speed to be applied, in specific speed unit */
  93         uint32_t shoulder_speed;
  94         uint32_t elbow_speed;
  95
  96         volatile int8_t state; /* see list of flags above */
  97         int8_t event; /* scheduler event, -1 if not running */
  98
  99         microseconds start_time; /* when we started that command */
 100         microseconds wrist_reach_time; /* when the wrist should reach dest */
 101         microseconds pos_reached_time; /* when last step is sent */
 102 };
 103
 104 struct arm {
 105         struct arm_config config;
 106         struct arm_status status;
 107         struct arm_request req;
 108 };
 109
 110 extern struct arm left_arm;
 111 extern struct arm right_arm;
 112
 113 uint8_t arm_test_traj_end(struct arm *arm, uint8_t mask);
 114 uint8_t arm_wait_traj_end(struct arm *arm, uint8_t mask);
 115
 116 /* do a specific move to distance, height. This function _must_ be
 117  * called from a context with a prio < ARM_PRIO to avoid any race
 118  * condition. */
 119 int8_t arm_do_xy(struct arm *arm, int16_t d_mm, int16_t h_mm, int16_t w_deg);
 120
 121 void arm_dump(struct arm *arm);
 122 void arm_calibrate(void);
 123 void arm_init(void);