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