2 * Copyright Droids Corporation (2008)
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.
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.
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
18 * Revision : $Id: commands.c,v 1.6 2009-03-15 20:08:51 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
27 #include <aversive/pgmspace.h>
28 #include <aversive/wait.h>
33 #include <parse_num.h>
34 #include <parse_string.h>
36 #include <encoders_microb.h>
42 #include <control_system_manager.h>
44 #include <scheduler.h>
52 #define AX12_MAX_RETRY 2
54 uint8_t AX12_read_int_retry(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
58 for (i=0;i<AX12_MAX_RETRY;i++){
59 err = AX12_read_int(ax12, id, address, val);
64 printf("AX12: retry %d times err: %d\r\n", AX12_MAX_RETRY, err);
68 uint8_t AX12_write_int_retry(AX12 *ax12, uint8_t id, AX12_ADDRESS address,
72 for (i=0;i<AX12_MAX_RETRY;i++){
73 err = AX12_write_int(ax12, id, address,data);
78 printf("AX12: retry %d times err: %d\r\n", AX12_MAX_RETRY, err);
82 uint8_t addr_from_string(const char *s)
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")))
113 if (!strcmp_P(s, PSTR("firmware")))
115 if (!strcmp_P(s, PSTR("id")))
117 if (!strcmp_P(s, PSTR("baudrate")))
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")))
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")))
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")))
153 if (!strcmp_P(s, PSTR("lock")))
161 /**********************************************************/
164 /* this structure is filled when cmd_reset is parsed successfully */
165 struct cmd_reset_result {
169 /* function called when cmd_reset is parsed successfully */
170 static void cmd_reset_parsed(void * parsed_result, void * data)
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);
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,
189 /**********************************************************/
192 /* this structure is filled when cmd_spi_test is parsed successfully */
193 struct cmd_spi_test_result {
197 /* function called when cmd_spi_test is parsed successfully */
198 static void cmd_spi_test_parsed(void * parsed_result, void * data)
203 for (i=0; i<3; i++) {
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);
210 printf_P(PSTR("disabled\r\n"));
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);
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,
228 /**********************************************************/
231 /* this structure is filled when cmd_bootloader is parsed successfully */
232 struct cmd_bootloader_result {
236 /* function called when cmd_bootloader is parsed successfully */
237 static void cmd_bootloader_parsed(void * parsed_result, void * data)
239 #ifdef __AVR_ATmega128__
240 #define BOOTLOADER_ADDR 0x1e000
242 #define BOOTLOADER_ADDR 0x3f000
244 if (pgm_read_byte_far(BOOTLOADER_ADDR) == 0xff) {
245 printf_P(PSTR("Bootloader is not present\r\n"));
249 /* ... very specific :( */
250 #ifdef __AVR_ATmega128__
264 #ifdef __AVR_ATmega128__
265 __asm__ __volatile__ ("ldi r30,0x00\n");
266 __asm__ __volatile__ ("ldi r31,0xf0\n");
267 __asm__ __volatile__ ("ijmp\n");
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");
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);
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,
290 /**********************************************************/
293 /* this structure is filled when cmd_ax12_stress is parsed successfully */
294 struct cmd_ax12_stress_result {
301 /* function called when cmd_ax12_stress is parsed successfully */
302 static void cmd_ax12_stress_parsed(void * parsed_result, void * data)
304 uint32_t i, nb_errs = 0;
306 microseconds t = time_get_us2();
307 struct cmd_ax12_stress_result *res = parsed_result;
309 for (i=0; i<res->num; i++) {
310 if (AX12_read_byte(&ax12, res->id, AA_ID, &val) != 0)
312 if (uart_recv_nowait(0) != -1)
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);
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);
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,
341 /**********************************************************/
344 /* this structure is filled when cmd_test is parsed successfully */
345 struct cmd_test_result {
351 #define R_ELBOW_AX12 1
352 #define R_WRIST_AX12 2
354 #define L_ELBOW_AX12 4
355 #define L_WRIST_AX12 3
358 #define FINGER_AX12 5
361 void arm_l_goto(int32_t shoulder, uint16_t elbow, uint16_t wrist)
365 //printf("%ld %d %d\r\n", shoulder, elbow, wrist);
367 //cs_set_consign(&arm.cs_mot, shoulder);
368 err = AX12_write_int(&ax12, L_ELBOW_AX12, AA_GOAL_POSITION_L, elbow);
370 err = AX12_write_int(&ax12, L_WRIST_AX12, AA_GOAL_POSITION_L, wrist);
372 printf_P(PSTR("AX12 error %x !\r\n"), err);
376 void arm_goto(Arm_Pos * arm_pos, int32_t shoulder, uint16_t elbow, uint16_t wrist)
380 //printf("%ld %d %d\r\n", shoulder, elbow, wrist);
382 cs_set_consign(&arm.cs_mot, shoulder);
383 err = AX12_write_int(&ax12, arm_pos->ELBOW_AX12, AA_GOAL_POSITION_L, elbow);
385 err = AX12_write_int(&ax12, arm_pos->WRIST_AX12, AA_GOAL_POSITION_L, wrist);
387 printf_P(PSTR("AX12 error %x !\r\n"), err);
390 void arm_goto_dh(Arm_Pos* arm_pos, int32_t shoulder, uint16_t elbow)
393 cs_set_consign(&arm.cs_mot, shoulder);
394 err = AX12_write_int(&ax12, arm_pos->ELBOW_AX12, AA_GOAL_POSITION_L, elbow);
396 printf_P(PSTR("AX12 error %x !\r\n"), err);
399 void finger_right(void)
401 AX12_write_int(&ax12, FINGER_AX12, AA_GOAL_POSITION_L, 666);
404 void finger_left(void)
406 AX12_write_int(&ax12, FINGER_AX12, AA_GOAL_POSITION_L, 340);
409 void finger_center(void)
411 AX12_write_int(&ax12, FINGER_AX12, AA_GOAL_POSITION_L, 490);
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)
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)
432 #define arm_drop_v3() arm_goto(-11280, 547, 396)
433 //#define arm_drop_v4() arm_goto(-11000, 656, 699)
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)
443 struct cmd_test_result *res = parsed_result;
444 uint16_t t_w = res->arg1;
445 int16_t ppwm = res->arg2;
447 AX12_write_int(&ax12, ELBOW_AX12, AA_MOVING_SPEED_L, 1000);
450 /* Set some AX12 parameters */
451 /* err = AX12_write_int(&ax12,0xFE,AA_TORQUE_ENABLE,0x1); */
453 /* AX12_write_int(&ax12,0xFE,AA_PUNCH_L,0x20); */
455 /* AX12_write_int(&ax12,0xFE,AA_TORQUE_LIMIT_L,0x3FF); */
457 /* AX12_write_int(&ax12,0xFE,AA_MOVING_SPEED_L,0x3FF); */
459 /* AX12_write_byte(&ax12,0xFE,AA_ALARM_LED,0xEF); */
461 /* printf_P(PSTR("AX12 error %x !\r\n"), err); */
465 for (i=0; i<1; i++) {
468 pwm_ng_set(&arm.pwm1B, ppwm);
477 pwm_ng_set(&arm.pwm3C, ppwm);
486 /* arm_intermediate2(); */
490 pwm_ng_set(&arm.pwm3C, -ppwm);
492 pwm_ng_set(&arm.pwm3C, 0);
500 pwm_ng_set(&arm.pwm1B, -ppwm);
502 pwm_ng_set(&arm.pwm1B, 0);
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);
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,
529 /**********************************************************/
532 /* this structure is filled when cmd_armxy is parsed successfully */
533 struct cmd_armxy_result {
545 #define ARM_STATE_MOV 1
546 #define ARM_STATE_IN_POS 2
556 int8_t arm_do_xy_sched_update(Arm_Pos *arm_pos)
560 int32_t fin_h, fin_d;
568 double as_fin_rad, ae_fin_rad;
569 int32_t as_deg, ae_deg;
572 //printf("upt called\r\n");
573 fin_h = arm_pos->goal_h;
574 fin_d = arm_pos->goal_d;
576 arm_pos->state = ARM_STATE_MOV;
578 //quadramp_set_1st_order_vars(&arm.qr_mot, 800, 800); /* set speed */
586 ret = arm_do_step(arm_pos, &arm_pos->h, &arm_pos->d,
589 &as_fin_rad, &ae_fin_rad,
590 &next_time, &s_quad, &e_quad);
591 //printf("ret: %d\r\n", ret);
593 printf("do_step: %d arm_h %ld arm_d: %ld as: %ld ae:%ld\r\n",
595 arm_pos->h, arm_pos->d,
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),
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;
608 aw = -arm_pos->goal_w+as_deg+ae_deg;
610 /* printf("as_deg: %ld ae_deg: %ld aw deg: %ld\r\n", as_deg, ae_deg, aw); */
612 wrist_angle_deg2robot(arm_pos->goal_w,
616 /* printf("w_out: %f\r\n", wrist_out); */
619 return arm_pos->state = ret; // XXX
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);
627 arm_goto_dh(arm_pos, as, ae);
629 //wait_ms(next_time/1000);
632 //printf("set arm end\r\n");
633 arm_pos->state = ARM_STATE_IN_POS;
637 if (next_time<SCHEDULER_UNIT)
638 next_time = SCHEDULER_UNIT;
640 ret = scheduler_add_single_event_priority((void *)arm_do_xy_sched_update,
642 next_time/SCHEDULER_UNIT,
646 //printf("add event %ld\r\n", next_time/SCHEDULER_UNIT );
657 #define SPEED_NEAR_STOP 15
658 #define CS_NEAR_STEP 50
661 #define ARM_NEAR_POS 8
662 #define ARM_NEAR_SPEED0 100
664 #define CS_NEAR_POS 100
669 #define ARM_MAX_AX12_SS 1000
672 #define MOV_MAX_TIMEOUT 100L
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)
681 double as_fin_rad, ae_fin_rad, wrist_out;
682 int32_t as_deg, ae_deg, aw_deg;
687 arm_pos->goal_h = armh;
688 arm_pos->goal_d = armd;
689 arm_pos->goal_w = w_angle;
691 arm_pos->state = ARM_STATE_MOV;
694 *wrist_wait_time = 0;
696 /* calc for final pos */
697 ret = cart2angle(armh, armd, &as_fin_rad, &ae_fin_rad);
703 /*give directly final wrist pos*/
704 //AX12_write_int(&ax12, WRIST_AX12, AA_MOVING_SPEED_L, 0x3ff);
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,
711 /* calc theorical wrist movement duration */
712 ret = AX12_read_int(&ax12, arm_pos->WRIST_AX12, AA_PRESENT_POSITION_L, &wrist_cur);
714 wrist_diff = wrist_out-wrist_cur;
718 printf_P(PSTR("AX12 read pos wrist error %x !\r\n"), ret);
720 if (wrist_diff && wrist_wait_time)
721 *wrist_wait_time = (1000*ABS((int32_t)wrist_diff))/ARM_MAX_AX12_SS;
724 ret = AX12_write_int(&ax12, arm_pos->WRIST_AX12, AA_GOAL_POSITION_L, wrist_out);
726 printf_P(PSTR("AX12 wrist final pos error %x !\r\n"), ret);
728 arm_do_xy_sched_update(arm_pos);
730 while(arm_pos->state == ARM_STATE_MOV){
733 if (arm_pos->state != ARM_STATE_IN_POS)
734 printf("error wait no speed ret: %d\r\n", arm_pos->state);
740 int8_t arm_do_xy_wait_nospeed(Arm_Pos *arm_pos, int16_t armh, int16_t armd, int16_t w_angle)
745 int16_t valp1, valp2;
750 int32_t wrist_wait_time;
753 ret = arm_do_xy_nowait(arm_pos, armh, armd, w_angle, &wrist_wait_time);
755 //printf("wwt %ld\r\n", wrist_wait_time);
757 //ret = arm_do_xy(arm_pos, armh, armd, w_angle);
758 //ret = arm_do_xy_sched(arm_pos, armh, armd, w_angle);
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);
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);
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);
776 cs_err = cs_get_error(&arm.cs_mot);
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)
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",
800 /* function called when cmd_armxy is parsed successfully */
801 static void cmd_armxy_parsed(void * parsed_result, void * data)
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;
809 if (!strcmp_P(res->arg0, PSTR("rarmxy")))
810 arm_pos = &arm_pos_r;
812 arm_pos = &arm_pos_l;
815 arm_do_xy_wait_nospeed(arm_pos, armx,army, wrist_angle_deg);
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);
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,
850 /**********************************************************/
853 /* this structure is filled when cmd_arm_circ is parsed successfully */
854 struct cmd_arm_circ_result {
859 /* function called when cmd_arm_circ is parsed successfully */
860 static void cmd_arm_circ_parsed(void * parsed_result, void * data)
865 struct cmd_arm_circ_result *res = parsed_result;
867 int32_t add_h, add_d;
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);
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);
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,
900 /**********************************************************/
903 /* this structure is filled when cmd_arm_harv is parsed successfully */
904 struct cmd_arm_harv_result {
913 #define PWM_PUMP -3000
917 //#define ARM_LOW_H -150
918 #define ARM_LOW_D 210
920 #define FINGER_DELAY 200
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)
927 struct cmd_arm_harv_result *res = parsed_result;
931 wait_ms(FINGER_DELAY);
933 wait_ms(FINGER_DELAY);
935 arm_do_xy_nowait(&arm_pos_r, ARM_GET_H+10, ARM_GET_D, ANGL1, NULL);
938 wait_ms(FINGER_DELAY);
941 //arm_do_xy_wait_nospeed(&arm_pos_r, 130, 130, ANGL1);
943 arm_do_xy_wait_nospeed(&arm_pos_r, ARM_GET_H, ARM_GET_D, ANGL1);
949 pwm_ng_set(&arm.pwm1B, PWM_PUMP);
952 //self.ser.write("pwm 1B -3000\n");
955 //arm_do_xy_wait_nospeed(&arm_pos_r, -120, ARM_GET_D, ANGL1);
957 arm_do_xy_wait_nospeed(&arm_pos_r, ARM_GET_H+10, ARM_GET_D, ANGL2);
961 /* WAIT LOLO FOR COLUMN*/
966 arm_do_xy_wait_nospeed(&arm_pos_r, ARM_GET_H, ARM_GET_D, ANGL2);
967 pwm_ng_set(&arm.pwm3C, PWM_PUMP);
976 arm_do_xy_wait_nospeed(&arm_pos_r, res->hight+80,ARM_LOW_D-140,ANGL2);
978 arm_do_xy_wait_nospeed(&arm_pos_r, res->hight+80, ARM_LOW_D, ANGL2);
985 /******** MAKE COLUMN *******/
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);
991 pwm_ng_set(&arm.pwm3C, -PWM_PUMP);
996 //arm_do_xy_nowait(&arm_pos_r, res->hight+65,ARM_LOW_D,ANGL2, NULL);
999 arm_do_xy_wait_nospeed(&arm_pos_r, res->hight+65,ARM_LOW_D,ANGL1);
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);
1005 pwm_ng_set(&arm.pwm1B, -PWM_PUMP);
1009 arm_do_xy_nowait(&arm_pos_r, res->hight+70,ARM_LOW_D,ANGL1, NULL);
1012 pwm_ng_set(&arm.pwm1B, 0);
1013 pwm_ng_set(&arm.pwm3C, 0);
1016 arm_do_xy_nowait(&arm_pos_r, res->hight+80,ARM_LOW_D-140,ANGL1, NULL);
1018 arm_do_xy_nowait(&arm_pos_r, -100,80,ANGL1, NULL);
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);
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,
1043 /**********************************************************/
1046 /* this structure is filled when cmd_arm_straight is parsed successfully */
1047 struct cmd_arm_straight_result {
1048 fixed_string_t arg0;
1051 fixed_string_t arg3;
1054 /* function called when cmd_arm_straight is parsed successfully */
1055 static void cmd_arm_straight_parsed(void * parsed_result, void * data)
1057 int32_t pos_shoulder[] = {
1086 /* vitesse servo : 3ff = 114RPM = 233.2 pas/100ms */
1087 int32_t pos_elbow[] = {
1118 int32_t speed_shoulder;
1119 int32_t speed_elbow;
1124 microseconds t_start;
1126 int8_t tab_len = sizeof(pos_elbow)/sizeof(int32_t);
1128 struct cmd_arm_straight_result *res = parsed_result;
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");
1136 arm_world = strcmp("real", res->arg3);
1137 printf("speed: %ld step:%d\r\n", arm_period, arm_step);
1139 //arm_goto(pos_shoulder[0], pos_elbow[0], 500);
1140 printf("ready\r\n");
1141 while(uart_recv_nowait(0) == -1);
1143 arm_max_pos = (tab_len/arm_step)*arm_step;
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();
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;
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;
1162 printf("shoulder : %ld, %ld / elbow : %ld, %ld\r\n",
1163 pos_shoulder[i], speed_shoulder,
1164 pos_elbow[i], speed_elbow);
1167 quadramp_set_1st_order_vars(&arm.qr_mot, speed_shoulder, speed_shoulder);
1168 cs_set_consign(&arm.cs_mot, pos_shoulder[i]);
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);
1176 printf("tstop %ld\r\n", time_get_us2());
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 */
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);
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,
1207 /**********************************************************/
1209 /* this structure is filled when cmd_baudrate is parsed successfully */
1210 struct cmd_baudrate_result {
1211 fixed_string_t arg0;
1215 /* function called when cmd_baudrate is parsed successfully */
1216 static void cmd_baudrate_parsed(void * parsed_result, void * data)
1218 struct cmd_baudrate_result *res = parsed_result;
1219 struct uart_config c;
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);
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);
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,
1244 /**********************************************************/
1246 /* this structure is filled when cmd_arm_goto is parsed successfully */
1247 struct cmd_arm_goto_result {
1248 fixed_string_t arg0;
1254 /* function called when cmd_arm_goto is parsed successfully */
1255 static void cmd_arm_goto_parsed(void * parsed_result, void * data)
1257 struct cmd_arm_goto_result *res = parsed_result;
1259 if (!strcmp(res->arg0, "rarm_goto"))
1260 arm_pos = &arm_pos_r;
1262 arm_pos = &arm_pos_l;
1263 arm_goto(arm_pos, res->arg1, res->arg2, res->arg3);
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);
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,
1287 /**********************************************************/
1289 /* this structure is filled when cmd_arm_capture is parsed successfully */
1290 struct cmd_arm_capture_result {
1291 fixed_string_t arg0;
1294 /* function called when cmd_arm_capture is parsed successfully */
1295 static void cmd_arm_capture_parsed(void * parsed_result, void * data)
1297 struct cmd_arm_goto_result *res = parsed_result;
1298 uint16_t elbow, wrist;
1303 if (!strcmp_P(res->arg0, "rarm_capture"))
1304 arm_pos = &arm_pos_r;
1306 arm_pos = &arm_pos_l;
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);
1313 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
1314 printf_P(PSTR("%ld %d %d\r\n"), shoulder, elbow, wrist);
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);
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,
1330 /**********************************************************/
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;
1342 /* function called when cmd_uint16_read is parsed successfully */
1343 static void cmd_uint16_read_parsed(void * parsed_result, void * data)
1345 struct cmd_uint16_result *res = parsed_result;
1348 uint8_t addr = addr_from_string(res->arg1);
1349 ret = AX12_read_int(&ax12, res->num, addr, &val);
1351 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
1352 printf_P(PSTR("%s: %d [0x%.4x]\r\n"), res->arg1, val, val);
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);
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,
1376 /* function called when cmd_uint16_write is parsed successfully */
1377 static void cmd_uint16_write_parsed(void * parsed_result, void * data)
1379 struct cmd_uint16_result *res = parsed_result;
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);
1386 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
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);
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,
1410 /**********************************************************/
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;
1422 /* function called when cmd_uint8_read is parsed successfully */
1423 static void cmd_uint8_read_parsed(void * parsed_result, void * data)
1425 struct cmd_uint8_result *res = parsed_result;
1428 uint8_t addr = addr_from_string(res->arg1);
1430 ret = AX12_read_byte(&ax12, res->num, addr, &val);
1432 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
1433 printf_P(PSTR("%s: %d [0x%.2x]\r\n"), res->arg1, val, val);
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);
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,
1459 /* function called when cmd_uint8_write is parsed successfully */
1460 static void cmd_uint8_write_parsed(void * parsed_result, void * data)
1462 struct cmd_uint8_result *res = parsed_result;
1463 uint8_t addr = addr_from_string(res->arg1);
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);
1469 printf_P(PSTR("AX12 error %.2x!\r\n"), ret);
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#"
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);
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,
1496 /**********************************************************/
1497 /* Encoders tests */
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;
1505 /* function called when cmd_encoders is parsed successfully */
1506 static void cmd_encoders_parsed(void * parsed_result, void * data)
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));
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);
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,
1535 /**********************************************************/
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;
1545 /* function called when cmd_pwm is parsed successfully */
1546 static void cmd_pwm_parsed(void * parsed_result, void * data)
1548 void * pwm_ptr = NULL;
1549 struct cmd_pwm_result * res = parsed_result;
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"))) {
1562 if (res->arg2 == 1) {
1566 if (res->arg2 == 0) {
1573 //pwm_ptr = &arm.pwm2;
1578 pwm_ng_set(pwm_ptr, res->arg2);
1580 printf_P(PSTR("done\r\n"));
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);
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,
1602 /**********************************************************/
1603 /* Gains for control system */
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;
1614 /* function called when cmd_gain is parsed successfully */
1615 static void cmd_gain_parsed(void * parsed_result, void * data)
1617 struct cmd_gain_result * res = parsed_result;
1619 if (!strcmp_P(res->arg1, PSTR("arm"))) {
1620 pid_set_gains(&arm.pid_mot, res->p, res->i, res->d);
1622 /* else it is a "show" */
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));
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);
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,
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);
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,
1671 /**********************************************************/
1672 /* Speeds for control system */
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;
1681 /* function called when cmd_speed is parsed successfully */
1682 static void cmd_speed_parsed(void * parsed_result, void * data)
1685 struct cmd_speed_result * res = parsed_result;
1687 if (!strcmp_P(res->arg1, PSTR("arm"))) {
1688 ramp_set_vars(&ext.r_b, res->s, res->s); /* set speed */
1691 printf_P(PSTR("arm %lu\r\n"),
1694 printf_P(PSTR("DISABLED FOR NOW\r\n"));
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);
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,
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);
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,
1735 /**********************************************************/
1736 /* Pos for control system */
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;
1745 /* function called when cmd_pos is parsed successfully */
1746 static void cmd_pos_parsed(void * parsed_result, void * data)
1748 struct cmd_pos_result * res = parsed_result;
1751 if (!strcmp_P(res->arg1, PSTR("arm"))) {
1752 cs_set_consign(&arm.cs_mot, res->p);
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)
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);
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,
1786 /**********************************************************/
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;
1799 uint32_t shoulder_robot;
1800 uint16_t elbow_robot, wrist_robot;
1801 double shoulder_rad, elbow_rad;
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);
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);
1813 arm_pos_r.wrist_angle_deg2robot = wrist_angle_deg2robot_r;
1814 arm_pos_l.wrist_angle_deg2robot = wrist_angle_deg2robot_l;
1816 arm_pos_r.angle_rad2robot = angle_rad2robot_r;
1817 arm_pos_l.angle_rad2robot = angle_rad2robot_l;
1819 arm_pos_r.angle_robot2rad = angle_robot2rad_r;
1820 arm_pos_l.angle_robot2rad = angle_robot2rad_l;
1822 arm_pos_r.ELBOW_AX12 = R_ELBOW_AX12;
1823 arm_pos_l.ELBOW_AX12 = L_ELBOW_AX12;
1825 arm_pos_r.WRIST_AX12 = R_WRIST_AX12;
1826 arm_pos_l.WRIST_AX12 = L_WRIST_AX12;
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);
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;
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;
1856 /* function called when cmd_event is parsed successfully */
1857 static void cmd_event_parsed(void * parsed_result, void * data)
1860 struct cmd_event_result * res = parsed_result;
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);
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;
1888 /* else if (!strcmp_P(res->arg1, PSTR("catapult"))) */
1889 /* bit = CATAPULT_CS_ON; */
1891 if (!strcmp_P(res->arg2, PSTR("on")))
1893 else if (!strcmp_P(res->arg2, PSTR("off")))
1894 arm.flags &= (~bit);
1896 printf_P(PSTR("%s is %s\r\n"), res->arg1,
1897 (bit & arm.flags) ? "on":"off");
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);
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,
1920 /**********************************************************/
1921 /* Maximums for control system */
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;
1932 /* function called when cmd_maximum is parsed successfully */
1933 static void cmd_maximum_parsed(void * parsed_result, void * data)
1935 struct cmd_maximum_result * res = parsed_result;
1937 if (!strcmp_P(res->arg1, PSTR("arm"))) {
1938 pid_set_maximums(&arm.pid_mot, res->in, res->i, res->out);
1940 /* else it is a "show" */
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));
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);
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,
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);
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,
1988 /**********************************************************/
1989 /* Quadramp for control system */
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;
2001 /* function called when cmd_quadramp is parsed successfully */
2002 static void cmd_quadramp_parsed(void * parsed_result, void * data)
2004 struct cmd_quadramp_result * res = parsed_result;
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);
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); */
2014 /* else it's a "show" */
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);
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);
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,
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);
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,
2067 /**********************************************************/
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;
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)
2083 struct cmd_sample_result * res = parsed_result;
2086 //int32_t cs_err, cs_out, cs_in;
2088 printf_P(PSTR("cmd sample called!\r\n"));
2089 printf_P(PSTR("arg %s %d!\r\n"), res->arg1, res->offset_a);
2091 offset_a = (((float)res->offset_a)*M_PI/180.);
2093 if (!strcmp_P(res->arg1, PSTR("start"))) {
2094 sample_i = MAX_SAMPLE;
2095 pos_start_scan = encoders_microb_get_value(SCANNER_ENC);
2097 memset(sample_tab, 0xff, MAX_SAMPLE*sizeof(uint8_t));
2100 //encoders_microb_set_value(SCANNER_ENC, 0);
2101 cs_set_consign(&scanner.cs_mot, pos_start_scan+SCANNER_STEP_TOUR*200L);
2107 while (uart_recv_nowait(0)==-1){
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);
2122 else if (!strcmp_P(res->arg1, PSTR("dump"))) {
2123 printf_P(PSTR("start dumping\r\n"));
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));
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);
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,
2153 /**********************************************************/
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,