projects/microb2009/tests/atm2560/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 2009-02-20 21:10:01 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23
  24 #include <stdio.h>
  25 #include <string.h>
  26
  27 #include <aversive/pgmspace.h>
  28 #include <aversive/wait.h>
  29
  30 #include <i2c.h>
  31 #include <ax12.h>
  32 #include <parse.h>
  33 #include <parse_num.h>
  34 #include <parse_string.h>
  35 #include <uart.h>
  36 #include <pwm_ng.h>
  37 #include <pid.h>
  38 #include <spi.h>
  39 #include <time.h>
  40 #include <quadramp.h>
  41 #include <control_system_manager.h>
  42
  43 #include "main.h"
  44
  45 extern AX12 ax12;
  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
 126 /**********************************************************/
 127 /* Reset */
 128
 129 /* this structure is filled when cmd_reset is parsed successfully */
 130 struct cmd_reset_result {
 131         fixed_string_t arg0;
 132 };
 133
 134 /* function called when cmd_reset is parsed successfully */
 135 static void cmd_reset_parsed(void * parsed_result, void * data)
 136 {
 137         reset();
 138 }
 139
 140 prog_char str_reset_arg0[] = "reset";
 141 parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0);
 142
 143 prog_char help_reset[] = "Reset the board";
 144 parse_pgm_inst_t cmd_reset = {
 145         .f = cmd_reset_parsed,  /* function to call */
 146         .data = NULL,      /* 2nd arg of func */
 147         .help_str = help_reset,
 148         .tokens = {        /* token list, NULL terminated */
 149                 (prog_void *)&cmd_reset_arg0,
 150                 NULL,
 151         },
 152 };
 153
 154 /**********************************************************/
 155 /* Spi_Test */
 156
 157 /* this structure is filled when cmd_spi_test is parsed successfully */
 158 struct cmd_spi_test_result {
 159         fixed_string_t arg0;
 160 };
 161
 162 /* function called when cmd_spi_test is parsed successfully */
 163 static void cmd_spi_test_parsed(void * parsed_result, void * data)
 164 {
 165 #if 0
 166         uint8_t i, ret;
 167
 168         for (i=0; i<3; i++) {
 169                 spi_slave_select(0);
 170                 ret = spi_send_and_receive_byte(i);
 171                 spi_slave_deselect(0);
 172                 printf_P(PSTR("Sent %d, received %d\r\n"), i, ret);
 173         }
 174 #else
 175         printf_P(PSTR("disabled\r\n"));
 176 #endif
 177 }
 178
 179 prog_char str_spi_test_arg0[] = "spi_test";
 180 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
 181
 182 prog_char help_spi_test[] = "Test the SPI";
 183 parse_pgm_inst_t cmd_spi_test = {
 184         .f = cmd_spi_test_parsed,  /* function to call */
 185         .data = NULL,      /* 2nd arg of func */
 186         .help_str = help_spi_test,
 187         .tokens = {        /* token list, NULL terminated */
 188                 (prog_void *)&cmd_spi_test_arg0,
 189                 NULL,
 190         },
 191 };
 192
 193 /**********************************************************/
 194 /* Bootloader */
 195
 196 /* this structure is filled when cmd_bootloader is parsed successfully */
 197 struct cmd_bootloader_result {
 198         fixed_string_t arg0;
 199 };
 200
 201 /* function called when cmd_bootloader is parsed successfully */
 202 static void cmd_bootloader_parsed(void *parsed_result, void *data)
 203 {
 204 #define BOOTLOADER_ADDR 0x3f000
 205         if (pgm_read_byte_far(BOOTLOADER_ADDR) == 0xff) {
 206                 printf_P(PSTR("Bootloader is not present\r\n"));
 207                 return;
 208         }
 209         cli();
 210         /* ... very specific :( */
 211         TIMSK0 = 0;
 212         TIMSK1 = 0;
 213         TIMSK2 = 0;
 214         TIMSK3 = 0;
 215         TIMSK4 = 0;
 216         TIMSK5 = 0;
 217         EIMSK = 0;
 218         UCSR0B = 0;
 219         UCSR1B = 0;
 220         UCSR2B = 0;
 221         UCSR3B = 0;
 222         SPCR = 0;
 223         TWCR = 0;
 224         ACSR = 0;
 225         ADCSRA = 0;
 226
 227         EIND = 1;
 228         __asm__ __volatile__ ("ldi r31,0xf8\n");
 229         __asm__ __volatile__ ("ldi r30,0x00\n");
 230         __asm__ __volatile__ ("eijmp\n");
 231 }
 232
 233 prog_char str_bootloader_arg0[] = "bootloader";
 234 parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0);
 235
 236 prog_char help_bootloader[] = "Launch the bootloader";
 237 parse_pgm_inst_t cmd_bootloader = {
 238         .f = cmd_bootloader_parsed,  /* function to call */
 239         .data = NULL,      /* 2nd arg of func */
 240         .help_str = help_bootloader,
 241         .tokens = {        /* token list, NULL terminated */
 242                 (prog_void *)&cmd_bootloader_arg0,
 243                 NULL,
 244         },
 245 };
 246
 247 /**********************************************************/
 248 /* Ax12_Stress */
 249
 250 /* this structure is filled when cmd_ax12_stress is parsed successfully */
 251 struct cmd_ax12_stress_result {
 252         fixed_string_t arg0;
 253 };
 254
 255 /* function called when cmd_ax12_stress is parsed successfully */
 256 static void cmd_ax12_stress_parsed(void * parsed_result, void * data)
 257 {
 258         int i, nb_errs = 0;
 259         uint8_t val;
 260         microseconds t = time_get_us2();
 261
 262         for (i=0; i<1000; i++) {
 263                 if (AX12_read_byte(&ax12, 3, AA_ID, &val) != 0)
 264                         nb_errs ++;
 265         }
 266
 267         printf_P(PSTR("%d errors / 1000\r\n"), nb_errs);
 268         t = (time_get_us2() - t) / 1000;
 269         printf_P(PSTR("Test done in %d ms\r\n"), (int)t);
 270 }
 271
 272 prog_char str_ax12_stress_arg0[] = "ax12_stress";
 273 parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0);
 274
 275 prog_char help_ax12_stress[] = "Ax12_Stress the board";
 276 parse_pgm_inst_t cmd_ax12_stress = {
 277         .f = cmd_ax12_stress_parsed,  /* function to call */
 278         .data = NULL,      /* 2nd arg of func */
 279         .help_str = help_ax12_stress,
 280         .tokens = {        /* token list, NULL terminated */
 281                 (prog_void *)&cmd_ax12_stress_arg0,
 282                 NULL,
 283         },
 284 };
 285
 286 /**********************************************************/
 287 /* Test */
 288
 289 /* this structure is filled when cmd_test is parsed successfully */
 290 struct cmd_test_result {
 291         fixed_string_t arg0;
 292 };
 293
 294 #define ELBOW_AX12 1
 295 #define WRIST_AX12 2
 296 void arm_goto(int32_t shoulder, uint16_t elbow, uint16_t wrist)
 297 {
 298         uint8_t err;
 299
 300         printf("%ld %d\r\n", shoulder, elbow);
 301         cs_set_consign(&arm.cs_mot, shoulder);
 302         err = AX12_write_int(&ax12, ELBOW_AX12, AA_GOAL_POSITION_L, elbow);
 303         if (!err)
 304                 AX12_write_int(&ax12, WRIST_AX12, AA_GOAL_POSITION_L, wrist);
 305         if (err)
 306                 printf_P(PSTR("AX12 error %x !\r\n"), err);
 307 }
 308
 309 #define arm_take_high_v1()  arm_goto(-18700, 204, 455)
 310 #define arm_take_low_v1()   arm_goto(-11000, 273, 480)
 311 #define arm_take_high_v2()  arm_goto(-18700, 204, 154)
 312 #define arm_take_low_v2()   arm_goto(-11000, 273, 139)
 313 #define arm_intermediate()  arm_goto(-35700, 297, 385)
 314 #define arm_drop_v2()       arm_goto(-16810, 667, 564)
 315 #define arm_drop_v1()       arm_goto(-16810, 667, 904)
 316
 317 /* function called when cmd_test is parsed successfully */
 318 static void cmd_test_parsed(void * parsed_result, void * data)
 319 {
 320         uint8_t i=0;
 321 /*      int8_t err; */
 322
 323         /* Set some AX12 parameters */
 324 /*      err = AX12_write_int(&ax12,0xFE,AA_TORQUE_ENABLE,0x1); */
 325 /*      if (!err) */
 326 /*              AX12_write_int(&ax12,0xFE,AA_PUNCH_L,0x20); */
 327 /*      if (!err) */
 328 /*              AX12_write_int(&ax12,0xFE,AA_TORQUE_LIMIT_L,0x3FF); */
 329 /*      if (!err) */
 330 /*              AX12_write_int(&ax12,0xFE,AA_MOVING_SPEED_L,0x3FF); */
 331 /*      if (!err) */
 332 /*              AX12_write_byte(&ax12,0xFE,AA_ALARM_LED,0xEF); */
 333 /*      if (err) { */
 334 /*              printf_P(PSTR("AX12 error %x !\r\n"), err); */
 335 /*              return; */
 336 /*      } */
 337
 338         for (i=0; i<1; i++) {
 339                 arm_take_high_v1();
 340                 wait_ms(500);
 341                 //              pwm_ng_set(&arm.pwm1B, 4095);
 342                 arm_take_low_v1();
 343                 wait_ms(500);
 344                 arm_take_high_v1();
 345                 wait_ms(500);
 346                 arm_take_high_v2();
 347                 wait_ms(500);
 348                 arm_take_low_v2();
 349                 wait_ms(500);
 350                 arm_take_high_v2();
 351                 wait_ms(500);
 352                 arm_intermediate();
 353                 wait_ms(500);
 354 /*              arm_intermediate2(); */
 355 /*              wait_ms(250); */
 356                 arm_drop_v2();
 357                 wait_ms(500);
 358                 //              pwm_ng_set(&arm.pwm1B, -4095);
 359                 arm_drop_v1();
 360                 wait_ms(500);
 361                 //              pwm_ng_set(&arm.pwm1B, 0);
 362                 wait_ms(500);
 363                 arm_intermediate();
 364                 wait_ms(500);
 365         }
 366 }
 367
 368 prog_char str_test_arg0[] = "test";
 369 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
 370
 371 prog_char help_test[] = "Test func";
 372 parse_pgm_inst_t cmd_test = {
 373         .f = cmd_test_parsed,  /* function to call */
 374         .data = NULL,      /* 2nd arg of func */
 375         .help_str = help_test,
 376         .tokens = {        /* token list, NULL terminated */
 377                 (prog_void *)&cmd_test_arg0,
 378                 NULL,
 379         },
 380 };
 381
 382 /**********************************************************/
 383 /* Arm_Straight */
 384
 385 /* this structure is filled when cmd_arm_straight is parsed successfully */
 386 struct cmd_arm_straight_result {
 387         fixed_string_t arg0;
 388 };
 389
 390 /* function called when cmd_arm_straight is parsed successfully */
 391 static void cmd_arm_straight_parsed(void * parsed_result, void * data)
 392 {
 393         int32_t pos_shoulder[] = {
 394                 -15510,
 395                 -17181,
 396                 -18852,
 397                 -20524,
 398                 -22470,
 399                 -24416,
 400                 -26363,
 401                 -28309,
 402                 -30255,
 403                 -32464,
 404                 -34673,
 405                 -36881,
 406                 -39090,
 407                 -41299,
 408                 -43080,
 409                 -44861,
 410                 -46642,
 411                 -48423,
 412                 -50204,
 413                 -51026,
 414                 -51849,
 415                 -52671,
 416                 -53493,
 417                 -54316,
 418                 -54443,
 419                 -54370,
 420                 -54398,
 421         };
 422         /* vitesse servo : 3ff = 114RPM = 233.2 pas/100ms */
 423         int32_t pos_elbow[] = {
 424                 316,
 425                 301,
 426                 286,
 427                 271,
 428                 261,
 429                 250,
 430                 240,
 431                 230,
 432                 220,
 433                 216,
 434                 212,
 435                 208,
 436                 204,
 437                 200,
 438                 204,
 439                 208,
 440                 212,
 441                 216,
 442                 220,
 443                 230,
 444                 240,
 445                 250,
 446                 261,
 447                 271,
 448                 286,
 449                 301,
 450                 316,
 451         };
 452
 453         int8_t i;
 454         int32_t speed_shoulder;
 455         int32_t speed_elbow;
 456
 457         arm_goto(pos_shoulder[0], pos_elbow[0], 500);
 458         printf("ready\r\n");
 459         while(uart_recv_nowait(0) == -1);
 460
 461         /* 50 ms per incr */
 462         for (i=1; i<26; i++) {
 463                 speed_shoulder = pos_shoulder[i] - pos_shoulder[i-1];
 464                 speed_shoulder /= 10; /* period is 5ms */
 465                 if (speed_shoulder < 0)
 466                         speed_shoulder = -speed_shoulder;
 467
 468                 speed_elbow = pos_elbow[i] - pos_elbow[i-1];
 469                 speed_elbow *= 0x3ff;
 470                 speed_elbow *= 2;
 471                 speed_elbow /= 233;
 472                 if (speed_elbow < 0)
 473                         speed_elbow = -speed_elbow;
 474
 475 #if 1
 476                 printf("shoulder : %ld, %ld / elbow : %ld, %ld\r\n",
 477                        pos_shoulder[i], speed_shoulder,
 478                        pos_elbow[i], speed_elbow);
 479 #else
 480                 quadramp_set_1st_order_vars(&arm.qr_mot, speed_shoulder, speed_shoulder);
 481                 cs_set_consign(&arm.cs_mot, pos_shoulder[i]);
 482
 483                 AX12_write_int(&ax12, ELBOW_AX12, AA_MOVING_SPEED_L, speed_elbow);
 484                 AX12_write_int(&ax12, ELBOW_AX12, AA_GOAL_POSITION_L, pos_elbow[i]);
 485
 486                 time_wait_ms(50);
 487 #endif
 488         }
 489 }
 490
 491 prog_char str_arm_straight_arg0[] = "arm_straight";
 492 parse_pgm_token_string_t cmd_arm_straight_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_straight_result, arg0, str_arm_straight_arg0);
 493
 494 prog_char help_arm_straight[] = "Arm_Straight func";
 495 parse_pgm_inst_t cmd_arm_straight = {
 496         .f = cmd_arm_straight_parsed,  /* function to call */
 497         .data = NULL,      /* 2nd arg of func */
 498         .help_str = help_arm_straight,
 499         .tokens = {        /* token list, NULL terminated */
 500                 (prog_void *)&cmd_arm_straight_arg0,
 501                 NULL,
 502         },
 503 };
 504
 505 /**********************************************************/
 506
 507 /* this structure is filled when cmd_baudrate is parsed successfully */
 508 struct cmd_baudrate_result {
 509         fixed_string_t arg0;
 510         uint32_t arg1;
 511 };
 512
 513 /* function called when cmd_baudrate is parsed successfully */
 514 static void cmd_baudrate_parsed(void * parsed_result, void * data)
 515 {
 516         struct cmd_baudrate_result *res = parsed_result;
 517         struct uart_config c;
 518
 519         printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
 520         uart_getconf(1, &c);
 521         c.baudrate = res->arg1;
 522         uart_setconf(1, &c);
 523         printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
 524 }
 525
 526 prog_char str_baudrate_arg0[] = "baudrate";
 527 parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0);
 528 parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32);
 529
 530 prog_char help_baudrate[] = "Change ax12 baudrate";
 531 parse_pgm_inst_t cmd_baudrate = {
 532         .f = cmd_baudrate_parsed,  /* function to call */
 533         .data = NULL,      /* 2nd arg of func */
 534         .help_str = help_baudrate,
 535         .tokens = {        /* token list, NULL terminated */
 536                 (prog_void *)&cmd_baudrate_arg0,
 537                 (prog_void *)&cmd_baudrate_arg1,
 538                 NULL,
 539         },
 540 };
 541
 542 /**********************************************************/
 543
 544 /* this structure is filled when cmd_arm_goto is parsed successfully */
 545 struct cmd_arm_goto_result {
 546         fixed_string_t arg0;
 547         int32_t arg1;
 548         uint16_t arg2;
 549         uint16_t arg3;
 550 };
 551
 552 /* function called when cmd_arm_goto is parsed successfully */
 553 static void cmd_arm_goto_parsed(void * parsed_result, void * data)
 554 {
 555         struct cmd_arm_goto_result *res = parsed_result;
 556         arm_goto(res->arg1, res->arg2, res->arg3);
 557 }
 558
 559 prog_char str_arm_goto_arg0[] = "arm_goto";
 560 parse_pgm_token_string_t cmd_arm_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_result, arg0, str_arm_goto_arg0);
 561 parse_pgm_token_num_t cmd_arm_goto_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg1, INT32);
 562 parse_pgm_token_num_t cmd_arm_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg2, UINT16);
 563 parse_pgm_token_num_t cmd_arm_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg3, UINT16);
 564
 565 prog_char help_arm_goto[] = "Change arm position (shoulder, elbow, wrist)";
 566 parse_pgm_inst_t cmd_arm_goto = {
 567         .f = cmd_arm_goto_parsed,  /* function to call */
 568         .data = NULL,      /* 2nd arg of func */
 569         .help_str = help_arm_goto,
 570         .tokens = {        /* token list, NULL terminated */
 571                 (prog_void *)&cmd_arm_goto_arg0,
 572                 (prog_void *)&cmd_arm_goto_arg1,
 573                 (prog_void *)&cmd_arm_goto_arg2,
 574                 (prog_void *)&cmd_arm_goto_arg3,
 575                 NULL,
 576         },
 577 };
 578
 579 /**********************************************************/
 580
 581 /* this structure is filled when cmd_arm_capture is parsed successfully */
 582 struct cmd_arm_capture_result {
 583         fixed_string_t arg0;
 584 };
 585
 586 /* function called when cmd_arm_capture is parsed successfully */
 587 static void cmd_arm_capture_parsed(void * parsed_result, void * data)
 588 {
 589 #ifdef notyet
 590         uint16_t elbow, wrist;
 591         int32_t shoulder;
 592         uint8_t ret = 0;
 593
 594         ret |= AX12_read_int(&ax12, ELBOW_AX12, AA_PRESENT_POSITION_L, &elbow);
 595         ret |= AX12_read_int(&ax12, WRIST_AX12, AA_PRESENT_POSITION_L, &wrist);
 596         shoulder = encoders_microb_get_value((void *)ARM_ENC);
 597         if (ret)
 598                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
 599         printf_P(PSTR("%ld %d %d\r\n"), shoulder, elbow, wrist);
 600 #endif
 601 }
 602
 603 prog_char str_arm_capture_arg0[] = "arm_capture";
 604 parse_pgm_token_string_t cmd_arm_capture_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_capture_result, arg0, str_arm_capture_arg0);
 605
 606 prog_char help_arm_capture[] = "Change arm position (shoulder, elbow, wrist)";
 607 parse_pgm_inst_t cmd_arm_capture = {
 608         .f = cmd_arm_capture_parsed,  /* function to call */
 609         .data = NULL,      /* 2nd arg of func */
 610         .help_str = help_arm_capture,
 611         .tokens = {        /* token list, NULL terminated */
 612                 (prog_void *)&cmd_arm_capture_arg0,
 613                 NULL,
 614         },
 615 };
 616 /**********************************************************/
 617 /* Uint16 */
 618
 619
 620 /* this structure is filled when cmd_uint16_read is parsed successfully */
 621 struct cmd_uint16_result {
 622         fixed_string_t arg0;
 623         fixed_string_t arg1;
 624         uint8_t num;
 625         uint16_t val;
 626 };
 627
 628 /* function called when cmd_uint16_read is parsed successfully */
 629 static void cmd_uint16_read_parsed(void * parsed_result, void * data)
 630 {
 631         struct cmd_uint16_result *res = parsed_result;
 632         uint8_t ret;
 633         uint16_t val;
 634         uint8_t addr = addr_from_string(res->arg1);
 635         ret = AX12_read_int(&ax12, res->num, addr, &val);
 636         if (ret)
 637                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
 638         printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val);
 639 }
 640
 641 prog_char str_uint16_arg0[] = "read";
 642 parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0);
 643 prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#"
 644                 "max_torque#down_calibration#up_calibration#torque_limit#"
 645                 "position#speed#load#punch";
 646 parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1);
 647 parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8);
 648
 649 prog_char help_uint16_read[] = "Read uint16 value (type, num)";
 650 parse_pgm_inst_t cmd_uint16_read = {
 651         .f = cmd_uint16_read_parsed,  /* function to call */
 652         .data = NULL,      /* 2nd arg of func */
 653         .help_str = help_uint16_read,
 654         .tokens = {        /* token list, NULL terminated */
 655                 (prog_void *)&cmd_uint16_arg0,
 656                 (prog_void *)&cmd_uint16_arg1,
 657                 (prog_void *)&cmd_uint16_num,
 658                 NULL,
 659         },
 660 };
 661
 662 /* function called when cmd_uint16_write is parsed successfully */
 663 static void cmd_uint16_write_parsed(void * parsed_result, void * data)
 664 {
 665         struct cmd_uint16_result *res = parsed_result;
 666         uint8_t ret;
 667         uint8_t addr = addr_from_string(res->arg1);
 668         printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1,
 669                  res->val, res->val);
 670         ret = AX12_write_int(&ax12, res->num, addr, res->val);
 671         if (ret)
 672                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
 673 }
 674
 675 prog_char str_uint16_arg0_w[] = "write";
 676 parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w);
 677 prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#"
 678                 "max_torque#torque_limit#punch";
 679 parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w);
 680 parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16);
 681
 682 prog_char help_uint16_write[] = "Write uint16 value (write, num, val)";
 683 parse_pgm_inst_t cmd_uint16_write = {
 684         .f = cmd_uint16_write_parsed,  /* function to call */
 685         .data = NULL,      /* 2nd arg of func */
 686         .help_str = help_uint16_write,
 687         .tokens = {        /* token list, NULL terminated */
 688                 (prog_void *)&cmd_uint16_arg0_w,
 689                 (prog_void *)&cmd_uint16_arg1_w,
 690                 (prog_void *)&cmd_uint16_num,
 691                 (prog_void *)&cmd_uint16_val,
 692                 NULL,
 693         },
 694 };
 695
 696 /**********************************************************/
 697 /* Uint8 */
 698
 699
 700 /* this structure is filled when cmd_uint8_read is parsed successfully */
 701 struct cmd_uint8_result {
 702         fixed_string_t arg0;
 703         fixed_string_t arg1;
 704         uint8_t num;
 705         uint8_t val;
 706 };
 707
 708 /* function called when cmd_uint8_read is parsed successfully */
 709 static void cmd_uint8_read_parsed(void * parsed_result, void * data)
 710 {
 711         struct cmd_uint8_result *res = parsed_result;
 712         uint8_t ret;
 713         uint8_t val;
 714         uint8_t addr = addr_from_string(res->arg1);
 715
 716         ret = AX12_read_byte(&ax12, res->num, addr, &val);
 717         if (ret)
 718                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
 719         printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val);
 720 }
 721
 722 prog_char str_uint8_arg0[] = "read";
 723 parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0);
 724 prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#"
 725                 "low_lim_volt#high_lim_volt#status_return#alarm_led#"
 726                 "alarm_shutdown#torque_enable#led#cw_comp_margin#"
 727                 "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
 728                 "voltage#temp#reginst#moving#lock";
 729 parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1);
 730 parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8);
 731
 732 prog_char help_uint8_read[] = "Read uint8 value (type, num)";
 733 parse_pgm_inst_t cmd_uint8_read = {
 734         .f = cmd_uint8_read_parsed,  /* function to call */
 735         .data = NULL,      /* 2nd arg of func */
 736         .help_str = help_uint8_read,
 737         .tokens = {        /* token list, NULL terminated */
 738                 (prog_void *)&cmd_uint8_arg0,
 739                 (prog_void *)&cmd_uint8_arg1,
 740                 (prog_void *)&cmd_uint8_num,
 741                 NULL,
 742         },
 743 };
 744
 745 /* function called when cmd_uint8_write is parsed successfully */
 746 static void cmd_uint8_write_parsed(void * parsed_result, void * data)
 747 {
 748         struct cmd_uint8_result *res = parsed_result;
 749         uint8_t addr = addr_from_string(res->arg1);
 750         uint8_t ret;
 751         printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1,
 752                  res->val, res->val);
 753         ret = AX12_write_byte(&ax12, res->num, addr, res->val);
 754         if (ret)
 755                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
 756 }
 757
 758 prog_char str_uint8_arg0_w[] = "write";
 759 parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w);
 760 prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#"
 761                 "low_lim_volt#high_lim_volt#status_return#alarm_led#"
 762                 "alarm_shutdown#torque_enable#led#cw_comp_margin#"
 763                 "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
 764                 "reginst#lock";
 765 parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w);
 766 parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8);
 767
 768 prog_char help_uint8_write[] = "Write uint8 value (write, num, val)";
 769 parse_pgm_inst_t cmd_uint8_write = {
 770         .f = cmd_uint8_write_parsed,  /* function to call */
 771         .data = NULL,      /* 2nd arg of func */
 772         .help_str = help_uint8_write,
 773         .tokens = {        /* token list, NULL terminated */
 774                 (prog_void *)&cmd_uint8_arg0_w,
 775                 (prog_void *)&cmd_uint8_arg1_w,
 776                 (prog_void *)&cmd_uint8_num,
 777                 (prog_void *)&cmd_uint8_val,
 778                 NULL,
 779         },
 780 };
 781
 782 /**********************************************************/
 783 /* Encoders tests */
 784
 785 /* this structure is filled when cmd_encoders is parsed successfully */
 786 struct cmd_encoders_result {
 787         fixed_string_t arg0;
 788         fixed_string_t arg1;
 789 };
 790
 791 /* function called when cmd_encoders is parsed successfully */
 792 static void cmd_encoders_parsed(void * parsed_result, void * data)
 793 {
 794 #ifdef notyet
 795         while(uart_recv_nowait(0) == -1) {
 796                 printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"),
 797                          encoders_microb_get_value((void *)0),
 798                          encoders_microb_get_value((void *)1),
 799                          encoders_microb_get_value((void *)2),
 800                          encoders_microb_get_value((void *)3));
 801                 wait_ms(100);
 802         }
 803 #endif
 804 }
 805
 806 prog_char str_encoders_arg0[] = "encoders";
 807 parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0);
 808 prog_char str_encoders_arg1[] = "show";
 809 parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1);
 810
 811 prog_char help_encoders[] = "Show encoders values";
 812 parse_pgm_inst_t cmd_encoders = {
 813         .f = cmd_encoders_parsed,  /* function to call */
 814         .data = NULL,      /* 2nd arg of func */
 815         .help_str = help_encoders,
 816         .tokens = {        /* token list, NULL terminated */
 817                 (prog_void *)&cmd_encoders_arg0,
 818                 (prog_void *)&cmd_encoders_arg1,
 819                 NULL,
 820         },
 821 };
 822
 823 /**********************************************************/
 824 /* Pwms tests */
 825
 826 /* this structure is filled when cmd_pwm is parsed successfully */
 827 struct cmd_pwm_result {
 828         fixed_string_t arg0;
 829         fixed_string_t arg1;
 830         int16_t arg2;
 831 };
 832
 833 /* function called when cmd_pwm is parsed successfully */
 834 static void cmd_pwm_parsed(void * parsed_result, void * data)
 835 {
 836         void * pwm_ptr = NULL;
 837         struct cmd_pwm_result * res = parsed_result;
 838
 839         if (!strcmp_P(res->arg1, PSTR("1(2A)")))
 840                 pwm_ptr = &arm.pwm1_2A;
 841         else if (!strcmp_P(res->arg1, PSTR("2(1A)")))
 842                 pwm_ptr = &arm.pwm2_1A;
 843         else if (!strcmp_P(res->arg1, PSTR("3(1B)")))
 844                 pwm_ptr = &arm.pwm3_1B;
 845         else if (!strcmp_P(res->arg1, PSTR("4(1C)")))
 846                 pwm_ptr = &arm.pwm4_1C;
 847
 848         else if (!strcmp_P(res->arg1, PSTR("s1(3A)")))
 849                 pwm_ptr = &arm.servo1;
 850         else if (!strcmp_P(res->arg1, PSTR("s2(3B)")))
 851                 pwm_ptr = &arm.servo2;
 852         else if (!strcmp_P(res->arg1, PSTR("s3(3C)")))
 853                 pwm_ptr = &arm.servo3;
 854         else if (!strcmp_P(res->arg1, PSTR("s4(5A)")))
 855                 pwm_ptr = &arm.servo4;
 856         else if (!strcmp_P(res->arg1, PSTR("s5(5B)")))
 857                 pwm_ptr = &arm.servo5;
 858         else if (!strcmp_P(res->arg1, PSTR("s6(5C)")))
 859                 pwm_ptr = &arm.servo6;
 860
 861         if (pwm_ptr)
 862                 pwm_ng_set(pwm_ptr, res->arg2);
 863
 864         printf_P(PSTR("done\r\n"));
 865 }
 866
 867 prog_char str_pwm_arg0[] = "pwm";
 868 parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0);
 869 prog_char str_pwm_arg1[] = "1(2A)#2(1A)#3(1B)#4(1C)#s1(3A)#s2(3B)#s3(3C)#s4(5A)#s5(5B)#s6(5C)";
 870 parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1);
 871 parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16);
 872
 873 prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]";
 874 parse_pgm_inst_t cmd_pwm = {
 875         .f = cmd_pwm_parsed,  /* function to call */
 876         .data = NULL,      /* 2nd arg of func */
 877         .help_str = help_pwm,
 878         .tokens = {        /* token list, NULL terminated */
 879                 (prog_void *)&cmd_pwm_arg0,
 880                 (prog_void *)&cmd_pwm_arg1,
 881                 (prog_void *)&cmd_pwm_arg2,
 882                 NULL,
 883         },
 884 };
 885
 886 /**********************************************************/
 887 /* Gains for control system */
 888
 889 /* this structure is filled when cmd_gain is parsed successfully */
 890 struct cmd_gain_result {
 891         fixed_string_t arg0;
 892         fixed_string_t arg1;
 893         int16_t p;
 894         int16_t i;
 895         int16_t d;
 896 };
 897
 898 /* function called when cmd_gain is parsed successfully */
 899 static void cmd_gain_parsed(void * parsed_result, void * data)
 900 {
 901         struct cmd_gain_result * res = parsed_result;
 902
 903         if (!strcmp_P(res->arg1, PSTR("arm"))) {
 904                 pid_set_gains(&arm.pid_mot, res->p, res->i, res->d);
 905         }
 906         /* else it is a "show" */
 907
 908         printf_P(PSTR("arm %d %d %d\r\n"),
 909                  pid_get_gain_P(&arm.pid_mot),
 910                  pid_get_gain_I(&arm.pid_mot),
 911                  pid_get_gain_D(&arm.pid_mot));
 912 }
 913
 914 prog_char str_gain_arg0[] = "gain";
 915 parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, arg0, str_gain_arg0);
 916 prog_char str_gain_arg1[] = "arm";
 917 parse_pgm_token_string_t cmd_gain_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, arg1, str_gain_arg1);
 918 parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16);
 919 parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16);
 920 parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16);
 921
 922 prog_char help_gain[] = "Set gain values for PID";
 923 parse_pgm_inst_t cmd_gain = {
 924         .f = cmd_gain_parsed,  /* function to call */
 925         .data = NULL,      /* 2nd arg of func */
 926         .help_str = help_gain,
 927         .tokens = {        /* token list, NULL terminated */
 928                 (prog_void *)&cmd_gain_arg0,
 929                 (prog_void *)&cmd_gain_arg1,
 930                 (prog_void *)&cmd_gain_p,
 931                 (prog_void *)&cmd_gain_i,
 932                 (prog_void *)&cmd_gain_d,
 933                 NULL,
 934         },
 935 };
 936
 937 /* show */
 938
 939 prog_char str_gain_show_arg[] = "show";
 940 parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, arg1, str_gain_show_arg);
 941
 942 prog_char help_gain_show[] = "Show gain values for PID";
 943 parse_pgm_inst_t cmd_gain_show = {
 944         .f = cmd_gain_parsed,  /* function to call */
 945         .data = NULL,      /* 2nd arg of func */
 946         .help_str = help_gain_show,
 947         .tokens = {        /* token list, NULL terminated */
 948                 (prog_void *)&cmd_gain_arg0,
 949                 (prog_void *)&cmd_gain_show_arg,
 950                 NULL,
 951         },
 952 };
 953
 954
 955 /**********************************************************/
 956 /* Speeds for control system */
 957
 958 /* this structure is filled when cmd_speed is parsed successfully */
 959 struct cmd_speed_result {
 960         fixed_string_t arg0;
 961         fixed_string_t arg1;
 962         uint16_t s;
 963 };
 964
 965 /* function called when cmd_speed is parsed successfully */
 966 static void cmd_speed_parsed(void * parsed_result, void * data)
 967 {
 968 #if 0
 969         struct cmd_speed_result * res = parsed_result;
 970
 971         if (!strcmp_P(res->arg1, PSTR("arm"))) {
 972                 ramp_set_vars(&ext.r_b, res->s, res->s); /* set speed */
 973         }
 974
 975         printf_P(PSTR("arm %lu\r\n"),
 976                  ext.r_b.var_pos);
 977 #else
 978         printf_P(PSTR("DISABLED FOR NOW\r\n"));
 979 #endif
 980 }
 981
 982 prog_char str_speed_arg0[] = "speed";
 983 parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, arg0, str_speed_arg0);
 984 prog_char str_speed_arg1[] = "arm#show";
 985 parse_pgm_token_string_t cmd_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, arg1, str_speed_arg1);
 986 parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16);
 987
 988 prog_char help_speed[] = "Set speed values for ramp filter";
 989 parse_pgm_inst_t cmd_speed = {
 990         .f = cmd_speed_parsed,  /* function to call */
 991         .data = NULL,      /* 2nd arg of func */
 992         .help_str = help_speed,
 993         .tokens = {        /* token list, NULL terminated */
 994                 (prog_void *)&cmd_speed_arg0,
 995                 (prog_void *)&cmd_speed_arg1,
 996                 (prog_void *)&cmd_speed_s,
 997                 NULL,
 998         },
 999 };
