--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands_mechboard.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <blocking_detection_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "sensor.h"
+#include "cmdline.h"
+#include "state.h"
+#include "i2c_protocol.h"
+#include "actuator.h"
+#include "arm_xy.h"
+#include "arm_highlevel.h"
+#include "actuator.h"
+
+extern uint16_t state_debug;
+
+struct cmd_event_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+
+/* function called when cmd_event is parsed successfully */
+static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void *data)
+{
+ u08 bit=0;
+
+ struct cmd_event_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("all"))) {
+ bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER;
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ mechboard.flags |= bit;
+ else if (!strcmp_P(res->arg2, PSTR("off")))
+ mechboard.flags &= bit;
+ else { /* show */
+ printf_P(PSTR("encoders is %s\r\n"),
+ (DO_ENCODERS & mechboard.flags) ? "on":"off");
+ printf_P(PSTR("cs is %s\r\n"),
+ (DO_CS & mechboard.flags) ? "on":"off");
+ printf_P(PSTR("bd is %s\r\n"),
+ (DO_BD & mechboard.flags) ? "on":"off");
+ printf_P(PSTR("power is %s\r\n"),
+ (DO_POWER & mechboard.flags) ? "on":"off");
+ }
+ return;
+ }
+
+ if (!strcmp_P(res->arg1, PSTR("encoders")))
+ bit = DO_ENCODERS;
+ else if (!strcmp_P(res->arg1, PSTR("cs"))) {
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ arm_calibrate();
+ bit = DO_CS;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("bd")))
+ bit = DO_BD;
+ else if (!strcmp_P(res->arg1, PSTR("power")))
+ bit = DO_POWER;
+
+
+ if (!strcmp_P(res->arg2, PSTR("on")))
+ mechboard.flags |= bit;
+ else if (!strcmp_P(res->arg2, PSTR("off"))) {
+ if (!strcmp_P(res->arg1, PSTR("cs"))) {
+ pwm_ng_set(LEFT_ARM_PWM, 0);
+ pwm_ng_set(RIGHT_ARM_PWM, 0);
+ }
+ mechboard.flags &= (~bit);
+ }
+ printf_P(PSTR("%s is %s\r\n"), res->arg1,
+ (bit & mechboard.flags) ? "on":"off");
+}
+
+prog_char str_event_arg0[] = "event";
+parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
+prog_char str_event_arg1[] = "all#encoders#cs#bd#power";
+parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
+prog_char str_event_arg2[] = "on#off#show";
+parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
+
+prog_char help_event[] = "Enable/disable events";
+parse_pgm_inst_t cmd_event = {
+ .f = cmd_event_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_event,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_event_arg0,
+ (prog_void *)&cmd_event_arg1,
+ (prog_void *)&cmd_event_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Color */
+
+/* this structure is filled when cmd_color is parsed successfully */
+struct cmd_color_result {
+ fixed_string_t arg0;
+ fixed_string_t color;
+};
+
+/* function called when cmd_color is parsed successfully */
+static void cmd_color_parsed(void *parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
+ if (!strcmp_P(res->color, PSTR("red"))) {
+ mechboard.our_color = I2C_COLOR_RED;
+ }
+ else if (!strcmp_P(res->color, PSTR("green"))) {
+ mechboard.our_color = I2C_COLOR_GREEN;
+ }
+ printf_P(PSTR("Done\r\n"));
+}
+
+prog_char str_color_arg0[] = "color";
+parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
+prog_char str_color_color[] = "green#red";
+parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
+
+prog_char help_color[] = "Set our color";
+parse_pgm_inst_t cmd_color = {
+ .f = cmd_color_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_color,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_color_arg0,
+ (prog_void *)&cmd_color_color,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* arm_show */
+
+/* this structure is filled when cmd_arm_show is parsed successfully */
+struct cmd_arm_show_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+/* function called when cmd_arm_show is parsed successfully */
+static void cmd_arm_show_parsed(void *parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_arm_show_result *res = parsed_result;
+
+ if (strcmp_P(res->arg1, PSTR("left")) == 0)
+ arm_dump(&left_arm);
+ else if (strcmp_P(res->arg1, PSTR("right")) == 0)
+ arm_dump(&right_arm);
+ else {
+ arm_dump(&left_arm);
+ arm_dump(&right_arm);
+ }
+}
+
+prog_char str_arm_show_arg0[] = "arm";
+parse_pgm_token_string_t cmd_arm_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_show_result, arg0, str_arm_show_arg0);
+prog_char str_arm_show_arg1[] = "left#right#both";
+parse_pgm_token_string_t cmd_arm_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_show_result, arg1, str_arm_show_arg1);
+prog_char str_arm_show_arg2[] = "show";
+parse_pgm_token_string_t cmd_arm_show_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_arm_show_result, arg2, str_arm_show_arg2);
+
+prog_char help_arm_show[] = "Show arm status";
+parse_pgm_inst_t cmd_arm_show = {
+ .f = cmd_arm_show_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_arm_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_arm_show_arg0,
+ (prog_void *)&cmd_arm_show_arg1,
+ (prog_void *)&cmd_arm_show_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* arm_goto */
+
+/* this structure is filled when cmd_arm_goto is parsed successfully */
+struct cmd_arm_goto_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ int16_t arg2;
+ int16_t arg3;
+ int16_t arg4;
+};
+
+/* function called when cmd_arm_goto is parsed successfully */
+static void cmd_arm_goto_parsed(void *parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_arm_goto_result *res = parsed_result;
+ uint8_t err;
+
+ if (strcmp_P(res->arg1, PSTR("left")) == 0) {
+ arm_do_xy(&left_arm, res->arg2, res->arg3, res->arg4);
+ err = arm_wait_traj_end(&left_arm, ARM_TRAJ_ALL);
+ if (err != ARM_TRAJ_END)
+ printf_P(PSTR("err %x\r\n"), err);
+ }
+ else if (strcmp_P(res->arg1, PSTR("right")) == 0) {
+ arm_do_xy(&right_arm, res->arg2, res->arg3, res->arg4);
+ err = arm_wait_traj_end(&right_arm, ARM_TRAJ_ALL);
+ if (err != ARM_TRAJ_END)
+ printf_P(PSTR("err %x\r\n"), err);
+ }
+ else {
+ arm_do_xy(&left_arm, res->arg2, res->arg3, res->arg4);
+ arm_do_xy(&right_arm, res->arg2, res->arg3, res->arg4);
+ err = arm_wait_traj_end(&left_arm, ARM_TRAJ_ALL);
+ if (err != ARM_TRAJ_END)
+ printf_P(PSTR("left err %x\r\n"), err);
+ err = arm_wait_traj_end(&right_arm, ARM_TRAJ_ALL);
+ if (err != ARM_TRAJ_END)
+ printf_P(PSTR("right err %x\r\n"), err);
+ }
+}
+
+prog_char str_arm_goto_arg0[] = "arm";
+parse_pgm_token_string_t cmd_arm_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_result, arg0, str_arm_goto_arg0);
+prog_char str_arm_goto_arg1[] = "left#right#both";
+parse_pgm_token_string_t cmd_arm_goto_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_result, arg1, str_arm_goto_arg1);
+parse_pgm_token_num_t cmd_arm_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg2, INT16);
+parse_pgm_token_num_t cmd_arm_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg3, INT16);
+parse_pgm_token_num_t cmd_arm_goto_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_arm_goto_result, arg4, INT16);
+
+prog_char help_arm_goto[] = "Arm goto d_mm,h_mm,w_deg";
+parse_pgm_inst_t cmd_arm_goto = {
+ .f = cmd_arm_goto_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_arm_goto,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_arm_goto_arg0,
+ (prog_void *)&cmd_arm_goto_arg1,
+ (prog_void *)&cmd_arm_goto_arg2,
+ (prog_void *)&cmd_arm_goto_arg3,
+ (prog_void *)&cmd_arm_goto_arg4,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* arm_goto_fixed */
+
+/* this structure is filled when cmd_arm_goto_fixed is parsed successfully */
+struct cmd_arm_goto_fixed_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+ fixed_string_t arg3;
+};
+
+/* function called when cmd_arm_goto_fixed is parsed successfully */
+static void cmd_arm_goto_fixed_parsed(void *parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_arm_goto_fixed_result *res = parsed_result;
+ void (*f)(uint8_t, uint8_t) = NULL;
+ uint8_t err, pump_num = 0;
+
+ if (strcmp_P(res->arg2, PSTR("prepare")) == 0)
+ f = arm_goto_prepare_get;
+ else if (strcmp_P(res->arg2, PSTR("get")) == 0)
+ f = arm_goto_get_column;
+ else if (strcmp_P(res->arg2, PSTR("inter")) == 0)
+ f = arm_goto_intermediate_get;
+ else if (strcmp_P(res->arg2, PSTR("inter")) == 0)
+ f = arm_goto_straight;
+
+ if (f == NULL)
+ return;
+
+ /* no matter if it's left or right here */
+ if (strcmp_P(res->arg3, PSTR("p1")) == 0)
+ pump_num = PUMP_LEFT1_NUM;
+ if (strcmp_P(res->arg3, PSTR("p2")) == 0)
+ pump_num = PUMP_LEFT2_NUM;
+
+ /* /!\ strcmp() inverted logic do handle "both" case */
+ if (strcmp_P(res->arg1, PSTR("right")))
+ f(ARM_LEFT_NUM, pump_num);
+ if (strcmp_P(res->arg1, PSTR("left")))
+ f(ARM_RIGHT_NUM, pump_num);
+
+ if (strcmp_P(res->arg1, PSTR("right"))) {
+ err = arm_wait_traj_end(&left_arm, ARM_TRAJ_ALL);
+ if (err != ARM_TRAJ_END)
+ printf_P(PSTR("left err %x\r\n"), err);
+ }
+ if (strcmp_P(res->arg1, PSTR("left"))) {
+ err = arm_wait_traj_end(&right_arm, ARM_TRAJ_ALL);
+ if (err != ARM_TRAJ_END)
+ printf_P(PSTR("right err %x\r\n"), err);
+ }
+}
+
+prog_char str_arm_goto_fixed_arg0[] = "arm";
+parse_pgm_token_string_t cmd_arm_goto_fixed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg0, str_arm_goto_fixed_arg0);
+prog_char str_arm_goto_fixed_arg1[] = "left#right#both";
+parse_pgm_token_string_t cmd_arm_goto_fixed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg1, str_arm_goto_fixed_arg1);
+prog_char str_arm_goto_fixed_arg2[] = "prepare#get#inter#straight";
+parse_pgm_token_string_t cmd_arm_goto_fixed_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg2, str_arm_goto_fixed_arg2);
+prog_char str_arm_goto_fixed_arg3[] = "p1#p2";
+parse_pgm_token_string_t cmd_arm_goto_fixed_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_arm_goto_fixed_result, arg3, str_arm_goto_fixed_arg3);
+
+prog_char help_arm_goto_fixed[] = "Goto fixed positions";
+parse_pgm_inst_t cmd_arm_goto_fixed = {
+ .f = cmd_arm_goto_fixed_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_arm_goto_fixed,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_arm_goto_fixed_arg0,
+ (prog_void *)&cmd_arm_goto_fixed_arg1,
+ (prog_void *)&cmd_arm_goto_fixed_arg2,
+ (prog_void *)&cmd_arm_goto_fixed_arg3,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* arm_simulate */
+
+/* this structure is filled when cmd_arm_simulate is parsed successfully */
+struct cmd_arm_simulate_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+/* function called when cmd_arm_simulate is parsed successfully */
+static void cmd_arm_simulate_parsed(void *parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_arm_simulate_result *res = parsed_result;
+ uint8_t val;
+
+ if (strcmp_P(res->arg2, PSTR("simulate")) == 0)
+ val = 1;
+ else
+ val = 0;
+
+ if (strcmp_P(res->arg1, PSTR("left")) == 0)
+ left_arm.config.simulate = 1;
+ else if (strcmp_P(res->arg1, PSTR("right")) == 0)
+ right_arm.config.simulate = 1;
+ else {
+ left_arm.config.simulate = 1;
+ right_arm.config.simulate = 1;
+ }
+}
+
+prog_char str_arm_simulate_arg0[] = "arm";
+parse_pgm_token_string_t cmd_arm_simulate_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_arm_simulate_result, arg0, str_arm_simulate_arg0);
+prog_char str_arm_simulate_arg1[] = "left#right#both";
+parse_pgm_token_string_t cmd_arm_simulate_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_arm_simulate_result, arg1, str_arm_simulate_arg1);
+prog_char str_arm_simulate_arg2[] = "simulate#real";
+parse_pgm_token_string_t cmd_arm_simulate_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_arm_simulate_result, arg2, str_arm_simulate_arg2);
+
+prog_char help_arm_simulate[] = "Simulation or real for arm";
+parse_pgm_inst_t cmd_arm_simulate = {
+ .f = cmd_arm_simulate_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_arm_simulate,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_arm_simulate_arg0,
+ (prog_void *)&cmd_arm_simulate_arg1,
+ (prog_void *)&cmd_arm_simulate_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* finger */
+
+/* this structure is filled when cmd_finger is parsed successfully */
+struct cmd_finger_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_finger is parsed successfully */
+static void cmd_finger_parsed(void *parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_finger_result *res = parsed_result;
+ uint16_t dest = 0;
+
+ if (strcmp_P(res->arg1, PSTR("left")) == 0)
+ dest = FINGER_LEFT;
+ else if (strcmp_P(res->arg1, PSTR("right")) == 0)
+ dest = FINGER_RIGHT;
+ else if (strcmp_P(res->arg1, PSTR("center")) == 0)
+ dest = FINGER_CENTER;
+ finger_goto(dest);
+}
+
+prog_char str_finger_arg0[] = "finger";
+parse_pgm_token_string_t cmd_finger_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_finger_result, arg0, str_finger_arg0);
+prog_char str_finger_arg1[] = "left#right#center";
+parse_pgm_token_string_t cmd_finger_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_finger_result, arg1, str_finger_arg1);
+
+prog_char help_finger[] = "Move finger";
+parse_pgm_inst_t cmd_finger = {
+ .f = cmd_finger_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_finger,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_finger_arg0,
+ (prog_void *)&cmd_finger_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* pump */
+
+/* this structure is filled when cmd_pump is parsed successfully */
+struct cmd_pump_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+/* function called when cmd_pump is parsed successfully */
+static void cmd_pump_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_pump_result *res = parsed_result;
+ int8_t pump_num = 0;
+ int16_t val = 0;
+
+ if (strcmp_P(res->arg1, PSTR("left1")) == 0)
+ pump_num = PUMP_LEFT1_NUM;
+ else if (strcmp_P(res->arg1, PSTR("right1")) == 0)
+ pump_num = PUMP_RIGHT1_NUM;
+ else if (strcmp_P(res->arg1, PSTR("left2")) == 0)
+ pump_num = PUMP_LEFT2_NUM;
+ else if (strcmp_P(res->arg1, PSTR("right2")) == 0)
+ pump_num = PUMP_RIGHT2_NUM;
+
+ if (strcmp_P(res->arg2, PSTR("on")) == 0)
+ val = PUMP_ON;
+ else if (strcmp_P(res->arg2, PSTR("off")) == 0)
+ val = PUMP_OFF;
+ else if (strcmp_P(res->arg2, PSTR("reverse")) == 0)
+ val = PUMP_REVERSE;
+
+ pump_set(pump_num, val);
+}
+
+prog_char str_pump_arg0[] = "pump";
+parse_pgm_token_string_t cmd_pump_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_result, arg0, str_pump_arg0);
+prog_char str_pump_arg1[] = "left1#left2#right1#right2";
+parse_pgm_token_string_t cmd_pump_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_result, arg1, str_pump_arg1);
+prog_char str_pump_arg2[] = "on#off#reverse";
+parse_pgm_token_string_t cmd_pump_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_pump_result, arg2, str_pump_arg2);
+
+prog_char help_pump[] = "activate pump";
+parse_pgm_inst_t cmd_pump = {
+ .f = cmd_pump_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pump,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pump_arg0,
+ (prog_void *)&cmd_pump_arg1,
+ (prog_void *)&cmd_pump_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* State1 */
+
+/* this structure is filled when cmd_state1 is parsed successfully */
+struct cmd_state1_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_state1 is parsed successfully */
+static void cmd_state1_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_state1_result *res = parsed_result;
+ struct i2c_cmd_mechboard_set_mode command;
+
+ if (!strcmp_P(res->arg1, PSTR("init"))) {
+ state_init();
+ return;
+ }
+
+ if (!strcmp_P(res->arg1, PSTR("manual")))
+ command.mode = I2C_MECHBOARD_MODE_MANUAL;
+ else if (!strcmp_P(res->arg1, PSTR("harvest")))
+ command.mode = I2C_MECHBOARD_MODE_HARVEST;
+ else if (!strcmp_P(res->arg1, PSTR("lazy_harvest")))
+ command.mode = I2C_MECHBOARD_MODE_LAZY_HARVEST;
+ else if (!strcmp_P(res->arg1, PSTR("pickup")))
+ command.mode = I2C_MECHBOARD_MODE_PICKUP;
+ else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel")))
+ command.mode = I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL;
+ else if (!strcmp_P(res->arg1, PSTR("get_lintel")))
+ command.mode = I2C_MECHBOARD_MODE_GET_LINTEL;
+ else if (!strcmp_P(res->arg1, PSTR("put_lintel")))
+ command.mode = I2C_MECHBOARD_MODE_PUT_LINTEL;
+ else if (!strcmp_P(res->arg1, PSTR("clear")))
+ command.mode = I2C_MECHBOARD_MODE_CLEAR;
+ else if (!strcmp_P(res->arg1, PSTR("loaded")))
+ command.mode = I2C_MECHBOARD_MODE_LOADED;
+ else if (!strcmp_P(res->arg1, PSTR("store")))
+ command.mode = I2C_MECHBOARD_MODE_STORE;
+ else if (!strcmp_P(res->arg1, PSTR("lazy_pickup")))
+ command.mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
+ state_set_mode(&command);
+}
+
+prog_char str_state1_arg0[] = "mechboard";
+parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0);
+prog_char str_state1_arg1[] = "init#manual#pickup#prepare_get_lintel#get_lintel#put_lintel#clear#lazy_harvest#harvest#loaded#store#lazy_pickup";
+parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1);
+
+prog_char help_state1[] = "set mechboard mode";
+parse_pgm_inst_t cmd_state1 = {
+ .f = cmd_state1_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_state1,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_state1_arg0,
+ (prog_void *)&cmd_state1_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* State2 */
+
+/* this structure is filled when cmd_state2 is parsed successfully */
+struct cmd_state2_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+/* function called when cmd_state2 is parsed successfully */
+static void cmd_state2_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_state2_result *res = parsed_result;
+ struct i2c_cmd_mechboard_set_mode command;
+ uint8_t side;
+
+ if (!strcmp_P(res->arg2, PSTR("left")))
+ side = I2C_LEFT_SIDE;
+ else if (!strcmp_P(res->arg2, PSTR("right")))
+ side = I2C_RIGHT_SIDE;
+ else if (!strcmp_P(res->arg2, PSTR("center")))
+ side = I2C_CENTER_SIDE;
+ else
+ side = I2C_AUTO_SIDE;
+
+ if (!strcmp_P(res->arg1, PSTR("prepare_pickup"))) {
+ command.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
+ command.prep_pickup.side = side;
+ command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("push_temple_disc"))) {
+ command.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC;
+ command.push_temple_disc.side = side;
+ }
+
+
+ state_set_mode(&command);
+}
+
+prog_char str_state2_arg0[] = "mechboard";
+parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
+prog_char str_state2_arg1[] = "prepare_pickup#push_temple_disc";
+parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
+prog_char str_state2_arg2[] = "left#right#auto#center";
+parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);
+
+prog_char help_state2[] = "set mechboard mode";
+parse_pgm_inst_t cmd_state2 = {
+ .f = cmd_state2_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_state2,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_state2_arg0,
+ (prog_void *)&cmd_state2_arg1,
+ (prog_void *)&cmd_state2_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* State3 */
+
+/* this structure is filled when cmd_state3 is parsed successfully */
+struct cmd_state3_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t level;
+};
+
+/* function called when cmd_state3 is parsed successfully */
+static void cmd_state3_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_state3_result *res = parsed_result;
+ struct i2c_cmd_mechboard_set_mode command;
+
+ if (!strcmp_P(res->arg1, PSTR("prepare_build"))) {
+ command.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD;
+ command.prep_build.level_l = res->level;
+ command.prep_build.level_r = res->level;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("prepare_inside"))) {
+ command.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE;
+ command.prep_inside.level_l = res->level;
+ command.prep_inside.level_r = res->level;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("autobuild"))) {
+ command.mode = I2C_MECHBOARD_MODE_AUTOBUILD;
+ command.autobuild.level_left = res->level;
+ command.autobuild.level_right = res->level;
+ command.autobuild.count_left = 2;
+ command.autobuild.count_right = 2;
+ command.autobuild.distance_left = I2C_AUTOBUILD_DEFAULT_DIST;
+ command.autobuild.distance_right = I2C_AUTOBUILD_DEFAULT_DIST;
+ command.autobuild.do_lintel = 1;
+ }
+ else if (!strcmp_P(res->arg1, PSTR("push_temple"))) {
+ command.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE;
+ command.push_temple.level = res->level;
+ }
+ state_set_mode(&command);
+}
+
+prog_char str_state3_arg0[] = "mechboard";
+parse_pgm_token_string_t cmd_state3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg0, str_state3_arg0);
+prog_char str_state3_arg1[] = "prepare_build#autobuild#prepare_inside";
+parse_pgm_token_string_t cmd_state3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg1, str_state3_arg1);
+parse_pgm_token_num_t cmd_state3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state3_result, level, UINT8);
+
+prog_char help_state3[] = "set mechboard mode";
+parse_pgm_inst_t cmd_state3 = {
+ .f = cmd_state3_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_state3,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_state3_arg0,
+ (prog_void *)&cmd_state3_arg1,
+ (prog_void *)&cmd_state3_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* State4 */
+
+/* this structure is filled when cmd_state4 is parsed successfully */
+struct cmd_state4_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t level_l;
+ uint8_t count_l;
+ uint8_t dist_l;
+ uint8_t level_r;
+ uint8_t count_r;
+ uint8_t dist_r;
+ uint8_t do_lintel;
+};
+
+/* function called when cmd_state4 is parsed successfully */
+static void cmd_state4_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_state4_result *res = parsed_result;
+ struct i2c_cmd_mechboard_set_mode command;
+
+ if (!strcmp_P(res->arg1, PSTR("autobuild"))) {
+ command.mode = I2C_MECHBOARD_MODE_AUTOBUILD;
+ command.autobuild.distance_left = res->dist_l;
+ command.autobuild.distance_right = res->dist_r;
+ command.autobuild.level_left = res->level_l;
+ command.autobuild.level_right = res->level_r;
+ command.autobuild.count_left = res->count_l;
+ command.autobuild.count_right = res->count_r;
+ command.autobuild.do_lintel = res->do_lintel;
+ }
+ state_set_mode(&command);
+}
+
+prog_char str_state4_arg0[] = "mechboard";
+parse_pgm_token_string_t cmd_state4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state4_result, arg0, str_state4_arg0);
+prog_char str_state4_arg1[] = "autobuild";
+parse_pgm_token_string_t cmd_state4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state4_result, arg1, str_state4_arg1);
+parse_pgm_token_num_t cmd_state4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, level_l, UINT8);
+parse_pgm_token_num_t cmd_state4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, count_l, UINT8);
+parse_pgm_token_num_t cmd_state4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, dist_l, UINT8);
+parse_pgm_token_num_t cmd_state4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, level_r, UINT8);
+parse_pgm_token_num_t cmd_state4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, count_r, UINT8);
+parse_pgm_token_num_t cmd_state4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, dist_r, UINT8);
+parse_pgm_token_num_t cmd_state4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_state4_result, do_lintel, UINT8);
+
+prog_char help_state4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)";
+parse_pgm_inst_t cmd_state4 = {
+ .f = cmd_state4_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_state4,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_state4_arg0,
+ (prog_void *)&cmd_state4_arg1,
+ (prog_void *)&cmd_state4_arg2,
+ (prog_void *)&cmd_state4_arg3,
+ (prog_void *)&cmd_state4_arg4,
+ (prog_void *)&cmd_state4_arg5,
+ (prog_void *)&cmd_state4_arg6,
+ (prog_void *)&cmd_state4_arg7,
+ (prog_void *)&cmd_state4_arg8,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* State5 */
+
+/* this structure is filled when cmd_state5 is parsed successfully */
+struct cmd_state5_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+ fixed_string_t arg3;
+};
+
+/* function called when cmd_state5 is parsed successfully */
+static void cmd_state5_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_state5_result *res = parsed_result;
+ struct i2c_cmd_mechboard_set_mode command;
+ uint8_t side;
+
+ if (!strcmp_P(res->arg2, PSTR("left")))
+ side = I2C_LEFT_SIDE;
+ else if (!strcmp_P(res->arg2, PSTR("right")))
+ side = I2C_RIGHT_SIDE;
+ else if (!strcmp_P(res->arg2, PSTR("center")))
+ side = I2C_CENTER_SIDE;
+ else
+ side = I2C_AUTO_SIDE;
+
+ command.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
+ command.prep_pickup.side = side;
+
+ if (!strcmp_P(res->arg3, PSTR("harvest")))
+ command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_HARVEST;
+ else if (!strcmp_P(res->arg3, PSTR("lazy_harvest")))
+ command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST;
+ else if (!strcmp_P(res->arg3, PSTR("pickup")))
+ command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_PICKUP;
+ else if (!strcmp_P(res->arg3, PSTR("clear")))
+ command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_CLEAR;
+ else if (!strcmp_P(res->arg3, PSTR("store")))
+ command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_STORE;
+ else if (!strcmp_P(res->arg3, PSTR("lazy_pickup")))
+ command.prep_pickup.next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
+
+ state_set_mode(&command);
+}
+
+prog_char str_state5_arg0[] = "mechboard";
+parse_pgm_token_string_t cmd_state5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg0, str_state5_arg0);
+prog_char str_state5_arg1[] = "prepare_pickup";
+parse_pgm_token_string_t cmd_state5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg1, str_state5_arg1);
+prog_char str_state5_arg2[] = "left#right#auto#center";
+parse_pgm_token_string_t cmd_state5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg2, str_state5_arg2);
+prog_char str_state5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear";
+parse_pgm_token_string_t cmd_state5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_state5_result, arg3, str_state5_arg3);
+
+prog_char help_state5[] = "set mechboard mode 2";
+parse_pgm_inst_t cmd_state5 = {
+ .f = cmd_state5_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_state5,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_state5_arg0,
+ (prog_void *)&cmd_state5_arg1,
+ (prog_void *)&cmd_state5_arg2,
+ (prog_void *)&cmd_state5_arg3,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* State_Machine */
+
+/* this structure is filled when cmd_state_machine is parsed successfully */
+struct cmd_state_machine_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_state_machine is parsed successfully */
+static void cmd_state_machine_parsed(__attribute__((unused)) void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ state_machine();
+}
+
+prog_char str_state_machine_arg0[] = "state_machine";
+parse_pgm_token_string_t cmd_state_machine_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_machine_result, arg0, str_state_machine_arg0);
+
+prog_char help_state_machine[] = "launch state machine";
+parse_pgm_inst_t cmd_state_machine = {
+ .f = cmd_state_machine_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_state_machine,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_state_machine_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* State_Debug */
+
+/* this structure is filled when cmd_state_debug is parsed successfully */
+struct cmd_state_debug_result {
+ fixed_string_t arg0;
+ uint8_t on;
+};
+
+/* function called when cmd_state_debug is parsed successfully */
+static void cmd_state_debug_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_state_debug_result *res = parsed_result;
+ state_debug = res->on;
+}
+
+prog_char str_state_debug_arg0[] = "state_debug";
+parse_pgm_token_string_t cmd_state_debug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_debug_result, arg0, str_state_debug_arg0);
+parse_pgm_token_num_t cmd_state_debug_on = TOKEN_NUM_INITIALIZER(struct cmd_state_debug_result, on, UINT8);
+
+prog_char help_state_debug[] = "Set debug timer for state machine";
+parse_pgm_inst_t cmd_state_debug = {
+ .f = cmd_state_debug_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_state_debug,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_state_debug_arg0,
+ (prog_void *)&cmd_state_debug_on,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Servo_Lintel */
+
+/* this structure is filled when cmd_servo_lintel is parsed successfully */
+struct cmd_servo_lintel_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_servo_lintel is parsed successfully */
+static void cmd_servo_lintel_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_servo_lintel_result *res = parsed_result;
+ if (!strcmp_P(res->arg1, PSTR("out")))
+ servo_lintel_out();
+ else if (!strcmp_P(res->arg1, PSTR("1lin")))
+ servo_lintel_1lin();
+ else if (!strcmp_P(res->arg1, PSTR("2lin")))
+ servo_lintel_2lin();
+
+}
+
+prog_char str_servo_lintel_arg0[] = "servo_lintel";
+parse_pgm_token_string_t cmd_servo_lintel_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_lintel_result, arg0, str_servo_lintel_arg0);
+prog_char str_servo_lintel_arg1[] = "out#1lin#2lin";
+parse_pgm_token_string_t cmd_servo_lintel_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_lintel_result, arg1, str_servo_lintel_arg1);
+
+prog_char help_servo_lintel[] = "Servo_Lintel function";
+parse_pgm_inst_t cmd_servo_lintel = {
+ .f = cmd_servo_lintel_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_servo_lintel,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_servo_lintel_arg0,
+ (prog_void *)&cmd_servo_lintel_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Pump_Current */
+
+/* this structure is filled when cmd_pump_current is parsed successfully */
+struct cmd_pump_current_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_pump_current is parsed successfully */
+static void cmd_pump_current_parsed(__attribute__((unused)) void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"),
+ mechboard.pump_left1_current, mechboard.pump_left2_current,
+ sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4));
+}
+
+prog_char str_pump_current_arg0[] = "pump_current";
+parse_pgm_token_string_t cmd_pump_current_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg0, str_pump_current_arg0);
+prog_char str_pump_current_arg1[] = "show";
+parse_pgm_token_string_t cmd_pump_current_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg1, str_pump_current_arg1);
+
+prog_char help_pump_current[] = "dump pump current";
+parse_pgm_inst_t cmd_pump_current = {
+ .f = cmd_pump_current_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_pump_current,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_pump_current_arg0,
+ (prog_void *)&cmd_pump_current_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Manivelle */
+
+/* this structure is filled when cmd_manivelle is parsed successfully */
+struct cmd_manivelle_result {
+ fixed_string_t arg0;
+ uint8_t step;
+};
+
+/* function called when cmd_manivelle is parsed successfully */
+static void cmd_manivelle_parsed(__attribute__((unused)) void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_manivelle_result *res = parsed_result;
+ state_manivelle(res->step);
+}
+
+prog_char str_manivelle_arg0[] = "manivelle";
+parse_pgm_token_string_t cmd_manivelle_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_manivelle_result, arg0, str_manivelle_arg0);
+parse_pgm_token_num_t cmd_manivelle_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_manivelle_result, step, UINT8);
+
+prog_char help_manivelle[] = "Manivelle function";
+parse_pgm_inst_t cmd_manivelle = {
+ .f = cmd_manivelle_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_manivelle,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_manivelle_arg0,
+ (prog_void *)&cmd_manivelle_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Test */
+
+/* this structure is filled when cmd_test is parsed successfully */
+struct cmd_test_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_test is parsed successfully */
+static void cmd_test_parsed(__attribute__((unused)) void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+}
+
+prog_char str_test_arg0[] = "test";
+parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
+
+prog_char help_test[] = "Test function";
+parse_pgm_inst_t cmd_test = {
+ .f = cmd_test_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_test,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_test_arg0,
+ NULL,
+ },
+};
git.droids-corp.org / aversive.git / blobdiff