projects/microb2010/sensorboard/commands_ax12.c

   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: commands_ax12.c,v 1.1 2009-03-29 18:44:54 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23 #include <stdio.h>
  24 #include <string.h>
  25
  26 #include <aversive/pgmspace.h>
  27 #include <aversive/wait.h>
  28 #include <aversive/error.h>
  29
  30 #include <ax12.h>
  31 #include <uart.h>
  32 #include <pwm_ng.h>
  33 #include <time.h>
  34
  35 #include <pid.h>
  36 #include <quadramp.h>
  37 #include <control_system_manager.h>
  38 #include <blocking_detection_manager.h>
  39
  40 #include <rdline.h>
  41 #include <parse.h>
  42 #include <parse_string.h>
  43 #include <parse_num.h>
  44
  45 #include "main.h"
  46
  47 uint8_t addr_from_string(const char *s)
  48 {
  49         /* 16 bits */
  50         if (!strcmp_P(s, PSTR("cw_angle_limit")))
  51                 return AA_CW_ANGLE_LIMIT_L;
  52         if (!strcmp_P(s, PSTR("ccw_angle_limit")))
  53                 return AA_CCW_ANGLE_LIMIT_L;
  54         if (!strcmp_P(s, PSTR("max_torque")))
  55                 return AA_MAX_TORQUE_L;
  56         if (!strcmp_P(s, PSTR("down_calibration")))
  57                 return AA_DOWN_CALIBRATION_L;
  58         if (!strcmp_P(s, PSTR("up_calibration")))
  59                 return AA_UP_CALIBRATION_L;
  60         if (!strcmp_P(s, PSTR("torque_limit")))
  61                 return AA_TORQUE_LIMIT_L;
  62         if (!strcmp_P(s, PSTR("position")))
  63                 return AA_PRESENT_POSITION_L;
  64         if (!strcmp_P(s, PSTR("speed")))
  65                 return AA_PRESENT_SPEED_L;
  66         if (!strcmp_P(s, PSTR("load")))
  67                 return AA_PRESENT_LOAD_L;
  68         if (!strcmp_P(s, PSTR("moving_speed")))
  69                 return AA_MOVING_SPEED_L;
  70         if (!strcmp_P(s, PSTR("model")))
  71                 return AA_MODEL_NUMBER_L;
  72         if (!strcmp_P(s, PSTR("goal_pos")))
  73                 return AA_GOAL_POSITION_L;
  74         if (!strcmp_P(s, PSTR("punch")))
  75                 return AA_PUNCH_L;
  76
  77         /* 8 bits */
  78         if (!strcmp_P(s, PSTR("firmware")))
  79                 return AA_FIRMWARE;
  80         if (!strcmp_P(s, PSTR("id")))
  81                 return AA_ID;
  82         if (!strcmp_P(s, PSTR("baudrate")))
  83                 return AA_BAUD_RATE;
  84         if (!strcmp_P(s, PSTR("delay")))
  85                 return AA_DELAY_TIME;
  86         if (!strcmp_P(s, PSTR("high_lim_temp")))
  87                 return AA_HIGHEST_LIMIT_TEMP;
  88         if (!strcmp_P(s, PSTR("low_lim_volt")))
  89                 return AA_LOWEST_LIMIT_VOLTAGE;
  90         if (!strcmp_P(s, PSTR("high_lim_volt")))
  91                 return AA_HIGHEST_LIMIT_VOLTAGE;
  92         if (!strcmp_P(s, PSTR("status_return")))
  93                 return AA_STATUS_RETURN_LEVEL;
  94         if (!strcmp_P(s, PSTR("alarm_led")))
  95                 return AA_ALARM_LED;
  96         if (!strcmp_P(s, PSTR("alarm_shutdown")))
  97                 return AA_ALARM_SHUTDOWN;
  98         if (!strcmp_P(s, PSTR("torque_enable")))
  99                 return AA_TORQUE_ENABLE;
 100         if (!strcmp_P(s, PSTR("led")))
 101                 return AA_LED;
 102         if (!strcmp_P(s, PSTR("cw_comp_margin")))
 103                 return AA_CW_COMPLIANCE_MARGIN;
 104         if (!strcmp_P(s, PSTR("ccw_comp_margin")))
 105                 return AA_CCW_COMPLIANCE_MARGIN;
 106         if (!strcmp_P(s, PSTR("cw_comp_slope")))
 107                 return AA_CW_COMPLIANCE_SLOPE;
 108         if (!strcmp_P(s, PSTR("ccw_comp_slope")))
 109                 return AA_CCW_COMPLIANCE_SLOPE;
 110         if (!strcmp_P(s, PSTR("voltage")))
 111                 return AA_PRESENT_VOLTAGE;
 112         if (!strcmp_P(s, PSTR("temp")))
 113                 return AA_PRESENT_TEMP;
 114         if (!strcmp_P(s, PSTR("reginst")))
 115                 return AA_PRESENT_REGINST;
 116         if (!strcmp_P(s, PSTR("moving")))
 117                 return AA_MOVING;
 118         if (!strcmp_P(s, PSTR("lock")))
 119                 return AA_LOCK;
 120
 121         return 0;
 122 }
 123
 124 /**********************************************************/
 125 /* Ax12_Stress */
 126
 127 /* this structure is filled when cmd_ax12_stress is parsed successfully */
 128 struct cmd_ax12_stress_result {
 129         fixed_string_t arg0;
 130         uint8_t id;
 131 };
 132
 133 /* function called when cmd_ax12_stress is parsed successfully */
 134 static void cmd_ax12_stress_parsed(void *parsed_result, void *data)
 135 {
 136         struct cmd_ax12_stress_result *res = parsed_result;
 137         int i, nb_errs = 0;
 138         uint8_t val;
 139         microseconds t = time_get_us2();
 140
 141         for (i=0; i<1000; i++) {
 142                 if (AX12_read_byte(&gen.ax12, res->id, AA_ID, &val) != 0)
 143                         nb_errs ++;
 144         }
 145
 146         printf_P(PSTR("%d errors / 1000\r\n"), nb_errs);
 147         t = (time_get_us2() - t) / 1000;
 148         printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
 149 }
 150
 151 prog_char str_ax12_stress_arg0[] = "ax12_stress";
 152 parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0);
 153 parse_pgm_token_num_t cmd_ax12_stress_id = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8);
 154
 155 prog_char help_ax12_stress[] = "Stress an AX12 with 1000 'read id' commands";
 156 parse_pgm_inst_t cmd_ax12_stress = {
 157         .f = cmd_ax12_stress_parsed,  /* function to call */
 158         .data = NULL,      /* 2nd arg of func */
 159         .help_str = help_ax12_stress,
 160         .tokens = {        /* token list, NULL terminated */
 161                 (prog_void *)&cmd_ax12_stress_arg0,
 162                 (prog_void *)&cmd_ax12_stress_id,
 163                 NULL,
 164         },
 165 };
 166
 167 /**********************************************************/
 168
 169 /* this structure is filled when cmd_baudrate is parsed successfully */
 170 struct cmd_baudrate_result {
 171         fixed_string_t arg0;
 172         uint32_t arg1;
 173 };
 174
 175 /* function called when cmd_baudrate is parsed successfully */
 176 static void cmd_baudrate_parsed(void * parsed_result, void * data)
 177 {
 178         struct cmd_baudrate_result *res = parsed_result;
 179         struct uart_config c;
 180
 181         printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
 182         uart_getconf(1, &c);
 183         c.baudrate = res->arg1;
 184         uart_setconf(1, &c);
 185         printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
 186 }
 187
 188 prog_char str_baudrate_arg0[] = "baudrate";
 189 parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0);
 190 parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32);
 191
 192 prog_char help_baudrate[] = "Change ax12 baudrate";
 193 parse_pgm_inst_t cmd_baudrate = {
 194         .f = cmd_baudrate_parsed,  /* function to call */
 195         .data = NULL,      /* 2nd arg of func */
 196         .help_str = help_baudrate,
 197         .tokens = {        /* token list, NULL terminated */
 198                 (prog_void *)&cmd_baudrate_arg0,
 199                 (prog_void *)&cmd_baudrate_arg1,
 200                 NULL,
 201         },
 202 };
 203
 204 /**********************************************************/
 205 /* Uint16 */
 206
 207
 208 /* this structure is filled when cmd_uint16_read is parsed successfully */
 209 struct cmd_uint16_result {
 210         fixed_string_t arg0;
 211         fixed_string_t arg1;
 212         uint8_t num;
 213         uint16_t val;
 214 };
 215
 216 /* function called when cmd_uint16_read is parsed successfully */
 217 static void cmd_uint16_read_parsed(void * parsed_result, void * data)
 218 {
 219         struct cmd_uint16_result *res = parsed_result;
 220         uint8_t ret;
 221         uint16_t val;
 222         uint8_t addr = addr_from_string(res->arg1);
 223         ret = AX12_read_int(&gen.ax12, res->num, addr, &val);
 224         if (ret)
 225                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
 226         printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val);
 227 }
 228
 229 prog_char str_uint16_arg0[] = "read";
 230 parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0);
 231 prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#"
 232                 "max_torque#down_calibration#up_calibration#torque_limit#"
 233                 "position#speed#load#punch";
 234 parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1);
 235 parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8);
 236
 237 prog_char help_uint16_read[] = "Read uint16 value (type, num)";
 238 parse_pgm_inst_t cmd_uint16_read = {
 239         .f = cmd_uint16_read_parsed,  /* function to call */
 240         .data = NULL,      /* 2nd arg of func */
 241         .help_str = help_uint16_read,
 242         .tokens = {        /* token list, NULL terminated */
 243                 (prog_void *)&cmd_uint16_arg0,
 244                 (prog_void *)&cmd_uint16_arg1,
 245                 (prog_void *)&cmd_uint16_num,
 246                 NULL,
 247         },
 248 };
 249
 250 /* function called when cmd_uint16_write is parsed successfully */
 251 static void cmd_uint16_write_parsed(void * parsed_result, void * data)
 252 {
 253         struct cmd_uint16_result *res = parsed_result;
 254         uint8_t ret;
 255         uint8_t addr = addr_from_string(res->arg1);
 256         printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1,
 257                  res->val, res->val);
 258         ret = AX12_write_int(&gen.ax12, res->num, addr, res->val);
 259         if (ret)
 260                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
 261 }
 262
 263 prog_char str_uint16_arg0_w[] = "write";
 264 parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w);
 265 prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#"
 266                 "max_torque#torque_limit#punch";
 267 parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w);
 268 parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16);
 269
 270 prog_char help_uint16_write[] = "Write uint16 value (write, num, val)";
 271 parse_pgm_inst_t cmd_uint16_write = {
 272         .f = cmd_uint16_write_parsed,  /* function to call */
 273         .data = NULL,      /* 2nd arg of func */
 274         .help_str = help_uint16_write,
 275         .tokens = {        /* token list, NULL terminated */
 276                 (prog_void *)&cmd_uint16_arg0_w,
 277                 (prog_void *)&cmd_uint16_arg1_w,
 278                 (prog_void *)&cmd_uint16_num,
 279                 (prog_void *)&cmd_uint16_val,
 280                 NULL,
 281         },
 282 };
 283
 284 /**********************************************************/
 285 /* Uint8 */
 286
 287
 288 /* this structure is filled when cmd_uint8_read is parsed successfully */
 289 struct cmd_uint8_result {
 290         fixed_string_t arg0;
 291         fixed_string_t arg1;
 292         uint8_t num;
 293         uint8_t val;
 294 };
 295
 296 /* function called when cmd_uint8_read is parsed successfully */
 297 static void cmd_uint8_read_parsed(void * parsed_result, void * data)
 298 {
 299         struct cmd_uint8_result *res = parsed_result;
 300         uint8_t ret;
 301         uint8_t val;
 302         uint8_t addr = addr_from_string(res->arg1);
 303
 304         ret = AX12_read_byte(&gen.ax12, res->num, addr, &val);
 305         if (ret)
 306                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
 307         printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val);
 308 }
 309
 310 prog_char str_uint8_arg0[] = "read";
 311 parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0);
 312 prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#"
 313                 "low_lim_volt#high_lim_volt#status_return#alarm_led#"
 314                 "alarm_shutdown#torque_enable#led#cw_comp_margin#"
 315                 "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
 316                 "voltage#temp#reginst#moving#lock";
 317 parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1);
 318 parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8);
 319
 320 prog_char help_uint8_read[] = "Read uint8 value (type, num)";
 321 parse_pgm_inst_t cmd_uint8_read = {
 322         .f = cmd_uint8_read_parsed,  /* function to call */
 323         .data = NULL,      /* 2nd arg of func */
 324         .help_str = help_uint8_read,
 325         .tokens = {        /* token list, NULL terminated */
 326                 (prog_void *)&cmd_uint8_arg0,
 327                 (prog_void *)&cmd_uint8_arg1,
 328                 (prog_void *)&cmd_uint8_num,
 329                 NULL,
 330         },
 331 };
 332
 333 /* function called when cmd_uint8_write is parsed successfully */
 334 static void cmd_uint8_write_parsed(void * parsed_result, void * data)
 335 {
 336         struct cmd_uint8_result *res = parsed_result;
 337         uint8_t addr = addr_from_string(res->arg1);
 338         uint8_t ret;
 339         printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1,
 340                  res->val, res->val);
 341         ret = AX12_write_byte(&gen.ax12, res->num, addr, res->val);
 342         if (ret)
 343                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
 344 }
 345
 346 prog_char str_uint8_arg0_w[] = "write";
 347 parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w);
 348 prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#"
 349                 "low_lim_volt#high_lim_volt#status_return#alarm_led#"
 350                 "alarm_shutdown#torque_enable#led#cw_comp_margin#"
 351                 "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
 352                 "reginst#lock";
 353 parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w);
 354 parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8);
 355
 356 prog_char help_uint8_write[] = "Write uint8 value (write, num, val)";
 357 parse_pgm_inst_t cmd_uint8_write = {
 358         .f = cmd_uint8_write_parsed,  /* function to call */
 359         .data = NULL,      /* 2nd arg of func */
 360         .help_str = help_uint8_write,
 361         .tokens = {        /* token list, NULL terminated */
 362                 (prog_void *)&cmd_uint8_arg0_w,
 363                 (prog_void *)&cmd_uint8_arg1_w,
 364                 (prog_void *)&cmd_uint8_num,
 365                 (prog_void *)&cmd_uint8_val,
 366                 NULL,
 367         },
 368 };