1000
1001 /* show */
1002
1003 prog_char str_speed_show_arg[] = "show";
1004 parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, arg1, str_speed_show_arg);
1005
1006 prog_char help_speed_show[] = "Show speed values for ramp filter";
1007 parse_pgm_inst_t cmd_speed_show = {
1008         .f = cmd_speed_parsed,  /* function to call */
1009         .data = NULL,      /* 2nd arg of func */
1010         .help_str = help_speed_show,
1011         .tokens = {        /* token list, NULL terminated */
1012                 (prog_void *)&cmd_speed_arg0,
1013                 (prog_void *)&cmd_speed_show_arg,
1014                 NULL,
1015         },
1016 };
1017
1018
1019 /**********************************************************/
1020 /* Pos for control system */
1021
1022 /* this structure is filled when cmd_pos is parsed successfully */
1023 struct cmd_pos_result {
1024         fixed_string_t arg0;
1025         fixed_string_t arg1;
1026         int32_t p;
1027 };
1028
1029 /* function called when cmd_pos is parsed successfully */
1030 static void cmd_pos_parsed(void * parsed_result, void * data)
1031 {
1032         struct cmd_pos_result * res = parsed_result;
1033         //      uint8_t i;
1034
1035         if (!strcmp_P(res->arg1, PSTR("arm"))) {
1036                 cs_set_consign(&arm.cs_mot, res->p);
1037         }
1038
1039 #if 0
1040         for (i=0; i<50; i++) {
1041                 printf("%ld %ld %ld\r\n",
1042                        pid_get_value_in(&arm.pid_mot),
1043                        pid_get_value_out(&arm.pid_mot),
1044                        pid_get_value_D(&arm.pid_mot)
1045                        );
1046         }
1047 #endif
1048 }
1049
1050 prog_char str_pos_arg0[] = "pos";
1051 parse_pgm_token_string_t cmd_pos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pos_result, arg0, str_pos_arg0);
1052 prog_char str_pos_arg1[] = "arm";
1053 parse_pgm_token_string_t cmd_pos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pos_result, arg1, str_pos_arg1);
1054 parse_pgm_token_num_t cmd_pos_p = TOKEN_NUM_INITIALIZER(struct cmd_pos_result, p, INT32);
1055
1056 prog_char help_pos[] = "Set pos value";
1057 parse_pgm_inst_t cmd_pos = {
1058         .f = cmd_pos_parsed,  /* function to call */
1059         .data = NULL,      /* 2nd arg of func */
1060         .help_str = help_pos,
1061         .tokens = {        /* token list, NULL terminated */
1062                 (prog_void *)&cmd_pos_arg0,
1063                 (prog_void *)&cmd_pos_arg1,
1064                 (prog_void *)&cmd_pos_p,
1065                 NULL,
1066         },
1067 };
1068
1069
1070 /**********************************************************/
1071 /* Events on/off */
1072
1073 /* this structure is filled when cmd_event is parsed successfully */
1074 struct cmd_event_result {
1075         fixed_string_t arg0;
1076         fixed_string_t arg1;
1077         fixed_string_t arg2;
1078 };
1079
1080 /* function called when cmd_event is parsed successfully */
1081 static void cmd_event_parsed(void * parsed_result, void * data)
1082 {
1083 #ifdef notyet
1084         u08 bit=0;
1085
1086         struct cmd_event_result * res = parsed_result;
1087
1088         if (!strcmp_P(res->arg1, PSTR("cs"))) {
1089                 if (!strcmp_P(res->arg2, PSTR("on"))) {
1090                         pwm_ng_set(ARM_MOT_PWM, 0);
1091                         printf_P(PSTR("ax12 will start\r\n"));
1092                         while(uart_recv_nowait(0) == -1);
1093                         AX12_write_int(&ax12,0xFE,AA_TORQUE_ENABLE,0x1);
1094                         AX12_write_int(&ax12, ELBOW_AX12, AA_GOAL_POSITION_L, 660);
1095                         AX12_write_int(&ax12, WRIST_AX12, AA_GOAL_POSITION_L, 613);
1096                         printf_P(PSTR("Set the arm to 0\r\n"));
1097                         while(uart_recv_nowait(0) == -1);
1098                         //                      encoders_microb_set_value(ARM_ENC, 0);
1099                 }
1100                 bit = CS_ON;
1101         }
1102 /*      else if (!strcmp_P(res->arg1, PSTR("catapult"))) */
1103 /*              bit = CATAPULT_CS_ON; */
1104
1105         if (!strcmp_P(res->arg2, PSTR("on")))
1106                 arm.flags |= bit;
1107         else if (!strcmp_P(res->arg2, PSTR("off")))
1108                 arm.flags &= (~bit);
1109         printf_P(PSTR("%s is %s\r\n"), res->arg1,
1110                       (bit & arm.flags) ? "on":"off");
1111 #endif
1112 }
1113
1114 prog_char str_event_arg0[] = "event";
1115 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg0);
1116 prog_char str_event_arg1[] = "cs";
1117 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
1118 prog_char str_event_arg2[] = "on#off#show";
1119 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
1120
1121 prog_char help_event[] = "Enable/disable events";
1122 parse_pgm_inst_t cmd_event = {
1123         .f = cmd_event_parsed,  /* function to call */
1124         .data = NULL,      /* 2nd arg of func */
1125         .help_str = help_event,
1126         .tokens = {        /* token list, NULL terminated */
1127                 (prog_void *)&cmd_event_arg0,
1128                 (prog_void *)&cmd_event_arg1,
1129                 (prog_void *)&cmd_event_arg2,
1130                 NULL,
1131         },
1132 };
1133
1134 /**********************************************************/
1135 /* Maximums for control system */
1136
1137 /* this structure is filled when cmd_maximum is parsed successfully */
1138 struct cmd_maximum_result {
1139         fixed_string_t arg0;
1140         fixed_string_t arg1;
1141         uint32_t in;
1142         uint32_t i;
1143         uint32_t out;
1144 };
1145
1146 /* function called when cmd_maximum is parsed successfully */
1147 static void cmd_maximum_parsed(void * parsed_result, void * data)
1148 {
1149         struct cmd_maximum_result * res = parsed_result;
1150
1151         if (!strcmp_P(res->arg1, PSTR("arm"))) {
1152                 pid_set_maximums(&arm.pid_mot, res->in, res->i, res->out);
1153         }
1154         /* else it is a "show" */
1155
1156         printf_P(PSTR("maximum arm %lu %lu %lu\r\n"),
1157                  pid_get_max_in(&arm.pid_mot),
1158                  pid_get_max_I(&arm.pid_mot),
1159                  pid_get_max_out(&arm.pid_mot));
1160 }
1161
1162 prog_char str_maximum_arg0[] = "maximum";
1163 parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, arg0, str_maximum_arg0);
1164 prog_char str_maximum_arg1[] = "arm";
1165 parse_pgm_token_string_t cmd_maximum_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, arg1, str_maximum_arg1);
1166 parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32);
1167 parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32);
1168 parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32);
1169
1170 prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
1171 parse_pgm_inst_t cmd_maximum = {
1172         .f = cmd_maximum_parsed,  /* function to call */
1173         .data = NULL,      /* 2nd arg of func */
1174         .help_str = help_maximum,
1175         .tokens = {        /* token list, NULL terminated */
1176                 (prog_void *)&cmd_maximum_arg0,
1177                 (prog_void *)&cmd_maximum_arg1,
1178                 (prog_void *)&cmd_maximum_in,
1179                 (prog_void *)&cmd_maximum_i,
1180                 (prog_void *)&cmd_maximum_out,
1181                 NULL,
1182         },
1183 };
1184
1185 /* show */
1186
1187 prog_char str_maximum_show_arg[] = "show";
1188 parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, arg1, str_maximum_show_arg);
1189
1190 prog_char help_maximum_show[] = "Show maximum values for PID";
1191 parse_pgm_inst_t cmd_maximum_show = {
1192         .f = cmd_maximum_parsed,  /* function to call */
1193         .data = NULL,      /* 2nd arg of func */
1194         .help_str = help_maximum_show,
1195         .tokens = {        /* token list, NULL terminated */
1196                 (prog_void *)&cmd_maximum_arg0,
1197                 (prog_void *)&cmd_maximum_show_arg,
1198                 NULL,
1199         },
1200 };
1201
1202 /**********************************************************/
1203 /* Quadramp for control system */
1204
1205 /* this structure is filled when cmd_quadramp is parsed successfully */
1206 struct cmd_quadramp_result {
1207         fixed_string_t arg0;
1208         fixed_string_t arg1;
1209         uint32_t ap;
1210         uint32_t an;
1211         uint32_t sp;
1212         uint32_t sn;
1213 };
1214
1215 /* function called when cmd_quadramp is parsed successfully */
1216 static void cmd_quadramp_parsed(void * parsed_result, void * data)
1217 {
1218         struct cmd_quadramp_result * res = parsed_result;
1219
1220         if (!strcmp_P(res->arg1, PSTR("arm"))) {
1221                 quadramp_set_1st_order_vars(&arm.qr_mot, res->sp, res->sn);
1222                 quadramp_set_2nd_order_vars(&arm.qr_mot, res->ap, res->an);
1223         }
1224 /*      else if (!strcmp_P(res->arg1, PSTR("distance"))) { */
1225 /*              quadramp_set_1st_order_vars(&arm.qr_d, res->sp, res->sn); */
1226 /*              quadramp_set_2nd_order_vars(&arm.qr_d, res->ap, res->an); */
1227 /*      } */
1228         /* else it's a "show" */
1229
1230         printf_P(PSTR("quadramp arm %ld %ld %ld %ld\r\n"),
1231                  arm.qr_mot.var_2nd_ord_pos,
1232                  arm.qr_mot.var_2nd_ord_neg,
1233                  arm.qr_mot.var_1st_ord_pos,
1234                  arm.qr_mot.var_1st_ord_neg);
1235 }
1236
1237 prog_char str_quadramp_arg0[] = "quadramp";
1238 parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, arg0, str_quadramp_arg0);
1239 prog_char str_quadramp_arg1[] = "arm";
1240 parse_pgm_token_string_t cmd_quadramp_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, arg1, str_quadramp_arg1);
1241 parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32);
1242 parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32);
1243 parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32);
1244 parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32);
1245
1246 prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
1247 parse_pgm_inst_t cmd_quadramp = {
1248         .f = cmd_quadramp_parsed,  /* function to call */
1249         .data = NULL,      /* 2nd arg of func */
1250         .help_str = help_quadramp,
1251         .tokens = {        /* token list, NULL terminated */
1252                 (prog_void *)&cmd_quadramp_arg0,
1253                 (prog_void *)&cmd_quadramp_arg1,
1254                 (prog_void *)&cmd_quadramp_ap,
1255                 (prog_void *)&cmd_quadramp_an,
1256                 (prog_void *)&cmd_quadramp_sp,
1257                 (prog_void *)&cmd_quadramp_sn,
1258
1259                 NULL,
1260         },
1261 };
1262
1263 /* show */
1264
1265 prog_char str_quadramp_show_arg[] = "show";
1266 parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, arg1, str_quadramp_show_arg);
1267
1268 prog_char help_quadramp_show[] = "Get quadramp values for control system";
1269 parse_pgm_inst_t cmd_quadramp_show = {
1270         .f = cmd_quadramp_parsed,  /* function to call */
1271         .data = NULL,      /* 2nd arg of func */
1272         .help_str = help_quadramp_show,
1273         .tokens = {        /* token list, NULL terminated */
1274                 (prog_void *)&cmd_quadramp_arg0,
1275                 (prog_void *)&cmd_quadramp_show_arg,
1276                 NULL,
1277         },
1278 };
1279
1280
1281 /**********************************************************/
1282
1283
1284 /* in progmem */
1285 parse_pgm_ctx_t main_ctx[] = {
1286         (parse_pgm_inst_t *)&cmd_reset,
1287         (parse_pgm_inst_t *)&cmd_spi_test,
1288         (parse_pgm_inst_t *)&cmd_bootloader,
1289         (parse_pgm_inst_t *)&cmd_ax12_stress,
1290         (parse_pgm_inst_t *)&cmd_test,
1291         (parse_pgm_inst_t *)&cmd_arm_straight,
1292         (parse_pgm_inst_t *)&cmd_baudrate,
1293         (parse_pgm_inst_t *)&cmd_arm_goto,
1294         (parse_pgm_inst_t *)&cmd_arm_capture,
1295         (parse_pgm_inst_t *)&cmd_uint16_read,
1296         (parse_pgm_inst_t *)&cmd_uint16_write,
1297         (parse_pgm_inst_t *)&cmd_uint8_read,
1298         (parse_pgm_inst_t *)&cmd_uint8_write,
1299         (parse_pgm_inst_t *)&cmd_encoders,
1300         (parse_pgm_inst_t *)&cmd_pwm,
1301         (parse_pgm_inst_t *)&cmd_gain,
1302         (parse_pgm_inst_t *)&cmd_gain_show,
1303         (parse_pgm_inst_t *)&cmd_speed,
1304         (parse_pgm_inst_t *)&cmd_speed_show,
1305         (parse_pgm_inst_t *)&cmd_pos,
1306         (parse_pgm_inst_t *)&cmd_event,
1307         (parse_pgm_inst_t *)&cmd_maximum,
1308         (parse_pgm_inst_t *)&cmd_maximum_show,
1309         (parse_pgm_inst_t *)&cmd_quadramp,
1310         (parse_pgm_inst_t *)&cmd_quadramp_show,
1311
1312         NULL,
1313 };