projects/microb2009/tests/arm_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.6 2009-03-15 20:08:51 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 <encoders_microb.h>
  37 #include <pwm_ng.h>
  38 #include <pid.h>
  39 #include <spi.h>
  40 #include <time.h>
  41 #include <quadramp.h>
  42 #include <control_system_manager.h>
  43
  44 #include <scheduler.h>
  45
  46 #include "main.h"
  47
  48 #include "arm_xy.h"
  49
  50 extern AX12 ax12;
  51
  52 #define AX12_MAX_RETRY 2
  53
  54 uint8_t AX12_read_int_retry(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
  55                          uint16_t *val)
  56 {
  57         int8_t err, i;
  58         for (i=0;i<AX12_MAX_RETRY;i++){
  59                 err = AX12_read_int(ax12, id, address, val);
  60                 if (!err)
  61                         return err;
  62         }
  63
  64         printf("AX12: retry %d times err: %d\r\n", AX12_MAX_RETRY, err);
  65         return err;
  66 }
  67
  68 uint8_t AX12_write_int_retry(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
  69                        uint16_t data)
  70 {
  71         int8_t err, i;
  72         for (i=0;i<AX12_MAX_RETRY;i++){
  73                 err = AX12_write_int(ax12, id, address,data);
  74                 if (!err)
  75                         return err;
  76         }
  77
  78         printf("AX12: retry %d times err: %d\r\n", AX12_MAX_RETRY, err);
  79         return err;
  80 }
  81
  82 uint8_t addr_from_string(const char *s)
  83 {
  84         /* 16 bits */
  85         if (!strcmp_P(s, PSTR("cw_angle_limit")))
  86                 return AA_CW_ANGLE_LIMIT_L;
  87         if (!strcmp_P(s, PSTR("ccw_angle_limit")))
  88                 return AA_CCW_ANGLE_LIMIT_L;
  89         if (!strcmp_P(s, PSTR("max_torque")))
  90                 return AA_MAX_TORQUE_L;
  91         if (!strcmp_P(s, PSTR("down_calibration")))
  92                 return AA_DOWN_CALIBRATION_L;
  93         if (!strcmp_P(s, PSTR("up_calibration")))
  94                 return AA_UP_CALIBRATION_L;
  95         if (!strcmp_P(s, PSTR("torque_limit")))
  96                 return AA_TORQUE_LIMIT_L;
  97         if (!strcmp_P(s, PSTR("position")))
  98                 return AA_PRESENT_POSITION_L;
  99         if (!strcmp_P(s, PSTR("speed")))
 100                 return AA_PRESENT_SPEED_L;
 101         if (!strcmp_P(s, PSTR("load")))
 102                 return AA_PRESENT_LOAD_L;
 103         if (!strcmp_P(s, PSTR("moving_speed")))
 104                 return AA_MOVING_SPEED_L;
 105         if (!strcmp_P(s, PSTR("model")))
 106                 return AA_MODEL_NUMBER_L;
 107         if (!strcmp_P(s, PSTR("goal_pos")))
 108                 return AA_GOAL_POSITION_L;
 109         if (!strcmp_P(s, PSTR("punch")))
 110                 return AA_PUNCH_L;
 111
 112         /* 8 bits */
 113         if (!strcmp_P(s, PSTR("firmware")))
 114                 return AA_FIRMWARE;
 115         if (!strcmp_P(s, PSTR("id")))
 116                 return AA_ID;
 117         if (!strcmp_P(s, PSTR("baudrate")))
 118                 return AA_BAUD_RATE;
 119         if (!strcmp_P(s, PSTR("delay")))
 120                 return AA_DELAY_TIME;
 121         if (!strcmp_P(s, PSTR("high_lim_temp")))
 122                 return AA_HIGHEST_LIMIT_TEMP;
 123         if (!strcmp_P(s, PSTR("low_lim_volt")))
 124                 return AA_LOWEST_LIMIT_VOLTAGE;
 125         if (!strcmp_P(s, PSTR("high_lim_volt")))
 126                 return AA_HIGHEST_LIMIT_VOLTAGE;
 127         if (!strcmp_P(s, PSTR("status_return")))
 128                 return AA_STATUS_RETURN_LEVEL;
 129         if (!strcmp_P(s, PSTR("alarm_led")))
 130                 return AA_ALARM_LED;
 131         if (!strcmp_P(s, PSTR("alarm_shutdown")))
 132                 return AA_ALARM_SHUTDOWN;
 133         if (!strcmp_P(s, PSTR("torque_enable")))
 134                 return AA_TORQUE_ENABLE;
 135         if (!strcmp_P(s, PSTR("led")))
 136                 return AA_LED;
 137         if (!strcmp_P(s, PSTR("cw_comp_margin")))
 138                 return AA_CW_COMPLIANCE_MARGIN;
 139         if (!strcmp_P(s, PSTR("ccw_comp_margin")))
 140                 return AA_CCW_COMPLIANCE_MARGIN;
 141         if (!strcmp_P(s, PSTR("cw_comp_slope")))
 142                 return AA_CW_COMPLIANCE_SLOPE;
 143         if (!strcmp_P(s, PSTR("ccw_comp_slope")))
 144                 return AA_CCW_COMPLIANCE_SLOPE;
 145         if (!strcmp_P(s, PSTR("voltage")))
 146                 return AA_PRESENT_VOLTAGE;
 147         if (!strcmp_P(s, PSTR("temp")))
 148                 return AA_PRESENT_TEMP;
 149         if (!strcmp_P(s, PSTR("reginst")))
 150                 return AA_PRESENT_REGINST;
 151         if (!strcmp_P(s, PSTR("moving")))
 152                 return AA_MOVING;
 153         if (!strcmp_P(s, PSTR("lock")))
 154                 return AA_LOCK;
 155
 156         return 0;
 157 }
 158
 159
 160
 161 /**********************************************************/
 162 /* Reset */
 163
 164 /* this structure is filled when cmd_reset is parsed successfully */
 165 struct cmd_reset_result {
 166         fixed_string_t arg0;
 167 };
 168
 169 /* function called when cmd_reset is parsed successfully */
 170 static void cmd_reset_parsed(void * parsed_result, void * data)
 171 {
 172         reset();
 173 }
 174
 175 prog_char str_reset_arg0[] = "reset";
 176 parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0);
 177
 178 prog_char help_reset[] = "Reset the board";
 179 parse_pgm_inst_t cmd_reset = {
 180         .f = cmd_reset_parsed,  /* function to call */
 181         .data = NULL,      /* 2nd arg of func */
 182         .help_str = help_reset,
 183         .tokens = {        /* token list, NULL terminated */
 184                 (prog_void *)&cmd_reset_arg0,
 185                 NULL,
 186         },
 187 };
 188
 189 /**********************************************************/
 190 /* Spi_Test */
 191
 192 /* this structure is filled when cmd_spi_test is parsed successfully */
 193 struct cmd_spi_test_result {
 194         fixed_string_t arg0;
 195 };
 196
 197 /* function called when cmd_spi_test is parsed successfully */
 198 static void cmd_spi_test_parsed(void * parsed_result, void * data)
 199 {
 200 #if 0
 201         uint8_t i, ret;
 202
 203         for (i=0; i<3; i++) {
 204                 spi_slave_select(0);
 205                 ret = spi_send_and_receive_byte(i);
 206                 spi_slave_deselect(0);
 207                 printf_P(PSTR("Sent %d, received %d\r\n"), i, ret);
 208         }
 209 #else
 210         printf_P(PSTR("disabled\r\n"));
 211 #endif
 212 }
 213
 214 prog_char str_spi_test_arg0[] = "spi_test";
 215 parse_pgm_token_string_t cmd_spi_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spi_test_result, arg0, str_spi_test_arg0);
 216
 217 prog_char help_spi_test[] = "Test the SPI";
 218 parse_pgm_inst_t cmd_spi_test = {
 219         .f = cmd_spi_test_parsed,  /* function to call */
 220         .data = NULL,      /* 2nd arg of func */
 221         .help_str = help_spi_test,
 222         .tokens = {        /* token list, NULL terminated */
 223                 (prog_void *)&cmd_spi_test_arg0,
 224                 NULL,
 225         },
 226 };
 227
 228 /**********************************************************/
 229 /* Bootloader */
 230
 231 /* this structure is filled when cmd_bootloader is parsed successfully */
 232 struct cmd_bootloader_result {
 233         fixed_string_t arg0;
 234 };
 235
 236 /* function called when cmd_bootloader is parsed successfully */
 237 static void cmd_bootloader_parsed(void * parsed_result, void * data)
 238 {
 239 #ifdef __AVR_ATmega128__
 240 #define BOOTLOADER_ADDR 0x1e000
 241 #else
 242 #define BOOTLOADER_ADDR 0x3f000
 243 #endif
 244         if (pgm_read_byte_far(BOOTLOADER_ADDR) == 0xff) {
 245                 printf_P(PSTR("Bootloader is not present\r\n"));
 246                 return;
 247         }
 248         cli();
 249         /* ... very specific :( */
 250 #ifdef __AVR_ATmega128__
 251         TIMSK = 0;
 252         ETIMSK = 0;
 253 #else
 254         /* XXX */
 255 #endif
 256         EIMSK = 0;
 257         UCSR0B = 0;
 258         UCSR1B = 0;
 259         SPCR = 0;
 260         TWCR = 0;
 261         ACSR = 0;
 262         ADCSRA = 0;
 263
 264 #ifdef __AVR_ATmega128__
 265         __asm__ __volatile__ ("ldi r30,0x00\n");
 266         __asm__ __volatile__ ("ldi r31,0xf0\n");
 267         __asm__ __volatile__ ("ijmp\n");
 268 #else
 269         __asm__ __volatile__ ("ldi r30,0x00\n");
 270         __asm__ __volatile__ ("ldi r31,0xf8\n");
 271         __asm__ __volatile__ ("ldi eind,0x01\n");
 272         __asm__ __volatile__ ("eijmp\n");
 273 #endif
 274 }
 275
 276 prog_char str_bootloader_arg0[] = "bootloader";
 277 parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0);
 278
 279 prog_char help_bootloader[] = "Bootloader the board";
 280 parse_pgm_inst_t cmd_bootloader = {
 281         .f = cmd_bootloader_parsed,  /* function to call */
 282         .data = NULL,      /* 2nd arg of func */
 283         .help_str = help_bootloader,
 284         .tokens = {        /* token list, NULL terminated */
 285                 (prog_void *)&cmd_bootloader_arg0,
 286                 NULL,
 287         },
 288 };
 289
 290 /**********************************************************/
 291 /* Ax12_Stress */
 292
 293 /* this structure is filled when cmd_ax12_stress is parsed successfully */
 294 struct cmd_ax12_stress_result {
 295         fixed_string_t arg0;
 296         uint8_t id;
 297         uint32_t num;
 298
 299 };
 300
 301 /* function called when cmd_ax12_stress is parsed successfully */
 302 static void cmd_ax12_stress_parsed(void * parsed_result, void * data)
 303 {
 304         uint32_t i, nb_errs = 0;
 305         uint8_t val;
 306         microseconds t = time_get_us2();
 307         struct cmd_ax12_stress_result *res = parsed_result;
 308
 309         for (i=0; i<res->num; i++) {
 310                 if (AX12_read_byte(&ax12, res->id, AA_ID, &val) != 0)
 311                         nb_errs ++;
 312                 if (uart_recv_nowait(0) != -1)
 313                         break;
 314
 315         }
 316
 317         printf_P(PSTR("%ld errors / %ld\r\n"), nb_errs, i);
 318         t = (time_get_us2() - t) / 1000;
 319         printf_P(PSTR("Test done in %ld ms\r\n"), t);
 320 }
 321
 322 prog_char str_ax12_stress_arg0[] = "ax12_stress";
 323 parse_pgm_token_string_t cmd_ax12_stress_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ax12_stress_result, arg0, str_ax12_stress_arg0);
 324 parse_pgm_token_num_t cmd_ax12_stress_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, id, UINT8);
 325 parse_pgm_token_num_t cmd_ax12_stress_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ax12_stress_result, num, UINT32);
 326
 327 prog_char help_ax12_stress[] = "Ax12_Stress the board";
 328 parse_pgm_inst_t cmd_ax12_stress = {
 329         .f = cmd_ax12_stress_parsed,  /* function to call */
 330         .data = NULL,      /* 2nd arg of func */
 331         .help_str = help_ax12_stress,
 332         .tokens = {        /* token list, NULL terminated */
 333                 (prog_void *)&cmd_ax12_stress_arg0,
 334                 (prog_void *)&cmd_ax12_stress_arg1,
 335                 (prog_void *)&cmd_ax12_stress_arg2,
 336                 NULL,
 337         },
 338 };
 339
 340
 341 /**********************************************************/
 342 /* Test */
 343
 344 /* this structure is filled when cmd_test is parsed successfully */
 345 struct cmd_test_result {
 346         fixed_string_t arg0;
 347         uint16_t arg1;
 348         uint16_t arg2;
 349 };
 350
 351 #define R_ELBOW_AX12 1
 352 #define R_WRIST_AX12 2
 353
 354 #define L_ELBOW_AX12 4
 355 #define L_WRIST_AX12 3
 356
 357
 358 #define FINGER_AX12 5
 359
 360
 361 void arm_l_goto(int32_t shoulder, uint16_t elbow, uint16_t wrist)
 362 {
 363         uint8_t err;
 364
 365         //printf("%ld %d %d\r\n", shoulder, elbow, wrist);
 366         //wait_ms(1);
 367         //cs_set_consign(&arm.cs_mot, shoulder);
 368         err = AX12_write_int(&ax12, L_ELBOW_AX12, AA_GOAL_POSITION_L, elbow);
 369         if (!err)
 370                 err = AX12_write_int(&ax12, L_WRIST_AX12, AA_GOAL_POSITION_L, wrist);
 371         if (err)
 372                 printf_P(PSTR("AX12 error %x !\r\n"), err);
 373 }
 374
 375
 376 void arm_goto(Arm_Pos * arm_pos, int32_t shoulder, uint16_t elbow, uint16_t wrist)
 377 {
 378         uint8_t err;
 379
 380         //printf("%ld %d %d\r\n", shoulder, elbow, wrist);
 381         //wait_ms(1);
 382         cs_set_consign(&arm.cs_mot, shoulder);
 383         err = AX12_write_int(&ax12, arm_pos->ELBOW_AX12, AA_GOAL_POSITION_L, elbow);
 384         if (!err)
 385                 err = AX12_write_int(&ax12, arm_pos->WRIST_AX12, AA_GOAL_POSITION_L, wrist);
 386         if (err)
 387                 printf_P(PSTR("AX12 error %x !\r\n"), err);
 388 }
 389
 390 void arm_goto_dh(Arm_Pos* arm_pos, int32_t shoulder, uint16_t elbow)
 391 {
 392         uint8_t err;
 393         cs_set_consign(&arm.cs_mot, shoulder);
 394         err = AX12_write_int(&ax12, arm_pos->ELBOW_AX12, AA_GOAL_POSITION_L, elbow);
 395         if (err)
 396                 printf_P(PSTR("AX12 error %x !\r\n"), err);
 397 }
 398
 399 void finger_right(void)
 400 {
 401         AX12_write_int(&ax12, FINGER_AX12, AA_GOAL_POSITION_L, 666);
 402 }
 403
 404 void finger_left(void)
 405 {
 406         AX12_write_int(&ax12, FINGER_AX12, AA_GOAL_POSITION_L, 340);
 407 }
 408
 409 void finger_center(void)
 410 {
 411         AX12_write_int(&ax12, FINGER_AX12, AA_GOAL_POSITION_L, 490);
 412 }
 413
 414 /*
 415 #define arm_take_high_v1()  arm_goto(-18700, 204, 455)
 416 #define arm_take_low_v1()   arm_goto(-11000, 273, 480)
 417 #define arm_take_high_v2()  arm_goto(-18700, 204, 154)
 418 #define arm_take_low_v2()   arm_goto(-11000, 273, 139)
 419 #define arm_intermediate()  arm_goto(-35700, 297, 385)
 420 #define arm_drop_v2()       arm_goto(-16810, 667, 564)
 421 #define arm_drop_v1()       arm_goto(-16810, 667, 904)
 422 */
 423
 424 #define arm_take_high_v1()  arm_goto(-11533, 295, 477)
 425 #define arm_take_low_v1()   arm_goto(-9835 , 311, 470)
 426 #define arm_take_high_v2()  arm_goto(-11553, 295, 126)
 427 #define arm_take_low_v2()   arm_goto(-9835,  311, 118)
 428 #define arm_intermediate()  arm_goto(-25753, 299, 252)
 429 #define arm_drop_v2()       arm_goto(-2427,  656, 396)
 430 #define arm_drop_v1()       arm_goto(-11280, 547, 699)
 431
 432 #define arm_drop_v3()       arm_goto(-11280, 547, 396)
 433 //#define arm_drop_v4()       arm_goto(-11000, 656, 699)
 434
 435
 436
 437 #define ARM_INTER_WAIT
 438 /* function called when cmd_test is parsed successfully */
 439 static void cmd_test_parsed(void * parsed_result, void * data)
 440 {
 441 #if 0
 442         uint8_t i=0;
 443         struct cmd_test_result *res = parsed_result;
 444         uint16_t t_w = res->arg1;
 445         int16_t ppwm = res->arg2;
 446
 447         AX12_write_int(&ax12, ELBOW_AX12, AA_MOVING_SPEED_L, 1000);
 448 /*      int8_t err; */
 449
 450         /* Set some AX12 parameters */
 451 /*      err = AX12_write_int(&ax12,0xFE,AA_TORQUE_ENABLE,0x1); */
 452 /*      if (!err) */
 453 /*              AX12_write_int(&ax12,0xFE,AA_PUNCH_L,0x20); */
 454 /*      if (!err) */
 455 /*              AX12_write_int(&ax12,0xFE,AA_TORQUE_LIMIT_L,0x3FF); */
 456 /*      if (!err) */
 457 /*              AX12_write_int(&ax12,0xFE,AA_MOVING_SPEED_L,0x3FF); */
 458 /*      if (!err) */
 459 /*              AX12_write_byte(&ax12,0xFE,AA_ALARM_LED,0xEF); */
 460 /*      if (err) { */
 461 /*              printf_P(PSTR("AX12 error %x !\r\n"), err); */
 462 /*              return; */
 463 /*      } */
 464
 465         for (i=0; i<1; i++) {
 466                 arm_take_high_v1();
 467                 wait_ms(t_w);
 468                 pwm_ng_set(&arm.pwm1B, ppwm);
 469                 arm_take_low_v1();
 470                 wait_ms(t_w);
 471                 arm_take_high_v1();
 472                 wait_ms(t_w);
 473
 474
 475                 arm_take_high_v2();
 476                 wait_ms(t_w);
 477                 pwm_ng_set(&arm.pwm3C, ppwm);
 478                 arm_take_low_v2();
 479                 wait_ms(t_w);
 480                 arm_take_high_v2();
 481                 wait_ms(t_w);
 482
 483
 484                 arm_intermediate();
 485                 wait_ms(t_w);
 486 /*              arm_intermediate2(); */
 487 /*              wait_ms(250); */
 488                 arm_drop_v2();
 489                 wait_ms(t_w);
 490                 pwm_ng_set(&arm.pwm3C, -ppwm);
 491                 wait_ms(50);
 492                 pwm_ng_set(&arm.pwm3C, 0);
 493
 494                 arm_drop_v3();
 495                 wait_ms(t_w);
 496                 //arm_drop_v4();
 497                 //wait_ms(t_w);
 498                 arm_drop_v1();
 499                 wait_ms(t_w);
 500                 pwm_ng_set(&arm.pwm1B, -ppwm);
 501                 wait_ms(50);
 502                 pwm_ng_set(&arm.pwm1B, 0);
 503
 504                 arm_intermediate();
 505                 wait_ms(t_w);
 506         }
 507 #endif
 508 }
 509
 510 prog_char str_test_arg0[] = "test";
 511 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
 512 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, arg1, UINT16);
 513 parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, arg2, UINT16);
 514
 515 prog_char help_test[] = "Test func timewait pump_pwm";
 516 parse_pgm_inst_t cmd_test = {
 517         .f = cmd_test_parsed,  /* function to call */
 518         .data = NULL,      /* 2nd arg of func */
 519         .help_str = help_test,
 520         .tokens = {        /* token list, NULL terminated */
 521                 (prog_void *)&cmd_test_arg0,
 522                 (prog_void *)&cmd_test_arg1,
 523                 (prog_void *)&cmd_test_arg2,
 524                 NULL,
 525         },
 526 };
 527
 528
 529 /**********************************************************/
 530 /* armxy */
 531
 532 /* this structure is filled when cmd_armxy is parsed successfully */
 533 struct cmd_armxy_result {
 534         fixed_string_t arg0;
 535         int16_t arg1;
 536         int16_t arg2;
 537         int16_t arg3;
 538 };
 539
 540
 541
 542 Arm_Pos arm_pos_r;
 543 Arm_Pos arm_pos_l;
 544
 545 #define ARM_STATE_MOV 1
 546 #define ARM_STATE_IN_POS  2
 547
 548
 549
 550 /*
 551 int32_t arm_h = 0;
 552 int32_t arm_d = 260;
 553 int32_t arm_w = 110;
 554 */
 555
 556 int8_t arm_do_xy_sched_update(Arm_Pos *arm_pos)
 557 {
 558
 559         int8_t ret;
 560         int32_t fin_h, fin_d;
 561         int32_t as;
 562         int32_t ae;
 563         int32_t aw;
 564         int32_t s_quad;
 565         int32_t e_quad;
 566
 567         uint32_t next_time;
 568         double as_fin_rad, ae_fin_rad;
 569         int32_t as_deg, ae_deg;
 570         //double wrist_out;
 571
 572         //printf("upt called\r\n");
 573         fin_h = arm_pos->goal_h;
 574         fin_d = arm_pos->goal_d;
 575
 576         arm_pos->state = ARM_STATE_MOV;
 577
 578         //quadramp_set_1st_order_vars(&arm.qr_mot, 800, 800); /* set speed */
 579
 580
 581
 582
 583
 584
 585
 586         ret = arm_do_step(arm_pos, &arm_pos->h, &arm_pos->d,
 587                           fin_h, fin_d,
 588                           &as, &ae, &aw,
 589                           &as_fin_rad, &ae_fin_rad,
 590                           &next_time, &s_quad, &e_quad);
 591         //printf("ret: %d\r\n", ret);
 592         /*
 593         printf("do_step: %d arm_h %ld arm_d: %ld as: %ld ae:%ld\r\n",
 594                ret,
 595                arm_pos->h, arm_pos->d,
 596                as, ae);
 597         printf("asdeg %f aedeg %f nextt:%ld squad: %ld equad: %ld\r\n",
 598                ((as_fin_rad*180)/M_PI), ((ae_fin_rad*180)/M_PI),
 599                next_time,
 600                s_quad, e_quad);
 601         */
 602
 603         //printf("ret: %d time: %ld\r\n", ret, next_time);
 604         as_deg = (as_fin_rad*180.)/M_PI;
 605         ae_deg = (ae_fin_rad*180.)/M_PI;
 606
 607
 608         aw = -arm_pos->goal_w+as_deg+ae_deg;
 609
 610         /* printf("as_deg: %ld ae_deg: %ld aw deg: %ld\r\n", as_deg, ae_deg, aw); */
 611         /*
 612         wrist_angle_deg2robot(arm_pos->goal_w,
 613                               &wrist_out);
 614         */
 615
 616         /* printf("w_out: %f\r\n", wrist_out); */
 617
 618         if (ret<0)
 619                 return arm_pos->state = ret; // XXX
 620
 621
 622         quadramp_set_1st_order_vars(&arm.qr_mot, s_quad, s_quad); /* set speed */
 623         AX12_write_int(&ax12, arm_pos->ELBOW_AX12, AA_MOVING_SPEED_L, e_quad);
 624
 625
 626
 627         arm_goto_dh(arm_pos, as, ae);
 628
 629         //wait_ms(next_time/1000);
 630
 631         if (ret == 0){
 632                 //printf("set arm end\r\n");
 633                 arm_pos->state = ARM_STATE_IN_POS;
 634                 return 0;
 635         }
 636
 637         if (next_time<SCHEDULER_UNIT)
 638                 next_time = SCHEDULER_UNIT;
 639
 640         ret = scheduler_add_single_event_priority((void *)arm_do_xy_sched_update,
 641                                                   arm_pos,
 642                                                   next_time/SCHEDULER_UNIT,
 643                                                   50);
 644
 645
 646         //printf("add event %ld\r\n", next_time/SCHEDULER_UNIT );
 647
 648
 649
 650         return 0;
 651
 652 }
 653
 654
 655
 656 /*
 657 #define SPEED_NEAR_STOP 15
 658 #define CS_NEAR_STEP 50
 659 */
 660
 661 #define ARM_NEAR_POS 8
 662 #define ARM_NEAR_SPEED0 100
 663
 664 #define CS_NEAR_POS 100
 665
 666
 667
 668
 669 #define ARM_MAX_AX12_SS 1000
 670
 671
 672 #define MOV_MAX_TIMEOUT 100L
 673
 674
 675
 676 int8_t arm_do_xy_nowait(Arm_Pos *arm_pos, int16_t armh, int16_t armd, int16_t w_angle, int32_t *wrist_wait_time)
 677 {
 678         int8_t ret;
 679
 680
 681         double as_fin_rad, ae_fin_rad, wrist_out;
 682         int32_t as_deg, ae_deg, aw_deg;
 683         uint16_t wrist_cur;
 684         int32_t wrist_diff;
 685
 686
 687         arm_pos->goal_h = armh;
 688         arm_pos->goal_d = armd;
 689         arm_pos->goal_w = w_angle;
 690
 691         arm_pos->state = ARM_STATE_MOV;
 692
 693         if (wrist_wait_time)
 694                 *wrist_wait_time = 0;
 695
 696         /* calc for final pos */
 697         ret = cart2angle(armh, armd, &as_fin_rad, &ae_fin_rad);
 698         if (ret)
 699                 return ret;
 700
 701
 702
 703         /*give directly final wrist pos*/
 704         //AX12_write_int(&ax12, WRIST_AX12, AA_MOVING_SPEED_L, 0x3ff);
 705
 706         as_deg = (as_fin_rad*180.)/M_PI;
 707         ae_deg = (ae_fin_rad*180.)/M_PI;
 708         aw_deg = -arm_pos->goal_w+as_deg+ae_deg;
 709         arm_pos->wrist_angle_deg2robot(aw_deg,
 710                                        &wrist_out);
 711         /* calc theorical wrist movement duration */
 712         ret = AX12_read_int(&ax12, arm_pos->WRIST_AX12, AA_PRESENT_POSITION_L, &wrist_cur);
 713
 714         wrist_diff = wrist_out-wrist_cur;
 715
 716
 717         if (ret)
 718                 printf_P(PSTR("AX12 read pos wrist error %x !\r\n"), ret);
 719         else{
 720                 if  (wrist_diff  && wrist_wait_time)
 721                         *wrist_wait_time = (1000*ABS((int32_t)wrist_diff))/ARM_MAX_AX12_SS;
 722         }
 723
 724         ret = AX12_write_int(&ax12, arm_pos->WRIST_AX12, AA_GOAL_POSITION_L, wrist_out);
 725         if (ret)
 726                 printf_P(PSTR("AX12 wrist final pos error %x !\r\n"), ret);
 727
 728         arm_do_xy_sched_update(arm_pos);
 729
 730         while(arm_pos->state == ARM_STATE_MOV){
 731         }
 732
 733         if (arm_pos->state != ARM_STATE_IN_POS)
 734                 printf("error wait no speed ret: %d\r\n", arm_pos->state);
 735         return ret;
 736
 737
 738 }
 739
 740 int8_t arm_do_xy_wait_nospeed(Arm_Pos *arm_pos, int16_t armh, int16_t armd, int16_t w_angle)
 741 {
 742         int8_t ret;
 743         int16_t spd1, spd2;
 744         int16_t val1, val2;
 745         int16_t valp1, valp2;
 746         int32_t cs_err;
 747
 748         microseconds t1,t2 ;
 749
 750         int32_t wrist_wait_time;
 751
 752
 753         ret = arm_do_xy_nowait(arm_pos, armh, armd, w_angle, &wrist_wait_time);
 754
 755         //printf("wwt %ld\r\n", wrist_wait_time);
 756
 757         //ret = arm_do_xy(arm_pos, armh, armd, w_angle);
 758         //ret = arm_do_xy_sched(arm_pos, armh, armd, w_angle);
 759
 760         t1 = time_get_us2();
 761         while (1){
 762                 AX12_read_int(&ax12, arm_pos->ELBOW_AX12, AA_PRESENT_SPEED_L, (uint16_t*)&spd1);
 763                 AX12_read_int(&ax12, arm_pos->WRIST_AX12, AA_PRESENT_SPEED_L, (uint16_t*)&spd2);
 764
 765                 AX12_read_int(&ax12, arm_pos->ELBOW_AX12, AA_GOAL_POSITION_L, (uint16_t*)&val1);
 766                 AX12_read_int(&ax12, arm_pos->WRIST_AX12, AA_GOAL_POSITION_L, (uint16_t*)&val2);
 767
 768                 AX12_read_int(&ax12, arm_pos->ELBOW_AX12, AA_PRESENT_POSITION_L, (uint16_t*)&valp1);
 769                 AX12_read_int(&ax12, arm_pos->WRIST_AX12, AA_PRESENT_POSITION_L, (uint16_t*)&valp2);
 770
 771
 772                 val1-=valp1;
 773                 val2-=valp2;
 774
 775
 776                 cs_err = cs_get_error(&arm.cs_mot);
 777
 778                 if (ABS(val1)<ARM_NEAR_POS && ABS(val2)<ARM_NEAR_POS && ABS(cs_err)<CS_NEAR_POS &&
 779                     ABS(spd1)<ARM_NEAR_SPEED0 && ABS(spd2)<ARM_NEAR_SPEED0)
 780                         break;
 781
 782                 t2 = time_get_us2();
 783
 784                 /* TIME OUT XXX ms */
 785                 if (t2-t1>(MOV_MAX_TIMEOUT+wrist_wait_time)*1000L){
 786                         printf("wait speed timeout: %d %d %ld spd %d %d\r\n",
 787                                val1,val2,
 788                                ABS(cs_err),
 789                                spd1, spd2);
 790                         break;
 791                 }
 792
 793
 794         }
 795
 796         return ret;
 797 }
 798
 799
 800 /* function called when cmd_armxy is parsed successfully */
 801 static void cmd_armxy_parsed(void * parsed_result, void * data)
 802 {
 803         struct cmd_armxy_result *res = parsed_result;
 804         int16_t armx = res->arg1;
 805         int16_t army = res->arg2;
 806         int16_t wrist_angle_deg = res->arg3;
 807         Arm_Pos *arm_pos;
 808
 809         if (!strcmp_P(res->arg0, PSTR("rarmxy")))
 810                 arm_pos = &arm_pos_r;
 811         else
 812                 arm_pos = &arm_pos_l;
 813
 814
 815         arm_do_xy_wait_nospeed(arm_pos, armx,army, wrist_angle_deg);
 816
 817
 818         return ;
 819
 820
 821
 822
 823
 824 }
 825
 826 prog_char str_armxy_arg0[] = "larmxy#rarmxy";
 827 parse_pgm_token_string_t cmd_armxy_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_armxy_result, arg0, str_armxy_arg0);
 828 parse_pgm_token_num_t cmd_armxy_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_armxy_result, arg1, INT16);
 829 parse_pgm_token_num_t cmd_armxy_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_armxy_result, arg2, INT16);
 830 parse_pgm_token_num_t cmd_armxy_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_armxy_result, arg3, INT16);
 831
 832 prog_char help_armxy[] = "Armxy x y ";
 833 parse_pgm_inst_t cmd_armxy = {
 834         .f = cmd_armxy_parsed,  /* function to call */
 835         .data = NULL,      /* 2nd arg of func */
 836         .help_str = help_armxy,
 837         .tokens = {        /* token list, NULL terminated */
 838                 (prog_void *)&cmd_armxy_arg0,
 839                 (prog_void *)&cmd_armxy_arg1,
 840                 (prog_void *)&cmd_armxy_arg2,
 841                 (prog_void *)&cmd_armxy_arg3,
 842                 NULL,
 843         },
 844 };
 845
 846
 847
 848
 849
 850 /**********************************************************/
 851 /* arm_circ */
 852
 853 /* this structure is filled when cmd_arm_circ is parsed successfully */
 854 struct cmd_arm_circ_result {
 855         fixed_string_t arg0;
 856         uint8_t step;
 857 };
 858
 859 /* function called when cmd_arm_circ is parsed successfully */
 860 static void cmd_arm_circ_parsed(void * parsed_result, void * data)
 861 {
 862 #if 0
 863         int32_t i;
 864         double a;
 865         struct cmd_arm_circ_result *res = parsed_result;
 866         //int32_t l = 90;
 867         int32_t add_h, add_d;
 868
 869         add_h = 0;
 870         add_d = 170;
 871
 872         /*
 873         for (i=0; i<360; i+=res->step) {
 874                 a = (double)(i*2*M_PI/360.);
 875                 arm_do_xy(&arm_pos_r, add_h+l*sin(a), add_d+l*cos(a) , 10);
 876
 877         }
 878         */
 879 #endif
 880 }
 881
 882 prog_char str_arm_circ_arg0[] = "arm_circ";
 883 parse_pgm_token_string_t cmd_arm_circ_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_circ_result, arg0, str_arm_circ_arg0);
 884 parse_pgm_token_num_t cmd_arm_circ_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_arm_circ_result, step, UINT8);
 885
 886 prog_char help_arm_circ[] = "Arm_Circ the board";
 887 parse_pgm_inst_t cmd_arm_circ = {
 888         .f = cmd_arm_circ_parsed,  /* function to call */
 889         .data = NULL,      /* 2nd arg of func */
 890         .help_str = help_arm_circ,
 891         .tokens = {        /* token list, NULL terminated */
 892                 (prog_void *)&cmd_arm_circ_arg0,
 893                 (prog_void *)&cmd_arm_circ_arg1,
 894                 NULL,
 895         },
 896 };
 897
 898
 899
 900 /**********************************************************/
 901 /* arm_harv */
 902
 903 /* this structure is filled when cmd_arm_harv is parsed successfully */
 904 struct cmd_arm_harv_result {
 905         fixed_string_t arg0;
 906         uint16_t wait;
 907         int16_t hight;
 908 };
 909
 910 #define ANGL1 2
 911 #define ANGL2 110
 912
 913 #define PWM_PUMP -3000
 914
 915 #define ARM_GET_D 68
 916
 917 //#define ARM_LOW_H -150
 918 #define ARM_LOW_D 210
 919
 920 #define FINGER_DELAY 200
 921
 922
 923 #define ARM_GET_H -150
 924 /* function called when cmd_arm_harv is parsed successfully */
 925 static void cmd_arm_harv_parsed(void * parsed_result, void * data)
 926 {
 927         struct cmd_arm_harv_result *res = parsed_result;
 928
 929         /*get column*/
 930         finger_left();
 931         wait_ms(FINGER_DELAY);
 932         finger_center();
 933         wait_ms(FINGER_DELAY);
 934
 935         arm_do_xy_nowait(&arm_pos_r, ARM_GET_H+10, ARM_GET_D, ANGL1, NULL);
 936
 937         finger_right();
 938         wait_ms(FINGER_DELAY);
 939
 940         /* TAKE COLUMN */
 941         //arm_do_xy_wait_nospeed(&arm_pos_r, 130, 130, ANGL1);
 942
 943         arm_do_xy_wait_nospeed(&arm_pos_r, ARM_GET_H, ARM_GET_D, ANGL1);
 944
 945         finger_left();
 946
 947         wait_ms(res->wait);
 948
 949         pwm_ng_set(&arm.pwm1B, PWM_PUMP);
 950         wait_ms(40);
 951
 952         //self.ser.write("pwm 1B -3000\n");
 953         wait_ms(res->wait);
 954
 955         //arm_do_xy_wait_nospeed(&arm_pos_r, -120, ARM_GET_D, ANGL1);
 956         wait_ms(res->wait);
 957         arm_do_xy_wait_nospeed(&arm_pos_r, ARM_GET_H+10, ARM_GET_D, ANGL2);
 958
 959
 960
 961         /* WAIT LOLO FOR COLUMN*/
 962         finger_right();
 963         wait_ms(600);
 964
 965         wait_ms(res->wait);
 966         arm_do_xy_wait_nospeed(&arm_pos_r, ARM_GET_H, ARM_GET_D, ANGL2);
 967         pwm_ng_set(&arm.pwm3C, PWM_PUMP);
 968
 969         wait_ms(res->wait);
 970
 971
 972         finger_left();
 973         wait_ms(40);
 974
 975
 976         arm_do_xy_wait_nospeed(&arm_pos_r, res->hight+80,ARM_LOW_D-140,ANGL2);
 977         wait_ms(res->wait);
 978         arm_do_xy_wait_nospeed(&arm_pos_r, res->hight+80, ARM_LOW_D, ANGL2);
 979         wait_ms(res->wait);
 980
 981
 982
 983
 984
 985         /******** MAKE COLUMN *******/
 986
 987         arm_do_xy_wait_nospeed(&arm_pos_r, res->hight+10, ARM_LOW_D, ANGL2);
 988         //plak arm_do_xy_wait_nospeed(&arm_pos_r, res->hight, ARM_LOW_D, ANGL2);
 989         wait_ms(res->wait);
 990         wait_ms(150);
 991         pwm_ng_set(&arm.pwm3C, -PWM_PUMP);
 992         wait_ms(res->wait);
 993         wait_ms(100);
 994
 995
 996         //arm_do_xy_nowait(&arm_pos_r, res->hight+65,ARM_LOW_D,ANGL2, NULL);
 997         wait_ms(res->wait);
 998
 999         arm_do_xy_wait_nospeed(&arm_pos_r, res->hight+65,ARM_LOW_D,ANGL1);
1000         wait_ms(res->wait);
1001         arm_do_xy_wait_nospeed(&arm_pos_r, res->hight+40,ARM_LOW_D,ANGL1);
1002         //plak arm_do_xy_wait_nospeed(&arm_pos_r, res->hight+30,ARM_LOW_D,ANGL1);
1003         wait_ms(res->wait);
1004         wait_ms(150);
1005         pwm_ng_set(&arm.pwm1B, -PWM_PUMP);
1006         wait_ms(res->wait);
1007         wait_ms(100);
1008
1009         arm_do_xy_nowait(&arm_pos_r, res->hight+70,ARM_LOW_D,ANGL1, NULL);
1010
1011
1012         pwm_ng_set(&arm.pwm1B, 0);
1013         pwm_ng_set(&arm.pwm3C, 0);
1014
1015
1016         arm_do_xy_nowait(&arm_pos_r, res->hight+80,ARM_LOW_D-140,ANGL1, NULL);
1017
1018         arm_do_xy_nowait(&arm_pos_r, -100,80,ANGL1, NULL);
1019
1020 }
1021
1022 prog_char str_arm_harv_arg0[] = "arm_harv";
1023 parse_pgm_token_string_t cmd_arm_harv_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_harv_result, arg0, str_arm_harv_arg0);
1024 parse_pgm_token_num_t cmd_arm_harv_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_arm_harv_result, wait, UINT16);
1025 parse_pgm_token_num_t cmd_arm_harv_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_arm_harv_result, hight, INT16);
1026
1027 prog_char help_arm_harv[] = "Arm_Harv the board";
1028 parse_pgm_inst_t cmd_arm_harv = {
1029         .f = cmd_arm_harv_parsed,  /* function to call */
1030         .data = NULL,      /* 2nd arg of func */
1031         .help_str = help_arm_harv,
1032         .tokens = {        /* token list, NULL terminated */
1033                 (prog_void *)&cmd_arm_harv_arg0,
1034                 (prog_void *)&cmd_arm_harv_arg1,
1035                 (prog_void *)&cmd_arm_harv_arg2,
1036                 NULL,
1037         },
1038 };
1039
1040
1041
1042
1043 /**********************************************************/
1044 /* Arm_Straight */
1045
1046 /* this structure is filled when cmd_arm_straight is parsed successfully */
1047 struct cmd_arm_straight_result {
1048         fixed_string_t arg0;
1049         uint32_t arg1;
1050         uint32_t arg2;
1051         fixed_string_t arg3;
1052 };
1053
1054 /* function called when cmd_arm_straight is parsed successfully */
1055 static void cmd_arm_straight_parsed(void * parsed_result, void * data)
1056 {
1057         int32_t pos_shoulder[] = {
1058                 -15510,
1059                 -17181,
1060                 -18852,
1061                 -20524,
1062                 -22470,
1063                 -24416,
1064                 -26363,
1065                 -28309,
1066                 -30255,
1067                 -32464,
1068                 -34673,
1069                 -36881,
1070                 -39090,
1071                 -41299,
1072                 -43080,
1073                 -44861,
1074                 -46642,
1075                 -48423,
1076                 -50204,
1077                 -51026,
1078                 -51849,
1079                 -52671,
1080                 -53493,
1081                 -54316,
1082                 -54443,
1083                 -54370,
1084                 -54398,
1085         };
1086         /* vitesse servo : 3ff = 114RPM = 233.2 pas/100ms */
1087         int32_t pos_elbow[] = {
1088                 316,
1089                 301,
1090                 286,
1091                 271,
1092                 261,
1093                 250,
1094                 240,
1095                 230,
1096                 220,
1097                 216,
1098                 212,
1099                 208,
1100                 204,
1101                 200,
1102                 204,
1103                 208,
1104                 212,
1105                 216,
1106                 220,
1107                 230,
1108                 240,
1109                 250,
1110                 261,
1111                 271,
1112                 286,
1113                 301,
1114                 316,
1115         };
1116
1117         int8_t i;
1118         int32_t speed_shoulder;
1119         int32_t speed_elbow;
1120         int32_t arm_period;
1121         int8_t arm_step;
1122         int8_t arm_max_pos;
1123         int8_t arm_world;
1124         microseconds t_start;
1125
1126         int8_t tab_len = sizeof(pos_elbow)/sizeof(int32_t);
1127
1128         struct cmd_arm_straight_result *res = parsed_result;
1129
1130         arm_period = res->arg1;
1131         arm_step = res->arg2;
1132         if (arm_step<0 || arm_step > tab_len){
1133                 printf("bad step => reset to 1\r\n");
1134                 arm_step = 1;
1135         }
1136         arm_world = strcmp("real", res->arg3);
1137         printf("speed: %ld step:%d\r\n", arm_period, arm_step);
1138
1139         //arm_goto(pos_shoulder[0], pos_elbow[0], 500);
1140         printf("ready\r\n");
1141         while(uart_recv_nowait(0) == -1);
1142
1143         arm_max_pos = (tab_len/arm_step)*arm_step;
1144
1145         printf("tstart %ld\r\n", time_get_us2());
1146         /* 50 ms per incr */
1147         for (i=0; i<tab_len-arm_step; i+=arm_step) {
1148                 t_start = time_get_us2();
1149
1150                 speed_shoulder = pos_shoulder[i+arm_step] - pos_shoulder[i];
1151                 speed_shoulder /= arm_period/CS_PERIOD; /* period is 5ms */
1152                 if (speed_shoulder < 0)
1153                         speed_shoulder = -speed_shoulder;
1154
1155                 speed_elbow = pos_elbow[i+arm_step] - pos_elbow[i];
1156                 speed_elbow *= 0x3ff;
1157                 speed_elbow /= (233*arm_period/100000);
1158                 if (speed_elbow < 0)
1159                         speed_elbow = -speed_elbow;
1160
1161                 if (arm_world){
1162                         printf("shoulder : %ld, %ld / elbow : %ld, %ld\r\n",
1163                                pos_shoulder[i], speed_shoulder,
1164                                pos_elbow[i], speed_elbow);
1165                 }
1166                 else{
1167                         quadramp_set_1st_order_vars(&arm.qr_mot, speed_shoulder, speed_shoulder);
1168                         cs_set_consign(&arm.cs_mot, pos_shoulder[i]);
1169
1170                         AX12_write_int(&ax12, R_ELBOW_AX12, AA_MOVING_SPEED_L, speed_elbow);
1171                         AX12_write_int(&ax12, R_ELBOW_AX12, AA_GOAL_POSITION_L, pos_elbow[i]);
1172                         while(time_get_us2() - t_start < arm_period);
1173
1174                 }
1175         }
1176         printf("tstop %ld\r\n", time_get_us2());
1177
1178
1179         /* reset QUADRAMP */
1180         quadramp_set_1st_order_vars(&arm.qr_mot, 800, 800); /* set speed */
1181         quadramp_set_2nd_order_vars(&arm.qr_mot, 100, 100); /* set accel */
1182
1183 }
1184
1185 prog_char str_arm_straight_arg0[] = "arm_straight";
1186 parse_pgm_token_string_t cmd_arm_straight_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_straight_result, arg0, str_arm_straight_arg0);
1187 parse_pgm_token_num_t cmd_arm_straight_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_arm_straight_result, arg1, UINT32);
1188 parse_pgm_token_num_t cmd_arm_straight_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_arm_straight_result, arg2, UINT32);
1189 prog_char str_event_arg3[] = "sim#real";
1190 parse_pgm_token_string_t cmd_arm_straight_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_arm_straight_result, arg3, str_event_arg3);
1191
1192
1193 prog_char help_arm_straight[] = "Arm_Straight func speed step";
1194 parse_pgm_inst_t cmd_arm_straight = {
1195         .f = cmd_arm_straight_parsed,  /* function to call */
1196         .data = NULL,      /* 2nd arg of func */
1197         .help_str = help_arm_straight,
1198         .tokens = {        /* token list, NULL terminated */
1199                 (prog_void *)&cmd_arm_straight_arg0,
1200                 (prog_void *)&cmd_arm_straight_arg1,
1201                 (prog_void *)&cmd_arm_straight_arg2,
1202                 (prog_void *)&cmd_arm_straight_arg3,
1203                 NULL,
1204         },
1205 };
1206
1207 /**********************************************************/
1208
1209 /* this structure is filled when cmd_baudrate is parsed successfully */
1210 struct cmd_baudrate_result {
1211         fixed_string_t arg0;
1212         uint32_t arg1;
1213 };
1214
1215 /* function called when cmd_baudrate is parsed successfully */
1216 static void cmd_baudrate_parsed(void * parsed_result, void * data)
1217 {
1218         struct cmd_baudrate_result *res = parsed_result;
1219         struct uart_config c;
1220
1221         printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
1222         uart_getconf(1, &c);
1223         c.baudrate = res->arg1;
1224         uart_setconf(1, &c);
1225         printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
1226 }
1227
1228 prog_char str_baudrate_arg0[] = "baudrate";
1229 parse_pgm_token_string_t cmd_baudrate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0, str_baudrate_arg0);
1230 parse_pgm_token_num_t cmd_baudrate_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1, UINT32);
1231
1232 prog_char help_baudrate[] = "Change ax12 baudrate";
1233 parse_pgm_inst_t cmd_baudrate = {
1234         .f = cmd_baudrate_parsed,  /* function to call */
1235         .data = NULL,      /* 2nd arg of func */
1236         .help_str = help_baudrate,
1237         .tokens = {        /* token list, NULL terminated */
1238                 (prog_void *)&cmd_baudrate_arg0,
1239                 (prog_void *)&cmd_baudrate_arg1,
1240                 NULL,
1241         },
1242 };
1243
1244 /**********************************************************/
1245
1246 /* this structure is filled when cmd_arm_goto is parsed successfully */
1247 struct cmd_arm_goto_result {
1248         fixed_string_t arg0;
1249         int32_t arg1;
1250         uint16_t arg2;
1251         uint16_t arg3;
1252 };
1253
1254 /* function called when cmd_arm_goto is parsed successfully */
1255 static void cmd_arm_goto_parsed(void * parsed_result, void * data)
1256 {
1257         struct cmd_arm_goto_result *res = parsed_result;
1258         Arm_Pos * arm_pos;
1259         if (!strcmp(res->arg0, "rarm_goto"))
1260                 arm_pos = &arm_pos_r;
1261         else
1262                 arm_pos = &arm_pos_l;
1263         arm_goto(arm_pos, res->arg1, res->arg2, res->arg3);
1264
1265 }
1266
1267 prog_char str_arm_goto_arg0[] = "larm_goto#rarm_goto";
1268 parse_pgm_token_string_t cmd_arm_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_result, arg0, str_arm_goto_arg0);
1269 parse_pgm_token_num_t cmd_arm_goto_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg1, INT32);
1270 parse_pgm_token_num_t cmd_arm_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg2, UINT16);
1271 parse_pgm_token_num_t cmd_arm_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg3, UINT16);
1272
1273 prog_char help_arm_goto[] = "Change arm position (shoulder, elbow, wrist)";
1274 parse_pgm_inst_t cmd_arm_goto = {
1275         .f = cmd_arm_goto_parsed,  /* function to call */
1276         .data = NULL,      /* 2nd arg of func */
1277         .help_str = help_arm_goto,
1278         .tokens = {        /* token list, NULL terminated */
1279                 (prog_void *)&cmd_arm_goto_arg0,
1280                 (prog_void *)&cmd_arm_goto_arg1,
1281                 (prog_void *)&cmd_arm_goto_arg2,
1282                 (prog_void *)&cmd_arm_goto_arg3,
1283                 NULL,
1284         },
1285 };
1286
1287 /**********************************************************/
1288
1289 /* this structure is filled when cmd_arm_capture is parsed successfully */
1290 struct cmd_arm_capture_result {
1291         fixed_string_t arg0;
1292 };
1293
1294 /* function called when cmd_arm_capture is parsed successfully */
1295 static void cmd_arm_capture_parsed(void * parsed_result, void * data)
1296 {
1297         struct cmd_arm_goto_result *res = parsed_result;
1298         uint16_t elbow, wrist;
1299         int32_t shoulder;
1300         uint8_t ret = 0;
1301         Arm_Pos *arm_pos;
1302
1303         if (!strcmp_P(res->arg0, "rarm_capture"))
1304                 arm_pos = &arm_pos_r;
1305         else
1306                 arm_pos = &arm_pos_l;
1307
1308
1309         ret |= AX12_read_int(&ax12, arm_pos->ELBOW_AX12, AA_PRESENT_POSITION_L, &elbow);
1310         ret |= AX12_read_int(&ax12, arm_pos->WRIST_AX12, AA_PRESENT_POSITION_L, &wrist);
1311         shoulder = encoders_microb_get_value((void *)ARM_ENC);
1312         if (ret)
1313                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
1314         printf_P(PSTR("%ld %d %d\r\n"), shoulder, elbow, wrist);
1315 }
1316
1317 prog_char str_arm_capture_arg0[] = "rarm_capture#larm_capture";
1318 parse_pgm_token_string_t cmd_arm_capture_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_capture_result, arg0, str_arm_capture_arg0);
1319
1320 prog_char help_arm_capture[] = "Change arm position (shoulder, elbow, wrist)";
1321 parse_pgm_inst_t cmd_arm_capture = {
1322         .f = cmd_arm_capture_parsed,  /* function to call */
1323         .data = NULL,      /* 2nd arg of func */
1324         .help_str = help_arm_capture,
1325         .tokens = {        /* token list, NULL terminated */
1326                 (prog_void *)&cmd_arm_capture_arg0,
1327                 NULL,
1328         },
1329 };
1330 /**********************************************************/
1331 /* Uint16 */
1332
1333
1334 /* this structure is filled when cmd_uint16_read is parsed successfully */
1335 struct cmd_uint16_result {
1336         fixed_string_t arg0;
1337         fixed_string_t arg1;
1338         uint8_t num;
1339         uint16_t val;
1340 };
1341
1342 /* function called when cmd_uint16_read is parsed successfully */
1343 static void cmd_uint16_read_parsed(void * parsed_result, void * data)
1344 {
1345         struct cmd_uint16_result *res = parsed_result;
1346         uint8_t ret;
1347         uint16_t val;
1348         uint8_t addr = addr_from_string(res->arg1);
1349         ret = AX12_read_int(&ax12, res->num, addr, &val);
1350         if (ret)
1351                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
1352         printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val);
1353 }
1354
1355 prog_char str_uint16_arg0[] = "read";
1356 parse_pgm_token_string_t cmd_uint16_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0);
1357 prog_char str_uint16_arg1[] = "moving_speed#model#goal_pos#cw_angle_limit#ccw_angle_limit#"
1358                 "max_torque#down_calibration#up_calibration#torque_limit#"
1359                 "position#speed#load#punch";
1360 parse_pgm_token_string_t cmd_uint16_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1);
1361 parse_pgm_token_num_t cmd_uint16_num = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, num, UINT8);
1362
1363 prog_char help_uint16_read[] = "Read uint16 value (type, num)";
1364 parse_pgm_inst_t cmd_uint16_read = {
1365         .f = cmd_uint16_read_parsed,  /* function to call */
1366         .data = NULL,      /* 2nd arg of func */
1367         .help_str = help_uint16_read,
1368         .tokens = {        /* token list, NULL terminated */
1369                 (prog_void *)&cmd_uint16_arg0,
1370                 (prog_void *)&cmd_uint16_arg1,
1371                 (prog_void *)&cmd_uint16_num,
1372                 NULL,
1373         },
1374 };
1375
1376 /* function called when cmd_uint16_write is parsed successfully */
1377 static void cmd_uint16_write_parsed(void * parsed_result, void * data)
1378 {
1379         struct cmd_uint16_result *res = parsed_result;
1380         uint8_t ret;
1381         uint8_t addr = addr_from_string(res->arg1);
1382         printf_P(PSTR("writing %s: %d [0x%.4x]\r\n"), res->arg1,
1383                  res->val, res->val);
1384         ret = AX12_write_int(&ax12, res->num, addr, res->val);
1385         if (ret)
1386                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
1387 }
1388
1389 prog_char str_uint16_arg0_w[] = "write";
1390 parse_pgm_token_string_t cmd_uint16_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg0, str_uint16_arg0_w);
1391 prog_char str_uint16_arg1_w[] = "moving_speed#goal_pos#cw_angle_limit#ccw_angle_limit#"
1392                 "max_torque#torque_limit#punch";
1393 parse_pgm_token_string_t cmd_uint16_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint16_result, arg1, str_uint16_arg1_w);
1394 parse_pgm_token_num_t cmd_uint16_val = TOKEN_NUM_INITIALIZER(struct cmd_uint16_result, val, UINT16);
1395
1396 prog_char help_uint16_write[] = "Write uint16 value (write, num, val)";
1397 parse_pgm_inst_t cmd_uint16_write = {
1398         .f = cmd_uint16_write_parsed,  /* function to call */
1399         .data = NULL,      /* 2nd arg of func */
1400         .help_str = help_uint16_write,
1401         .tokens = {        /* token list, NULL terminated */
1402                 (prog_void *)&cmd_uint16_arg0_w,
1403                 (prog_void *)&cmd_uint16_arg1_w,
1404                 (prog_void *)&cmd_uint16_num,
1405                 (prog_void *)&cmd_uint16_val,
1406                 NULL,
1407         },
1408 };
1409
1410 /**********************************************************/
1411 /* Uint8 */
1412
1413
1414 /* this structure is filled when cmd_uint8_read is parsed successfully */
1415 struct cmd_uint8_result {
1416         fixed_string_t arg0;
1417         fixed_string_t arg1;
1418         uint8_t num;
1419         uint8_t val;
1420 };
1421
1422 /* function called when cmd_uint8_read is parsed successfully */
1423 static void cmd_uint8_read_parsed(void * parsed_result, void * data)
1424 {
1425         struct cmd_uint8_result *res = parsed_result;
1426         uint8_t ret;
1427         uint8_t val;
1428         uint8_t addr = addr_from_string(res->arg1);
1429
1430         ret = AX12_read_byte(&ax12, res->num, addr, &val);
1431         if (ret)
1432                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
1433         printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val);
1434 }
1435
1436 prog_char str_uint8_arg0[] = "read";
1437 parse_pgm_token_string_t cmd_uint8_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0);
1438 prog_char str_uint8_arg1[] = "id#firmware#baudrate#delay#high_lim_temp#"
1439                 "low_lim_volt#high_lim_volt#status_return#alarm_led#"
1440                 "alarm_shutdown#torque_enable#led#cw_comp_margin#"
1441                 "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
1442                 "voltage#temp#reginst#moving#lock";
1443 parse_pgm_token_string_t cmd_uint8_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1);
1444 parse_pgm_token_num_t cmd_uint8_num = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, num, UINT8);
1445
1446 prog_char help_uint8_read[] = "Read uint8 value (type, num)";
1447 parse_pgm_inst_t cmd_uint8_read = {
1448         .f = cmd_uint8_read_parsed,  /* function to call */
1449         .data = NULL,      /* 2nd arg of func */
1450         .help_str = help_uint8_read,
1451         .tokens = {        /* token list, NULL terminated */
1452                 (prog_void *)&cmd_uint8_arg0,
1453                 (prog_void *)&cmd_uint8_arg1,
1454                 (prog_void *)&cmd_uint8_num,
1455                 NULL,
1456         },
1457 };
1458
1459 /* function called when cmd_uint8_write is parsed successfully */
1460 static void cmd_uint8_write_parsed(void * parsed_result, void * data)
1461 {
1462         struct cmd_uint8_result *res = parsed_result;
1463         uint8_t addr = addr_from_string(res->arg1);
1464         uint8_t ret;
1465         printf_P(PSTR("writing %s: %d [0x%.2x]\r\n"), res->arg1,
1466                  res->val, res->val);
1467         ret = AX12_write_byte(&ax12, res->num, addr, res->val);
1468         if (ret)
1469                 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
1470 }
1471
1472 prog_char str_uint8_arg0_w[] = "write";
1473 parse_pgm_token_string_t cmd_uint8_arg0_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg0, str_uint8_arg0_w);
1474 prog_char str_uint8_arg1_w[] = "id#baudrate#delay#high_lim_temp#"
1475                 "low_lim_volt#high_lim_volt#status_return#alarm_led#"
1476                 "alarm_shutdown#torque_enable#led#cw_comp_margin#"
1477                 "ccw_comp_margin#cw_comp_slope#ccw_comp_slope#"
1478                 "reginst#lock";
1479 parse_pgm_token_string_t cmd_uint8_arg1_w = TOKEN_STRING_INITIALIZER(struct cmd_uint8_result, arg1, str_uint8_arg1_w);
1480 parse_pgm_token_num_t cmd_uint8_val = TOKEN_NUM_INITIALIZER(struct cmd_uint8_result, val, UINT8);
1481
1482 prog_char help_uint8_write[] = "Write uint8 value (write, num, val)";
1483 parse_pgm_inst_t cmd_uint8_write = {
1484         .f = cmd_uint8_write_parsed,  /* function to call */
1485         .data = NULL,      /* 2nd arg of func */
1486         .help_str = help_uint8_write,
1487         .tokens = {        /* token list, NULL terminated */
1488                 (prog_void *)&cmd_uint8_arg0_w,
1489                 (prog_void *)&cmd_uint8_arg1_w,
1490                 (prog_void *)&cmd_uint8_num,
1491                 (prog_void *)&cmd_uint8_val,
1492                 NULL,
1493         },
1494 };
1495
1496 /**********************************************************/
1497 /* Encoders tests */
1498
1499 /* this structure is filled when cmd_encoders is parsed successfully */
1500 struct cmd_encoders_result {
1501         fixed_string_t arg0;
1502         fixed_string_t arg1;
1503 };
1504
1505 /* function called when cmd_encoders is parsed successfully */
1506 static void cmd_encoders_parsed(void * parsed_result, void * data)
1507 {
1508         while(uart_recv_nowait(0) == -1) {
1509                 printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"),
1510                          encoders_microb_get_value((void *)0),
1511                          encoders_microb_get_value((void *)1),
1512                          encoders_microb_get_value((void *)2),
1513                          encoders_microb_get_value((void *)3));
1514                 wait_ms(100);
1515         }
1516 }
1517
1518 prog_char str_encoders_arg0[] = "encoders";
1519 parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0);
1520 prog_char str_encoders_arg1[] = "show";
1521 parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1);
1522
1523 prog_char help_encoders[] = "Show encoders values";
1524 parse_pgm_inst_t cmd_encoders = {
1525         .f = cmd_encoders_parsed,  /* function to call */
1526         .data = NULL,      /* 2nd arg of func */
1527         .help_str = help_encoders,
1528         .tokens = {        /* token list, NULL terminated */
1529                 (prog_void *)&cmd_encoders_arg0,
1530                 (prog_void *)&cmd_encoders_arg1,
1531                 NULL,
1532         },
1533 };
1534
1535 /**********************************************************/
1536 /* Pwms tests */
1537
1538 /* this structure is filled when cmd_pwm is parsed successfully */
1539 struct cmd_pwm_result {
1540         fixed_string_t arg0;
1541         fixed_string_t arg1;
1542         int16_t arg2;
1543 };
1544
1545 /* function called when cmd_pwm is parsed successfully */
1546 static void cmd_pwm_parsed(void * parsed_result, void * data)
1547 {
1548         void * pwm_ptr = NULL;
1549         struct cmd_pwm_result * res = parsed_result;
1550
1551         DDRB |= 0x84;
1552
1553         if (!strcmp_P(res->arg1, PSTR("1A")))
1554                 pwm_ptr = &arm.pwm1A;
1555         else if (!strcmp_P(res->arg1, PSTR("1B")))
1556                 pwm_ptr = &arm.pwm1B;
1557         else if (!strcmp_P(res->arg1, PSTR("3C")))
1558                 pwm_ptr = &arm.pwm3C;
1559         else if (!strcmp_P(res->arg1, PSTR("2"))) {
1560                 if (res->arg2 == 0)
1561                         PORTB &= ~0x84;
1562                 if (res->arg2 == 1) {
1563                         PORTB &= ~0x80;
1564                         PORTB |= 0x4;
1565                 }
1566                 if (res->arg2 == 0) {
1567                         PORTB &= ~0x04;
1568                         PORTB |= 0x80;
1569                 }
1570                 if (res->arg2 == 0)
1571                         PORTB |= 0x84;
1572
1573                 //pwm_ptr = &arm.pwm2;
1574
1575         }
1576
1577         if (pwm_ptr)
1578                 pwm_ng_set(pwm_ptr, res->arg2);
1579
1580         printf_P(PSTR("done\r\n"));
1581 }
1582
1583 prog_char str_pwm_arg0[] = "pwm";
1584 parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0);
1585 prog_char str_pwm_arg1[] = "1A#1B#3C#2";
1586 parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1);
1587 parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16);
1588
1589 prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]";
1590 parse_pgm_inst_t cmd_pwm = {
1591         .f = cmd_pwm_parsed,  /* function to call */
1592         .data = NULL,      /* 2nd arg of func */
1593         .help_str = help_pwm,
1594         .tokens = {        /* token list, NULL terminated */
1595                 (prog_void *)&cmd_pwm_arg0,
1596                 (prog_void *)&cmd_pwm_arg1,
1597                 (prog_void *)&cmd_pwm_arg2,
1598                 NULL,
1599         },
1600 };
1601
1602 /**********************************************************/
1603 /* Gains for control system */
1604
1605 /* this structure is filled when cmd_gain is parsed successfully */
1606 struct cmd_gain_result {
1607         fixed_string_t arg0;
1608         fixed_string_t arg1;
1609         int16_t p;
1610         int16_t i;
1611         int16_t d;
1612 };
1613
1614 /* function called when cmd_gain is parsed successfully */
1615 static void cmd_gain_parsed(void * parsed_result, void * data)
1616 {
1617         struct cmd_gain_result * res = parsed_result;
1618
1619         if (!strcmp_P(res->arg1, PSTR("arm"))) {
1620                 pid_set_gains(&arm.pid_mot, res->p, res->i, res->d);
1621         }
1622         /* else it is a "show" */
1623
1624         printf_P(PSTR("arm %d %d %d\r\n"),
1625                  pid_get_gain_P(&arm.pid_mot),
1626                  pid_get_gain_I(&arm.pid_mot),
1627                  pid_get_gain_D(&arm.pid_mot));
1628 }
1629
1630 prog_char str_gain_arg0[] = "gain";
1631 parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, arg0, str_gain_arg0);
1632 prog_char str_gain_arg1[] = "arm";
1633 parse_pgm_token_string_t cmd_gain_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, arg1, str_gain_arg1);
1634 parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16);
1635 parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16);
1636 parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16);
1637
1638 prog_char help_gain[] = "Set gain values for PID";
1639 parse_pgm_inst_t cmd_gain = {
1640         .f = cmd_gain_parsed,  /* function to call */
1641         .data = NULL,      /* 2nd arg of func */
1642         .help_str = help_gain,
1643         .tokens = {        /* token list, NULL terminated */
1644                 (prog_void *)&cmd_gain_arg0,
1645                 (prog_void *)&cmd_gain_arg1,
1646                 (prog_void *)&cmd_gain_p,
1647                 (prog_void *)&cmd_gain_i,
1648                 (prog_void *)&cmd_gain_d,
1649                 NULL,
1650         },
1651 };
1652
1653 /* show */
1654
1655 prog_char str_gain_show_arg[] = "show";
1656 parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, arg1, str_gain_show_arg);
1657
1658 prog_char help_gain_show[] = "Show gain values for PID";
1659 parse_pgm_inst_t cmd_gain_show = {
1660         .f = cmd_gain_parsed,  /* function to call */
1661         .data = NULL,      /* 2nd arg of func */
1662         .help_str = help_gain_show,
1663         .tokens = {        /* token list, NULL terminated */
1664                 (prog_void *)&cmd_gain_arg0,
1665                 (prog_void *)&cmd_gain_show_arg,
1666                 NULL,
1667         },
1668 };
1669
1670
1671 /**********************************************************/
1672 /* Speeds for control system */
1673
1674 /* this structure is filled when cmd_speed is parsed successfully */
1675 struct cmd_speed_result {
1676         fixed_string_t arg0;
1677         fixed_string_t arg1;
1678         uint16_t s;
1679 };
1680
1681 /* function called when cmd_speed is parsed successfully */
1682 static void cmd_speed_parsed(void * parsed_result, void * data)
1683 {
1684 #if 0
1685         struct cmd_speed_result * res = parsed_result;
1686
1687         if (!strcmp_P(res->arg1, PSTR("arm"))) {
1688                 ramp_set_vars(&ext.r_b, res->s, res->s); /* set speed */
1689         }
1690
1691         printf_P(PSTR("arm %lu\r\n"),
1692                  ext.r_b.var_pos);
1693 #else
1694         printf_P(PSTR("DISABLED FOR NOW\r\n"));
1695 #endif
1696 }
1697
1698 prog_char str_speed_arg0[] = "speed";
1699 parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, arg0, str_speed_arg0);
1700 prog_char str_speed_arg1[] = "arm#show";
1701 parse_pgm_token_string_t cmd_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, arg1, str_speed_arg1);
1702 parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16);
1703
1704 prog_char help_speed[] = "Set speed values for ramp filter";
1705 parse_pgm_inst_t cmd_speed = {
1706         .f = cmd_speed_parsed,  /* function to call */
1707         .data = NULL,      /* 2nd arg of func */
1708         .help_str = help_speed,
1709         .tokens = {        /* token list, NULL terminated */
1710                 (prog_void *)&cmd_speed_arg0,
1711                 (prog_void *)&cmd_speed_arg1,
1712                 (prog_void *)&cmd_speed_s,
1713                 NULL,
1714         },
1715 };
1716
1717 /* show */
1718
1719 prog_char str_speed_show_arg[] = "show";
1720 parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, arg1, str_speed_show_arg);
1721
1722 prog_char help_speed_show[] = "Show speed values for ramp filter";
1723 parse_pgm_inst_t cmd_speed_show = {
1724         .f = cmd_speed_parsed,  /* function to call */
1725         .data = NULL,      /* 2nd arg of func */
1726         .help_str = help_speed_show,
1727         .tokens = {        /* token list, NULL terminated */
1728                 (prog_void *)&cmd_speed_arg0,
1729                 (prog_void *)&cmd_speed_show_arg,
1730                 NULL,
1731         },
1732 };
1733
1734
1735 /**********************************************************/
1736 /* Pos for control system */
1737
1738 /* this structure is filled when cmd_pos is parsed successfully */
1739 struct cmd_pos_result {
1740         fixed_string_t arg0;
1741         fixed_string_t arg1;
1742         int32_t p;
1743 };
1744
1745 /* function called when cmd_pos is parsed successfully */
1746 static void cmd_pos_parsed(void * parsed_result, void * data)
1747 {
1748         struct cmd_pos_result * res = parsed_result;
1749         //      uint8_t i;
1750
1751         if (!strcmp_P(res->arg1, PSTR("arm"))) {
1752                 cs_set_consign(&arm.cs_mot, res->p);
1753         }
1754
1755 #if 0
1756         for (i=0; i<50; i++) {
1757                 printf("%ld %ld %ld\r\n",
1758                        pid_get_value_in(&arm.pid_mot),
1759                        pid_get_value_out(&arm.pid_mot),
1760                        pid_get_value_D(&arm.pid_mot)
1761                        );
1762         }
1763 #endif
1764 }
1765
1766 prog_char str_pos_arg0[] = "pos";
1767 parse_pgm_token_string_t cmd_pos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pos_result, arg0, str_pos_arg0);
1768 prog_char str_pos_arg1[] = "arm";
1769 parse_pgm_token_string_t cmd_pos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pos_result, arg1, str_pos_arg1);
1770 parse_pgm_token_num_t cmd_pos_p = TOKEN_NUM_INITIALIZER(struct cmd_pos_result, p, INT32);
1771
1772 prog_char help_pos[] = "Set pos value";
1773 parse_pgm_inst_t cmd_pos = {
1774         .f = cmd_pos_parsed,  /* function to call */
1775         .data = NULL,      /* 2nd arg of func */
1776         .help_str = help_pos,
1777         .tokens = {        /* token list, NULL terminated */
1778                 (prog_void *)&cmd_pos_arg0,
1779                 (prog_void *)&cmd_pos_arg1,
1780                 (prog_void *)&cmd_pos_p,
1781                 NULL,
1782         },
1783 };
1784
1785
1786 /**********************************************************/
1787 /* Events on/off */
1788
1789 /* this structure is filled when cmd_event is parsed successfully */
1790 struct cmd_event_result {
1791         fixed_string_t arg0;
1792         fixed_string_t arg1;
1793         fixed_string_t arg2;
1794 };
1795
1796
1797 void init_arm(void)
1798 {
1799         uint32_t shoulder_robot;
1800         uint16_t elbow_robot, wrist_robot;
1801         double shoulder_rad, elbow_rad;
1802         int32_t h, d;
1803
1804         /* init arm xy pos */
1805         shoulder_robot = encoders_microb_get_value(ARM_ENC);
1806         AX12_read_int(&ax12, R_ELBOW_AX12, AA_PRESENT_POSITION_L, &elbow_robot);
1807         AX12_read_int(&ax12, R_WRIST_AX12, AA_PRESENT_POSITION_L, &wrist_robot);
1808
1809         AX12_write_int(&ax12, R_ELBOW_AX12, AA_MOVING_SPEED_L, 0x3ff);
1810         AX12_write_int(&ax12, R_WRIST_AX12, AA_MOVING_SPEED_L, 0x3ff);
1811
1812
1813         arm_pos_r.wrist_angle_deg2robot = wrist_angle_deg2robot_r;
1814         arm_pos_l.wrist_angle_deg2robot = wrist_angle_deg2robot_l;
1815
1816         arm_pos_r.angle_rad2robot = angle_rad2robot_r;
1817         arm_pos_l.angle_rad2robot = angle_rad2robot_l;
1818
1819         arm_pos_r.angle_robot2rad = angle_robot2rad_r;
1820         arm_pos_l.angle_robot2rad = angle_robot2rad_l;
1821
1822         arm_pos_r.ELBOW_AX12 = R_ELBOW_AX12;
1823         arm_pos_l.ELBOW_AX12 = L_ELBOW_AX12;
1824
1825         arm_pos_r.WRIST_AX12 = R_WRIST_AX12;
1826         arm_pos_l.WRIST_AX12 = L_WRIST_AX12;
1827
1828
1829         arm_pos_r.angle_robot2rad(shoulder_robot, elbow_robot,
1830                                    &shoulder_rad, &elbow_rad);
1831         angle2cart(shoulder_rad, elbow_rad, &h, &d);
1832         printf("init: h:%ld d:%ld w:%d\r\n", h, d, wrist_robot);
1833
1834         arm_pos_r.h = h;
1835         arm_pos_r.d = d;
1836         arm_pos_r.w = wrist_robot;
1837         arm_pos_r.goal_h = h;
1838         arm_pos_r.goal_d = d;
1839         arm_pos_r.goal_w = wrist_robot;
1840         arm_pos_r.state = ARM_STATE_IN_POS;
1841
1842
1843         arm_pos_l.h = h;
1844         arm_pos_l.d = d;
1845         arm_pos_l.w = wrist_robot;
1846         arm_pos_l.goal_h = h;
1847         arm_pos_l.goal_d = d;
1848         arm_pos_l.goal_w = wrist_robot;
1849         arm_pos_l.state = ARM_STATE_IN_POS;
1850
1851
1852
1853 }
1854
1855
1856 /* function called when cmd_event is parsed successfully */
1857 static void cmd_event_parsed(void * parsed_result, void * data)
1858 {
1859         u08 bit=0;
1860         struct cmd_event_result * res = parsed_result;
1861
1862
1863
1864         if (!strcmp_P(res->arg1, PSTR("cs"))) {
1865                 if (!strcmp_P(res->arg2, PSTR("on"))) {
1866                         pwm_ng_set(ARM_MOT_PWM, 0);
1867                         printf_P(PSTR("ax12 will start\r\n"));
1868                         while(uart_recv_nowait(0) == -1);
1869                         AX12_write_int(&ax12,0xFE,AA_TORQUE_ENABLE,0x1);
1870                         AX12_write_int(&ax12, R_ELBOW_AX12, AA_GOAL_POSITION_L, 660);
1871                         AX12_write_int(&ax12, R_WRIST_AX12, AA_GOAL_POSITION_L, 613);
1872                         printf_P(PSTR("Set the arm to 0\r\n"));
1873                         while(uart_recv_nowait(0) == -1);
1874                         encoders_microb_set_value(ARM_ENC, 0);
1875
1876
1877                         printf_P(PSTR("Set scanner to 0\r\n"));
1878                         while(uart_recv_nowait(0) == -1);
1879                         encoders_microb_set_value(SCANNER_ENC, 0);
1880                         scanner.flags |= CS_ON;
1881
1882                         init_arm();
1883
1884
1885                 }
1886                 bit = CS_ON;
1887         }
1888 /*      else if (!strcmp_P(res->arg1, PSTR("catapult"))) */
1889 /*              bit = CATAPULT_CS_ON; */
1890
1891         if (!strcmp_P(res->arg2, PSTR("on")))
1892                 arm.flags |= bit;
1893         else if (!strcmp_P(res->arg2, PSTR("off")))
1894                 arm.flags &= (~bit);
1895
1896         printf_P(PSTR("%s is %s\r\n"), res->arg1,
1897                       (bit & arm.flags) ? "on":"off");
1898 }
1899
1900 prog_char str_event_arg0[] = "event";
1901 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg0);
1902 prog_char str_event_arg1[] = "cs";
1903 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
1904 prog_char str_event_arg2[] = "on#off#show";
1905 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
1906
1907 prog_char help_event[] = "Enable/disable events";
1908 parse_pgm_inst_t cmd_event = {
1909         .f = cmd_event_parsed,  /* function to call */
1910         .data = NULL,      /* 2nd arg of func */
1911         .help_str = help_event,
1912         .tokens = {        /* token list, NULL terminated */
1913                 (prog_void *)&cmd_event_arg0,
1914                 (prog_void *)&cmd_event_arg1,
1915                 (prog_void *)&cmd_event_arg2,
1916                 NULL,
1917         },
1918 };
1919
1920 /**********************************************************/
1921 /* Maximums for control system */
1922
1923 /* this structure is filled when cmd_maximum is parsed successfully */
1924 struct cmd_maximum_result {
1925         fixed_string_t arg0;
1926         fixed_string_t arg1;
1927         uint32_t in;
1928         uint32_t i;
1929         uint32_t out;
1930 };
1931
1932 /* function called when cmd_maximum is parsed successfully */
1933 static void cmd_maximum_parsed(void * parsed_result, void * data)
1934 {
1935         struct cmd_maximum_result * res = parsed_result;
1936
1937         if (!strcmp_P(res->arg1, PSTR("arm"))) {
1938                 pid_set_maximums(&arm.pid_mot, res->in, res->i, res->out);
1939         }
1940         /* else it is a "show" */
1941
1942         printf_P(PSTR("maximum arm %lu %lu %lu\r\n"),
1943                  pid_get_max_in(&arm.pid_mot),
1944                  pid_get_max_I(&arm.pid_mot),
1945                  pid_get_max_out(&arm.pid_mot));
1946 }
1947
1948 prog_char str_maximum_arg0[] = "maximum";
1949 parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, arg0, str_maximum_arg0);
1950 prog_char str_maximum_arg1[] = "arm";
1951 parse_pgm_token_string_t cmd_maximum_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, arg1, str_maximum_arg1);
1952 parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32);
1953 parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32);
1954 parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32);
1955
1956 prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
1957 parse_pgm_inst_t cmd_maximum = {
1958         .f = cmd_maximum_parsed,  /* function to call */
1959         .data = NULL,      /* 2nd arg of func */
1960         .help_str = help_maximum,
1961         .tokens = {        /* token list, NULL terminated */
1962                 (prog_void *)&cmd_maximum_arg0,
1963                 (prog_void *)&cmd_maximum_arg1,
1964                 (prog_void *)&cmd_maximum_in,
1965                 (prog_void *)&cmd_maximum_i,
1966                 (prog_void *)&cmd_maximum_out,
1967                 NULL,
1968         },
1969 };
1970
1971 /* show */
1972
1973 prog_char str_maximum_show_arg[] = "show";
1974 parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, arg1, str_maximum_show_arg);
1975
1976 prog_char help_maximum_show[] = "Show maximum values for PID";
1977 parse_pgm_inst_t cmd_maximum_show = {
1978         .f = cmd_maximum_parsed,  /* function to call */
1979         .data = NULL,      /* 2nd arg of func */
1980         .help_str = help_maximum_show,
1981         .tokens = {        /* token list, NULL terminated */
1982                 (prog_void *)&cmd_maximum_arg0,
1983                 (prog_void *)&cmd_maximum_show_arg,
1984                 NULL,
1985         },
1986 };
1987
1988 /**********************************************************/
1989 /* Quadramp for control system */
1990
1991 /* this structure is filled when cmd_quadramp is parsed successfully */
1992 struct cmd_quadramp_result {
1993         fixed_string_t arg0;
1994         fixed_string_t arg1;
1995         uint32_t ap;
1996         uint32_t an;
1997         uint32_t sp;
1998         uint32_t sn;
1999 };
2000
2001 /* function called when cmd_quadramp is parsed successfully */
2002 static void cmd_quadramp_parsed(void * parsed_result, void * data)
2003 {
2004         struct cmd_quadramp_result * res = parsed_result;
2005
2006         if (!strcmp_P(res->arg1, PSTR("arm"))) {
2007                 quadramp_set_1st_order_vars(&arm.qr_mot, res->sp, res->sn);
2008                 quadramp_set_2nd_order_vars(&arm.qr_mot, res->ap, res->an);
2009         }
2010 /*      else if (!strcmp_P(res->arg1, PSTR("distance"))) { */
2011 /*              quadramp_set_1st_order_vars(&arm.qr_d, res->sp, res->sn); */
2012 /*              quadramp_set_2nd_order_vars(&arm.qr_d, res->ap, res->an); */
2013 /*      } */
2014         /* else it's a "show" */
2015
2016         printf_P(PSTR("quadramp arm %ld %ld %ld %ld\r\n"),
2017                  arm.qr_mot.var_2nd_ord_pos,
2018                  arm.qr_mot.var_2nd_ord_neg,
2019                  arm.qr_mot.var_1st_ord_pos,
2020                  arm.qr_mot.var_1st_ord_neg);
2021 }
2022
2023 prog_char str_quadramp_arg0[] = "quadramp";
2024 parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, arg0, str_quadramp_arg0);
2025 prog_char str_quadramp_arg1[] = "arm";
2026 parse_pgm_token_string_t cmd_quadramp_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, arg1, str_quadramp_arg1);
2027 parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32);
2028 parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32);
2029 parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32);
2030 parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32);
2031
2032 prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
2033 parse_pgm_inst_t cmd_quadramp = {
2034         .f = cmd_quadramp_parsed,  /* function to call */
2035         .data = NULL,      /* 2nd arg of func */
2036         .help_str = help_quadramp,
2037         .tokens = {        /* token list, NULL terminated */
2038                 (prog_void *)&cmd_quadramp_arg0,
2039                 (prog_void *)&cmd_quadramp_arg1,
2040                 (prog_void *)&cmd_quadramp_ap,
2041                 (prog_void *)&cmd_quadramp_an,
2042                 (prog_void *)&cmd_quadramp_sp,
2043                 (prog_void *)&cmd_quadramp_sn,
2044
2045                 NULL,
2046         },
2047 };
2048
2049 /* show */
2050
2051 prog_char str_quadramp_show_arg[] = "show";
2052 parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, arg1, str_quadramp_show_arg);
2053
2054 prog_char help_quadramp_show[] = "Get quadramp values for control system";
2055 parse_pgm_inst_t cmd_quadramp_show = {
2056         .f = cmd_quadramp_parsed,  /* function to call */
2057         .data = NULL,      /* 2nd arg of func */
2058         .help_str = help_quadramp_show,
2059         .tokens = {        /* token list, NULL terminated */
2060                 (prog_void *)&cmd_quadramp_arg0,
2061                 (prog_void *)&cmd_quadramp_show_arg,
2062                 NULL,
2063         },
2064 };
2065
2066
2067 /**********************************************************/
2068 /* sample ADC */
2069 extern uint16_t sample_i;
2070 //extern uint16_t sample_tab[MAX_SAMPLE];
2071 /* this structure is filled when cmd_sample is parsed successfully */
2072 struct cmd_sample_result {
2073         fixed_string_t arg0;
2074         fixed_string_t arg1;
2075         uint16_t offset_a;
2076 };
2077
2078 extern int32_t pos_start_scan;
2079 int32_t scan_frwd = 0;
2080 /* function called when cmd_sample is parsed successfully */
2081 static void cmd_sample_parsed(void * parsed_result, void * data)
2082 {
2083         struct cmd_sample_result * res = parsed_result;
2084         uint16_t i;
2085         scan_frwd++;
2086         //int32_t cs_err, cs_out, cs_in;
2087
2088         printf_P(PSTR("cmd sample called!\r\n"));
2089         printf_P(PSTR("arg %s %d!\r\n"), res->arg1, res->offset_a);
2090
2091         offset_a = (((float)res->offset_a)*M_PI/180.);
2092
2093         if (!strcmp_P(res->arg1, PSTR("start"))) {
2094                 sample_i = MAX_SAMPLE;
2095                 pos_start_scan = encoders_microb_get_value(SCANNER_ENC);
2096
2097                 memset(sample_tab, 0xff, MAX_SAMPLE*sizeof(uint8_t));
2098
2099
2100                 //encoders_microb_set_value(SCANNER_ENC, 0);
2101                 cs_set_consign(&scanner.cs_mot, pos_start_scan+SCANNER_STEP_TOUR*200L);
2102
2103                 last_tour_n = 0;
2104                 last_tour_pos = 0;
2105
2106                 /*
2107                 while (uart_recv_nowait(0)==-1){
2108                         wait_ms(200);
2109                         cs_err = cs_get_error(&scanner.cs_mot);
2110                         cs_out = cs_get_out(&scanner.cs_mot);
2111                         cs_in = cs_get_filtered_feedback(&scanner.cs_mot);
2112                         printf_P(PSTR("err: %ld out: %ld in: %ld\r\n"), cs_err, cs_out, cs_in);
2113
2114                 }
2115                 */
2116
2117         }
2118
2119
2120
2121
2122         else if (!strcmp_P(res->arg1, PSTR("dump"))) {
2123                 printf_P(PSTR("start dumping\r\n"));
2124
2125                 for (i=0;i<MAX_SAMPLE;i++)
2126                         printf_P(PSTR("%d %d \r\n"),sample_tab[i]&0x1ff, sample_tab[i]&0x200?1:0);
2127                 printf_P(PSTR("end dumping  (pos: %ld)\r\n"),
2128                          encoders_microb_get_value((void *)SCANNER_ENC));
2129         }
2130
2131 }
2132
2133 prog_char str_sample_arg0[] = "sample";
2134 parse_pgm_token_string_t cmd_sample_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sample_result, arg0, str_sample_arg0);
2135 prog_char str_sample_arg1[] = "start#dump";
2136 parse_pgm_token_string_t cmd_sample_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_sample_result, arg1, str_sample_arg1);
2137 parse_pgm_token_num_t cmd_sample_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_sample_result, offset_a, UINT16);
2138
2139 prog_char help_sample[] = "Sample func";
2140 parse_pgm_inst_t cmd_sample = {
2141         .f = cmd_sample_parsed,  /* function to call */
2142         .data = NULL,      /* 2nd arg of func */
2143         .help_str = help_sample,
2144         .tokens = {        /* token list, NULL terminated */
2145                 (prog_void *)&cmd_sample_arg0,
2146                 (prog_void *)&cmd_sample_arg1,
2147                 (prog_void *)&cmd_sample_arg2,
2148                 NULL,
2149         },
2150 };
2151
2152
2153 /**********************************************************/
2154
2155
2156 /* in progmem */
2157 parse_pgm_ctx_t main_ctx[] = {
2158         (parse_pgm_inst_t *)&cmd_reset,
2159         (parse_pgm_inst_t *)&cmd_spi_test,
2160         (parse_pgm_inst_t *)&cmd_bootloader,
2161         (parse_pgm_inst_t *)&cmd_ax12_stress,
2162         (parse_pgm_inst_t *)&cmd_armxy,
2163         (parse_pgm_inst_t *)&cmd_arm_circ,
2164         (parse_pgm_inst_t *)&cmd_arm_harv,
2165         (parse_pgm_inst_t *)&cmd_test,
2166         (parse_pgm_inst_t *)&cmd_arm_straight,
2167         (parse_pgm_inst_t *)&cmd_baudrate,
2168         (parse_pgm_inst_t *)&cmd_arm_goto,
2169         (parse_pgm_inst_t *)&cmd_arm_capture,
2170         (parse_pgm_inst_t *)&cmd_sample,
2171         (parse_pgm_inst_t *)&cmd_uint16_read,
2172         (parse_pgm_inst_t *)&cmd_uint16_write,
2173         (parse_pgm_inst_t *)&cmd_uint8_read,
2174         (parse_pgm_inst_t *)&cmd_uint8_write,
2175         (parse_pgm_inst_t *)&cmd_encoders,
2176         (parse_pgm_inst_t *)&cmd_pwm,
2177         (parse_pgm_inst_t *)&cmd_gain,
2178         (parse_pgm_inst_t *)&cmd_gain_show,
2179         (parse_pgm_inst_t *)&cmd_speed,
2180         (parse_pgm_inst_t *)&cmd_speed_show,
2181         (parse_pgm_inst_t *)&cmd_pos,
2182         (parse_pgm_inst_t *)&cmd_event,
2183         (parse_pgm_inst_t *)&cmd_maximum,
2184         (parse_pgm_inst_t *)&cmd_maximum_show,
2185         (parse_pgm_inst_t *)&cmd_quadramp,
2186         (parse_pgm_inst_t *)&cmd_quadramp_show,
2187
2188         NULL,
2189 };