modules/devices/servo/ax12/test/commands.c

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