#include "strat.h"
#include "strat_utils.h"
#include "strat_base.h"
+#include "strat_db.h"
+#include "strat_corn.h"
#include "i2c_protocol.h"
#include "actuator.h"
},
};
+/**********************************************************/
+/* Beacon_Start */
+
+/* this structure is filled when cmd_beacon_start is parsed successfully */
+struct cmd_beacon_start_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_beacon_start is parsed successfully */
+static void cmd_beacon_start_parsed(void *parsed_result, void *data)
+{
+ struct cmd_beacon_start_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("start")))
+ i2c_ballboard_set_beacon(1);
+ else
+ i2c_ballboard_set_beacon(0);
+}
+
+prog_char str_beacon_start_arg0[] = "beacon";
+parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0);
+prog_char str_beacon_start_arg1[] = "start#stop";
+parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1);
+
+prog_char help_beacon_start[] = "Beacon enabled/disable";
+parse_pgm_inst_t cmd_beacon_start = {
+ .f = cmd_beacon_start_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_beacon_start,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_beacon_start_arg0,
+ (prog_void *)&cmd_beacon_start_arg1,
+ NULL,
+ },
+};
/**********************************************************/
/* Start */
/* function called when cmd_start is parsed successfully */
static void cmd_start_parsed(void *parsed_result, void *data)
{
-#ifdef HOST_VERSION
- printf("not implemented\n");
-#else
struct cmd_start_result *res = parsed_result;
uint8_t old_level = gen.log_level;
gen.logs[NB_LOGS] = E_USER_STRAT;
if (!strcmp_P(res->debug, PSTR("debug"))) {
- strat_infos.dump_enabled = 1;
+ strat_db.dump_enabled = 1;
gen.log_level = 5;
}
else {
- strat_infos.dump_enabled = 0;
+ strat_db.dump_enabled = 0;
gen.log_level = 0;
}
i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
}
+ printf_P(PSTR("Press a key when beacon ready\r\n"));
+ i2c_ballboard_set_beacon(0);
+ while(!cmdline_keypressed());
+ i2c_ballboard_set_beacon(1);
+
strat_start();
gen.logs[NB_LOGS] = 0;
gen.log_level = old_level;
-#endif
}
prog_char str_start_arg0[] = "start";
}
}
else {
+#ifdef HOST_VERSION
+#define PWM_INTERACT 1200
+#else
+#define PWM_INTERACT 1200
+#endif
switch(cmd) {
case KEY_UP_ARR:
- pwm_set_and_save(LEFT_PWM, 1200);
- pwm_set_and_save(RIGHT_PWM, 1200);
+ pwm_set_and_save(LEFT_PWM, PWM_INTERACT);
+ pwm_set_and_save(RIGHT_PWM, PWM_INTERACT);
break;
case KEY_LEFT_ARR:
- pwm_set_and_save(LEFT_PWM, -1200);
- pwm_set_and_save(RIGHT_PWM, 1200);
+ pwm_set_and_save(LEFT_PWM, -PWM_INTERACT);
+ pwm_set_and_save(RIGHT_PWM, PWM_INTERACT);
break;
case KEY_DOWN_ARR:
- pwm_set_and_save(LEFT_PWM, -1200);
- pwm_set_and_save(RIGHT_PWM, -1200);
+ pwm_set_and_save(LEFT_PWM, -PWM_INTERACT);
+ pwm_set_and_save(RIGHT_PWM, -PWM_INTERACT);
break;
case KEY_RIGHT_ARR:
- pwm_set_and_save(LEFT_PWM, 1200);
- pwm_set_and_save(RIGHT_PWM, -1200);
+ pwm_set_and_save(LEFT_PWM, PWM_INTERACT);
+ pwm_set_and_save(RIGHT_PWM, -PWM_INTERACT);
break;
}
}
struct cmd_cobboard_setmode1_result *res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("init")))
- i2c_cobboard_mode_init();
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT);
else if (!strcmp_P(res->arg1, PSTR("eject")))
- i2c_cobboard_mode_eject();
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
+ else if (!strcmp_P(res->arg1, PSTR("kickstand_up")))
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
+ else if (!strcmp_P(res->arg1, PSTR("kickstand_down")))
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
}
prog_char str_cobboard_setmode1_arg0[] = "cobboard";
parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0);
-prog_char str_cobboard_setmode1_arg1[] = "init#eject";
+prog_char str_cobboard_setmode1_arg1[] = "init#eject#kickstand_up#kickstand_down";
parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1);
prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)";
};
/* function called when cmd_cobboard_setmode2 is parsed successfully */
-static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
+static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data)
{
struct cmd_cobboard_setmode2_result *res = parsed_result;
uint8_t side = I2C_LEFT_SIDE;
else if (!strcmp_P(res->arg2, PSTR("right")))
side = I2C_RIGHT_SIDE;
- if (!strcmp_P(res->arg1, PSTR("deploy")))
- i2c_cobboard_mode_deploy(side);
- else if (!strcmp_P(res->arg1, PSTR("harvest")))
- i2c_cobboard_mode_harvest(side);
- else if (!strcmp_P(res->arg1, PSTR("pack")))
- i2c_cobboard_mode_pack(side);
+ if (!strcmp_P(res->arg1, PSTR("deploy"))) {
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ i2c_cobboard_deploy(side);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("harvest"))) {
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ i2c_cobboard_autoharvest(side);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("pack"))) {
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ i2c_cobboard_pack(side);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("weak_pack"))) {
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ i2c_cobboard_pack_weak(side);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) {
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ i2c_cobboard_deploy_nomove(side);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) {
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+ i2c_cobboard_autoharvest_nomove(side);
+ }
}
prog_char str_cobboard_setmode2_arg0[] = "cobboard";
parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
-prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
+prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack#weak_pack#harvest_nomove#deploy_nomove";
parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
prog_char str_cobboard_setmode2_arg2[] = "left#right";
parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
printf_P(PSTR("status = %x\r\n"), ballboard.status);
printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
+ printf_P(PSTR("lcob = %d\r\n"), ballboard.lcob);
+ printf_P(PSTR("rcob = %d\r\n"), ballboard.rcob);
#endif
}
i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
else if (!strcmp_P(res->arg1, PSTR("harvest")))
i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
+ else if (!strcmp_P(res->arg1, PSTR("prepare")))
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
+ else if (!strcmp_P(res->arg1, PSTR("take")))
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
/* other commands */
}
prog_char str_ballboard_setmode1_arg0[] = "ballboard";
parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
-prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off";
+prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off#take#prepare";
parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
/* function called when cmd_ballboard_setmode2 is parsed successfully */
static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
{
- struct cmd_ballboard_setmode2_result *res = parsed_result;
- uint8_t mode = I2C_BALLBOARD_MODE_INIT;
-
- if (!strcmp_P(res->arg2, PSTR("left"))) {
- if (!strcmp_P(res->arg1, PSTR("prepare")))
- mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
- else if (!strcmp_P(res->arg1, PSTR("take")))
- mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
- }
- else {
- if (!strcmp_P(res->arg1, PSTR("prepare")))
- mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
- else if (!strcmp_P(res->arg1, PSTR("take")))
- mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
- }
- i2c_ballboard_set_mode(mode);
}
prog_char str_ballboard_setmode2_arg0[] = "ballboard";
parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
-prog_char str_ballboard_setmode2_arg1[] = "prepare#take";
+prog_char str_ballboard_setmode2_arg1[] = "xxx";
parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
prog_char str_ballboard_setmode2_arg2[] = "left#right";
parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
};
/**********************************************************/
-/* Test */
+/* Time_Monitor */
-/* this structure is filled when cmd_test is parsed successfully */
-struct cmd_test_result {
+/* this structure is filled when cmd_time_monitor is parsed successfully */
+struct cmd_time_monitor_result {
fixed_string_t arg0;
- int32_t radius;
- int32_t dist;
-};
-
-#define LINE_UP 0
-#define LINE_DOWN 1
-#define LINE_R_UP 2
-#define LINE_L_DOWN 3
-#define LINE_L_UP 4
-#define LINE_R_DOWN 5
-
-struct line_2pts {
- point_t p1;
- point_t p2;
+ fixed_string_t arg1;
};
-static void num2line(struct line_2pts *l, uint8_t dir, uint8_t num)
+/* function called when cmd_time_monitor is parsed successfully */
+static void cmd_time_monitor_parsed(void *parsed_result, void *data)
{
- float n = num;
-
- switch (dir) {
-
- case LINE_UP:
- l->p1.x = n * 450 + 375;
- l->p1.y = COLOR_Y(0);
- l->p2.x = n * 450 + 375;
- l->p2.y = COLOR_Y(2100);
- break;
- case LINE_DOWN:
- l->p1.x = n * 450 + 375;
- l->p1.y = COLOR_Y(2100);
- l->p2.x = n * 450 + 375;
- l->p2.y = COLOR_Y(0);
- break;
- case LINE_R_UP:
- l->p1.x = 150;
- l->p1.y = COLOR_Y(-n * 500 + 1472);
- l->p2.x = 2850;
- l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
- break;
- case LINE_L_DOWN:
- l->p1.x = 2850;
- l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
- l->p2.x = 150;
- l->p2.y = COLOR_Y(-n * 500 + 1472);
- break;
- case LINE_L_UP:
- l->p1.x = 2850;
- l->p1.y = COLOR_Y(-n * 500 + 1472);
- l->p2.x = 150;
- l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
- break;
- case LINE_R_DOWN:
- l->p1.x = 150;
- l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
- l->p2.x = 2850;
- l->p2.y = COLOR_Y(-n * 500 + 1472);
- break;
- default:
- break;
+#ifndef HOST_VERSION
+ struct cmd_time_monitor_result *res = parsed_result;
+ uint16_t seconds;
+
+ if (!strcmp_P(res->arg1, PSTR("reset"))) {
+ eeprom_write_word(EEPROM_TIME_ADDRESS, 0);
}
+ seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
+ printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
+#endif
}
-#if 0
-static void reverse_line(struct line_2pts *l)
+prog_char str_time_monitor_arg0[] = "time_monitor";
+parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0);
+prog_char str_time_monitor_arg1[] = "show#reset";
+parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1);
+
+prog_char help_time_monitor[] = "Show since how long we are running";
+parse_pgm_inst_t cmd_time_monitor = {
+ .f = cmd_time_monitor_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_time_monitor,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_time_monitor_arg0,
+ (prog_void *)&cmd_time_monitor_arg1,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Strat_Event */
+
+/* this structure is filled when cmd_strat_event is parsed successfully */
+struct cmd_strat_event_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_strat_event is parsed successfully */
+static void cmd_strat_event_parsed(void *parsed_result, void *data)
{
- point_t tmp;
-
- tmp.x = l->p1.x;
- tmp.y = l->p1.y;
- l->p1.x = l->p2.x;
- l->p1.y = l->p2.y;
- l->p2.x = tmp.x;
- l->p2.y = tmp.y;
+ struct cmd_strat_event_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("on")))
+ strat_event_enable();
+ else
+ strat_event_disable();
}
-#endif
-static void line2line(uint8_t dir1, uint8_t num1,
- uint8_t dir2, uint8_t num2)
+prog_char str_strat_event_arg0[] = "strat_event";
+parse_pgm_token_string_t cmd_strat_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg0, str_strat_event_arg0);
+prog_char str_strat_event_arg1[] = "on#off";
+parse_pgm_token_string_t cmd_strat_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg1, str_strat_event_arg1);
+
+prog_char help_strat_event[] = "Enable/disable strat_event callback";
+parse_pgm_inst_t cmd_strat_event = {
+ .f = cmd_strat_event_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_strat_event,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_strat_event_arg0,
+ (prog_void *)&cmd_strat_event_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Sleep */
+
+/* this structure is filled when cmd_sleep is parsed successfully */
+struct cmd_sleep_result {
+ fixed_string_t arg0;
+ uint32_t ms;
+};
+
+/* function called when cmd_sleep is parsed successfully */
+static void cmd_sleep_parsed(void *parsed_result, void *data)
{
- double line1_a_rad, line1_a_deg, line2_a_rad;
- double diff_a_deg, diff_a_deg_abs, beta_deg;
- double radius;
- struct line_2pts l1, l2;
- line_t ll1, ll2;
- point_t p;
-
- /* convert to 2 points */
- num2line(&l1, dir1, num1);
- num2line(&l2, dir2, num2);
-
- printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
- l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
- printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
- l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
-
- /* convert to line eq and find intersection */
- pts2line(&l1.p1, &l1.p2, &ll1);
- pts2line(&l2.p1, &l2.p2, &ll2);
- intersect_line(&ll1, &ll2, &p);
-
- line1_a_rad = atan2(l1.p2.y - l1.p1.y,
- l1.p2.x - l1.p1.x);
- line1_a_deg = DEG(line1_a_rad);
- line2_a_rad = atan2(l2.p2.y - l2.p1.y,
- l2.p2.x - l2.p1.x);
- diff_a_deg = DEG(line2_a_rad - line1_a_rad);
- diff_a_deg_abs = fabs(diff_a_deg);
-
- if (diff_a_deg_abs < 70.) {
- radius = 200;
- if (diff_a_deg > 0)
- beta_deg = 40;
- else
- beta_deg = -40;
- }
- else if (diff_a_deg_abs < 100.) {
- radius = 100;
- if (diff_a_deg > 0)
- beta_deg = 40;
- else
- beta_deg = -40;
- }
- else {
- radius = 120;
- if (diff_a_deg > 0)
- beta_deg = 60;
- else
- beta_deg = -60;
- }
- trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
- line1_a_deg, 150., diff_a_deg, beta_deg,
- radius, xy_norm(l1.p1.x, l1.p1.y,
- p.x, p.y));
- wait_traj_end(0xFF);
+ struct cmd_sleep_result *res = parsed_result;
+ time_wait_ms(res->ms);
}
+prog_char str_sleep_arg0[] = "sleep";
+parse_pgm_token_string_t cmd_sleep_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sleep_result, arg0, str_sleep_arg0);
+parse_pgm_token_num_t cmd_sleep_ms = TOKEN_NUM_INITIALIZER(struct cmd_sleep_result, ms, UINT32);
+
+prog_char help_sleep[] = "Sleep during some miliseconds";
+parse_pgm_inst_t cmd_sleep = {
+ .f = cmd_sleep_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_sleep,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_sleep_arg0,
+ (prog_void *)&cmd_sleep_ms,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Test */
+
+/* this structure is filled when cmd_test is parsed successfully */
+struct cmd_test_result {
+ fixed_string_t arg0;
+ int32_t radius;
+ int32_t dist;
+};
+
/* function called when cmd_test is parsed successfully */
static void cmd_test_parsed(void *parsed_result, void *data)
{
-#ifdef HOST_VERSION
- strat_reset_pos(298.48, 309.21, 70.02);
- mainboard.angle.on = 1;
- mainboard.distance.on = 1;
-#endif
- time_wait_ms(100);
-
- line2line(LINE_UP, 0, LINE_R_DOWN, 2);
- line2line(LINE_R_DOWN, 2, LINE_R_UP, 1);
-
- trajectory_hardstop(&mainboard.traj);
+ strat_db.dump_enabled = 1;
+ strat_db_dump(__FUNCTION__);
+
+ corn_set_color(strat_db.corn_table[0], I2C_COB_BLACK);
+ strat_db_dump(__FUNCTION__);
+
+ corn_set_color(strat_db.corn_table[3], I2C_COB_WHITE);
+ strat_db_dump(__FUNCTION__);
+ corn_set_color(strat_db.corn_table[4], I2C_COB_WHITE);
+ strat_db_dump(__FUNCTION__);
+ corn_set_color(strat_db.corn_table[5], I2C_COB_WHITE);
+ strat_db_dump(__FUNCTION__);
}
prog_char str_test_arg0[] = "test";