X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_mainboard.c;h=2335ce38662fc347a09540081d0c547470bf8724;hp=fe4fabbc0aa9f8e78f5dc1559e2364d8dcb86e28;hb=90945acf48061ebc3960195cabcb11175475e426;hpb=fa8546ea39c7442ad3bf5a822a72a2b50a41045d diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index fe4fabb..2335ce3 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -1,6 +1,6 @@ /* * 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 @@ -17,7 +17,7 @@ * * Revision : $Id: commands_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -65,6 +66,8 @@ #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" @@ -81,29 +84,30 @@ static void cmd_event_parsed(void *parsed_result, 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_RS | DO_POS | - DO_BD | DO_TIMER | DO_POWER; + bit = 0xFF; if (!strcmp_P(res->arg2, PSTR("on"))) mainboard.flags |= bit; else if (!strcmp_P(res->arg2, PSTR("off"))) mainboard.flags &= bit; else { /* show */ - printf_P(PSTR("encoders is %s\r\n"), + printf_P(PSTR("encoders is %s\r\n"), (DO_ENCODERS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("cs is %s\r\n"), + printf_P(PSTR("cs is %s\r\n"), (DO_CS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("rs is %s\r\n"), + printf_P(PSTR("rs is %s\r\n"), (DO_RS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("pos is %s\r\n"), + printf_P(PSTR("pos is %s\r\n"), (DO_POS & mainboard.flags) ? "on":"off"); - printf_P(PSTR("bd is %s\r\n"), + printf_P(PSTR("bd is %s\r\n"), (DO_BD & mainboard.flags) ? "on":"off"); - printf_P(PSTR("timer is %s\r\n"), + printf_P(PSTR("timer is %s\r\n"), (DO_TIMER & mainboard.flags) ? "on":"off"); - printf_P(PSTR("power is %s\r\n"), + printf_P(PSTR("power is %s\r\n"), (DO_POWER & mainboard.flags) ? "on":"off"); + printf_P(PSTR("errblock is %s\r\n"), + (DO_ERRBLOCKING & mainboard.flags) ? "on":"off"); } return; } @@ -126,6 +130,8 @@ static void cmd_event_parsed(void *parsed_result, void *data) } else if (!strcmp_P(res->arg1, PSTR("power"))) bit = DO_POWER; + else if (!strcmp_P(res->arg1, PSTR("errblock"))) + bit = DO_ERRBLOCKING; if (!strcmp_P(res->arg2, PSTR("on"))) mainboard.flags |= bit; @@ -141,13 +147,13 @@ static void cmd_event_parsed(void *parsed_result, void *data) } mainboard.flags &= (~bit); } - printf_P(PSTR("%s is %s\r\n"), res->arg1, + printf_P(PSTR("%s is %s\r\n"), res->arg1, (bit & mainboard.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#rs#pos#bd#timer#power"; +prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power#errblock"; 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); @@ -158,9 +164,9 @@ parse_pgm_inst_t cmd_event = { .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, + (prog_void *)&cmd_event_arg0, + (prog_void *)&cmd_event_arg1, + (prog_void *)&cmd_event_arg2, NULL, }, }; @@ -181,7 +187,7 @@ static void cmd_spi_test_parsed(void * parsed_result, void * data) printf("not implemented\n"); #else uint16_t i = 0, ret = 0, ret2 = 0; - + if (mainboard.flags & DO_ENCODERS) { printf_P(PSTR("Disable encoder event first\r\n")); return; @@ -211,7 +217,7 @@ parse_pgm_inst_t cmd_spi_test = { .data = NULL, /* 2nd arg of func */ .help_str = help_spi_test, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spi_test_arg0, + (prog_void *)&cmd_spi_test_arg0, NULL, }, }; @@ -251,8 +257,8 @@ parse_pgm_inst_t cmd_opponent = { .data = NULL, /* 2nd arg of func */ .help_str = help_opponent, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_opponent_arg0, - (prog_void *)&cmd_opponent_arg1, + (prog_void *)&cmd_opponent_arg0, + (prog_void *)&cmd_opponent_arg1, NULL, }, }; @@ -269,10 +275,10 @@ parse_pgm_inst_t cmd_opponent_set = { .data = NULL, /* 2nd arg of func */ .help_str = help_opponent_set, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_opponent_arg0, + (prog_void *)&cmd_opponent_arg0, (prog_void *)&cmd_opponent_arg1_set, - (prog_void *)&cmd_opponent_arg2, - (prog_void *)&cmd_opponent_arg3, + (prog_void *)&cmd_opponent_arg2, + (prog_void *)&cmd_opponent_arg3, NULL, }, }; @@ -291,19 +297,16 @@ struct cmd_start_result { /* 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; } @@ -322,7 +325,6 @@ static void cmd_start_parsed(void *parsed_result, void *data) gen.logs[NB_LOGS] = 0; gen.log_level = old_level; -#endif } prog_char str_start_arg0[] = "start"; @@ -338,9 +340,9 @@ parse_pgm_inst_t cmd_start = { .data = NULL, /* 2nd arg of func */ .help_str = help_start, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_start_arg0, - (prog_void *)&cmd_start_color, - (prog_void *)&cmd_start_debug, + (prog_void *)&cmd_start_arg0, + (prog_void *)&cmd_start_color, + (prog_void *)&cmd_start_debug, NULL, }, }; @@ -358,7 +360,7 @@ struct cmd_interact_result { static void print_cs(void) { printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" " - "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"), + "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"), cs_get_consign(&mainboard.distance.cs), cs_get_consign(&mainboard.angle.cs), cs_get_filtered_consign(&mainboard.distance.cs), @@ -371,7 +373,7 @@ static void print_cs(void) static void print_pos(void) { - printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), + printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), position_get_x_s16(&mainboard.pos), position_get_y_s16(&mainboard.pos), position_get_a_deg_s16(&mainboard.pos)); @@ -484,7 +486,7 @@ static void cmd_interact_parsed(void * parsed_result, void * data) wait_ms(10); continue; } - + if (cmd == -1) { switch(c) { case '1': print ^= PRINT_POS; break; @@ -494,7 +496,7 @@ static void cmd_interact_parsed(void * parsed_result, void * data) case '5': print ^= PRINT_TIME; break; case '6': print ^= PRINT_BLOCKING; break; - case 'q': + case 'q': if (mainboard.flags & DO_CS) strat_hardstop(); pwm_set_and_save(LEFT_PWM, 0); @@ -504,27 +506,32 @@ static void cmd_interact_parsed(void * parsed_result, void * data) pwm_set_and_save(LEFT_PWM, 0); pwm_set_and_save(RIGHT_PWM, 0); break; - default: + default: break; } } else { +#ifdef HOST_VERSION +#define PWM_INTERACT 300 +#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); + case KEY_UP_ARR: + 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); + case KEY_LEFT_ARR: + 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); + case KEY_DOWN_ARR: + 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; } } @@ -541,7 +548,7 @@ parse_pgm_inst_t cmd_interact = { .data = NULL, /* 2nd arg of func */ .help_str = help_interact, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_interact_arg0, + (prog_void *)&cmd_interact_arg0, NULL, }, }; @@ -588,8 +595,8 @@ parse_pgm_inst_t cmd_color = { .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, + (prog_void *)&cmd_color_arg0, + (prog_void *)&cmd_color_color, NULL, }, }; @@ -609,11 +616,11 @@ static void cmd_rs_parsed(void *parsed_result, void *data) { // struct cmd_rs_result *res = parsed_result; do { - printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), + printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), cs_get_consign(&mainboard.angle.cs), cs_get_filtered_feedback(&mainboard.angle.cs), cs_get_out(&mainboard.angle.cs)); - printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), + printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), cs_get_consign(&mainboard.distance.cs), cs_get_filtered_feedback(&mainboard.distance.cs), cs_get_out(&mainboard.distance.cs)); @@ -634,8 +641,8 @@ parse_pgm_inst_t cmd_rs = { .data = NULL, /* 2nd arg of func */ .help_str = help_rs, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_rs_arg0, - (prog_void *)&cmd_rs_arg1, + (prog_void *)&cmd_rs_arg0, + (prog_void *)&cmd_rs_arg1, NULL, }, }; @@ -668,7 +675,7 @@ parse_pgm_inst_t cmd_i2cdebug = { .data = NULL, /* 2nd arg of func */ .help_str = help_i2cdebug, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_i2cdebug_arg0, + (prog_void *)&cmd_i2cdebug_arg0, NULL, }, }; @@ -707,8 +714,8 @@ parse_pgm_inst_t cmd_cobboard_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_show_arg0, - (prog_void *)&cmd_cobboard_show_arg1, + (prog_void *)&cmd_cobboard_show_arg0, + (prog_void *)&cmd_cobboard_show_arg1, NULL, }, }; @@ -725,16 +732,12 @@ struct cmd_cobboard_setmode1_result { /* function called when cmd_cobboard_setmode1 is parsed successfully */ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else 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(); -#endif + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); } prog_char str_cobboard_setmode1_arg0[] = "cobboard"; @@ -748,8 +751,8 @@ parse_pgm_inst_t cmd_cobboard_setmode1 = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_setmode1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_setmode1_arg0, - (prog_void *)&cmd_cobboard_setmode1_arg1, + (prog_void *)&cmd_cobboard_setmode1_arg0, + (prog_void *)&cmd_cobboard_setmode1_arg1, NULL, }, }; @@ -765,11 +768,8 @@ struct cmd_cobboard_setmode2_result { }; /* 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) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_cobboard_setmode2_result *res = parsed_result; uint8_t side = I2C_LEFT_SIDE; @@ -778,18 +778,35 @@ static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) 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); -#endif + 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("pack_weak"))) { + 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#pack_weak#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); @@ -800,9 +817,9 @@ parse_pgm_inst_t cmd_cobboard_setmode2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_setmode2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_setmode2_arg0, - (prog_void *)&cmd_cobboard_setmode2_arg1, - (prog_void *)&cmd_cobboard_setmode2_arg2, + (prog_void *)&cmd_cobboard_setmode2_arg0, + (prog_void *)&cmd_cobboard_setmode2_arg1, + (prog_void *)&cmd_cobboard_setmode2_arg2, NULL, }, }; @@ -820,13 +837,9 @@ struct cmd_cobboard_setmode3_result { /* function called when cmd_cobboard_setmode3 is parsed successfully */ static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_cobboard_setmode3_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("xxx"))) printf("faux\r\n"); -#endif } prog_char str_cobboard_setmode3_arg0[] = "cobboard"; @@ -841,9 +854,9 @@ parse_pgm_inst_t cmd_cobboard_setmode3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobboard_setmode3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobboard_setmode3_arg0, - (prog_void *)&cmd_cobboard_setmode3_arg1, - (prog_void *)&cmd_cobboard_setmode3_arg2, + (prog_void *)&cmd_cobboard_setmode3_arg0, + (prog_void *)&cmd_cobboard_setmode3_arg1, + (prog_void *)&cmd_cobboard_setmode3_arg2, NULL, }, }; @@ -866,6 +879,8 @@ static void cmd_ballboard_show_parsed(void * parsed_result, void * data) 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 } @@ -880,8 +895,8 @@ parse_pgm_inst_t cmd_ballboard_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_show_arg0, - (prog_void *)&cmd_ballboard_show_arg1, + (prog_void *)&cmd_ballboard_show_arg0, + (prog_void *)&cmd_ballboard_show_arg1, NULL, }, }; @@ -898,9 +913,6 @@ struct cmd_ballboard_setmode1_result { /* function called when cmd_ballboard_setmode1 is parsed successfully */ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_ballboard_setmode1_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("init"))) @@ -913,7 +925,6 @@ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); /* other commands */ -#endif } prog_char str_ballboard_setmode1_arg0[] = "ballboard"; @@ -927,8 +938,8 @@ parse_pgm_inst_t cmd_ballboard_setmode1 = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_setmode1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_setmode1_arg0, - (prog_void *)&cmd_ballboard_setmode1_arg1, + (prog_void *)&cmd_ballboard_setmode1_arg0, + (prog_void *)&cmd_ballboard_setmode1_arg1, NULL, }, }; @@ -946,9 +957,6 @@ struct cmd_ballboard_setmode2_result { /* function called when cmd_ballboard_setmode2 is parsed successfully */ static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_ballboard_setmode2_result *res = parsed_result; uint8_t mode = I2C_BALLBOARD_MODE_INIT; @@ -965,7 +973,6 @@ static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data) mode = I2C_BALLBOARD_MODE_TAKE_R_FORK; } i2c_ballboard_set_mode(mode); -#endif } prog_char str_ballboard_setmode2_arg0[] = "ballboard"; @@ -981,9 +988,9 @@ parse_pgm_inst_t cmd_ballboard_setmode2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_setmode2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_setmode2_arg0, - (prog_void *)&cmd_ballboard_setmode2_arg1, - (prog_void *)&cmd_ballboard_setmode2_arg2, + (prog_void *)&cmd_ballboard_setmode2_arg0, + (prog_void *)&cmd_ballboard_setmode2_arg1, + (prog_void *)&cmd_ballboard_setmode2_arg2, NULL, }, }; @@ -1001,13 +1008,9 @@ struct cmd_ballboard_setmode3_result { /* function called when cmd_ballboard_setmode3 is parsed successfully */ static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_ballboard_setmode3_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("xxx"))) printf("faux\r\n"); -#endif } prog_char str_ballboard_setmode3_arg0[] = "ballboard"; @@ -1022,9 +1025,9 @@ parse_pgm_inst_t cmd_ballboard_setmode3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_ballboard_setmode3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_ballboard_setmode3_arg0, - (prog_void *)&cmd_ballboard_setmode3_arg1, - (prog_void *)&cmd_ballboard_setmode3_arg2, + (prog_void *)&cmd_ballboard_setmode3_arg0, + (prog_void *)&cmd_ballboard_setmode3_arg1, + (prog_void *)&cmd_ballboard_setmode3_arg2, NULL, }, }; @@ -1063,8 +1066,8 @@ parse_pgm_inst_t cmd_servo_balls = { .data = NULL, /* 2nd arg of func */ .help_str = help_servo_balls, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_servo_balls_arg0, - (prog_void *)&cmd_servo_balls_arg1, + (prog_void *)&cmd_servo_balls_arg0, + (prog_void *)&cmd_servo_balls_arg1, NULL, }, }; @@ -1083,145 +1086,20 @@ struct cmd_clitoid_result { float d_inter_mm; }; -/** - * do a superb curve joining line1 to line2 which is composed of: - * - a clothoid starting from line1 - * - a circle - * - another clothoid up to line2 - * - * the function assumes that the initial linear speed is Vd and - * angular speed is 0. - * - * - alpha: total angle - * - beta: circular part of angle (lower than alpha) - * - R: the radius of the circle (must be != 0) - * - Vd: linear speed to use (in imp per cs period) - * - Amax: maximum angular acceleration - * - d_inter: distance in mm until the intersection of the - * 2 lines - * - * return 0 on success: in this case these parameters are filled: - * - Aa_out: the angular acceleration to configure in quadramp - * - remain_d_mm_out: remaining distance before start to turn - */ -uint8_t clitoid(double alpha_deg, double beta_deg, double R_mm, - double Vd, double Amax, double d_inter_mm) -{ - double Vd_mm_s; - double Va, Va_rd_s; - double t, d_mm, alpha_rad, beta_rad; - double remain_d_mm; - double Aa, Aa_rd_s2; - line_t line1, line2; - double x, y, a_rad; - point_t robot, intersect, pt2, center, proj; - vect_t v; - - /* param check */ - if (fabs(alpha_deg) <= fabs(beta_deg)) { - DEBUG(E_USER_STRAT, "alpha is smaller than beta"); - return END_ERROR; - } - - /* get angular speed Va */ - Vd_mm_s = Vd * (CS_HZ/DIST_IMP_MM); - DEBUG(E_USER_STRAT, "Vd_mm_s=%2.2f", Vd_mm_s); - Va_rd_s = Vd_mm_s / R_mm; - Va = Va_rd_s * (DIST_IMP_MM * EXT_TRACK_MM / (2 * CS_HZ)); - DEBUG(E_USER_STRAT, "Va_rd_s=%2.2f Va=%2.2f", Va_rd_s, Va); - - /* process 't', the time in seconds that we will take to do - * the first clothoid */ - alpha_rad = RAD(alpha_deg); - beta_rad = RAD(beta_deg); - t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s); - DEBUG(E_USER_STRAT, "R_mm=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f", - R_mm, alpha_rad, beta_rad, t); - - /* process the angular acceleration */ - Aa_rd_s2 = Va_rd_s / t; - Aa = Aa_rd_s2 * (DIST_IMP_MM * EXT_TRACK_MM / - (2 * CS_HZ * CS_HZ)); - DEBUG(E_USER_STRAT, "Aa_rd_s2=%2.2f Aa=%2.2f", Aa_rd_s2, Aa); - - /* exit if the robot cannot physically do it */ - if (Aa > Amax) { - DEBUG(E_USER_STRAT, "greater than max acceleration"); - return END_ERROR; - } - - /* the robot position */ - x = position_get_x_double(&mainboard.pos); - y = position_get_y_double(&mainboard.pos); - a_rad = position_get_a_rad_double(&mainboard.pos); - - /* define line1 and line2 */ - robot.x = x; - robot.y = y; - intersect.x = x + cos(a_rad) * d_inter_mm; - intersect.y = y + sin(a_rad) * d_inter_mm; - pts2line(&robot, &intersect, &line1); - pt2.x = intersect.x + cos(a_rad + alpha_rad); - pt2.y = intersect.y + sin(a_rad + alpha_rad); - pts2line(&intersect, &pt2, &line2); - DEBUG(E_USER_STRAT, "intersect=(%2.2f, %2.2f)", - intersect.x, intersect.y); - - /* the center of the circle is at (d_mm, d_mm) when we have to - * start the clothoid */ - d_mm = R_mm * sqrt(fabs(alpha_rad - beta_rad)) * - sqrt(M_PI) / 2.; - DEBUG(E_USER_STRAT, "d_mm=%2.2f", d_mm); - - /* translate line1 */ - v.x = intersect.x - robot.x; - v.y = intersect.y - robot.y; - if (a_rad > 0) - vect_rot_trigo(&v); - else - vect_rot_retro(&v); - vect_resize(&v, d_mm); - line_translate(&line1, &v); - - /* translate line2 */ - v.x = intersect.x - pt2.x; - v.y = intersect.y - pt2.y; - if (a_rad > 0) - vect_rot_trigo(&v); - else - vect_rot_retro(&v); - vect_resize(&v, d_mm); - line_translate(&line2, &v); - - /* find the center of the circle, at the intersection of the - * new translated lines */ - if (intersect_line(&line1, &line2, ¢er) != 1) { - DEBUG(E_USER_STRAT, "cannot find circle center"); - return END_ERROR; - } - DEBUG(E_USER_STRAT, "center=(%2.2f,%2.2f)", center.x, center.y); - - /* project center of circle on line1 */ - proj_pt_line(¢er, &line1, &proj); - DEBUG(E_USER_STRAT, "proj=(%2.2f,%2.2f)", proj.x, proj.y); - - /* process remaining distance before start turning */ - remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + d_mm); - DEBUG(E_USER_STRAT, "remain_d=%2.2f", remain_d_mm); - if (remain_d_mm < 0) { - DEBUG(E_USER_STRAT, "too late, cannot turn"); - return END_ERROR; - } - - return END_TRAJ; -} - /* function called when cmd_test is parsed successfully */ static void cmd_clitoid_parsed(void *parsed_result, void *data) { struct cmd_clitoid_result *res = parsed_result; - clitoid(res->alpha_deg, res->beta_deg, res->R_mm, - res->Vd, res->Amax, res->d_inter_mm); +/* clitoid(res->alpha_deg, res->beta_deg, res->R_mm, */ +/* res->Vd, res->Amax, res->d_inter_mm); */ + double x = position_get_x_double(&mainboard.pos); + double y = position_get_y_double(&mainboard.pos); + double a = position_get_a_rad_double(&mainboard.pos); + + strat_set_speed(res->Vd, SPEED_ANGLE_FAST); + trajectory_clitoid(&mainboard.traj, x, y, a, 150., + res->alpha_deg, res->beta_deg, res->R_mm, + res->d_inter_mm); } prog_char str_clitoid_arg0[] = "clitoid"; @@ -1264,127 +1142,118 @@ parse_pgm_inst_t cmd_clitoid = { }, }; -////////////////////// +/**********************************************************/ +/* Time_Monitor */ -// 500 -- 5 -// 400 -- 3 -#define TEST_SPEED 400 -#define TEST_ACC 3 +/* this structure is filled when cmd_time_monitor is parsed successfully */ +struct cmd_time_monitor_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; -static void line2line(double line1x1, double line1y1, - double line1x2, double line1y2, - double line2x1, double line2y1, - double line2x2, double line2y2, - double radius, double dist) +/* function called when cmd_time_monitor is parsed successfully */ +static void cmd_time_monitor_parsed(void *parsed_result, void *data) { - uint8_t err; - double speed_d, speed_a; - double distance, angle; - double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1); - double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1); - - printf_P(PSTR("%s()\r\n"), __FUNCTION__); - - strat_set_speed(TEST_SPEED, TEST_SPEED); - quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC); - - circle_get_da_speed_from_radius(&mainboard.traj, radius, - &speed_d, &speed_a); - trajectory_line_abs(&mainboard.traj, - line1x1, line1y1, - line1x2, line1y2, 150.); - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) < - dist, TRAJ_FLAGS_NO_NEAR); - /* circle */ - strat_set_speed(speed_d, speed_a); - angle = line2_angle - line1_angle; - distance = angle * radius; - if (distance < 0) - distance = -distance; - angle = simple_modulo_2pi(angle); - angle = DEG(angle); - printf_P(PSTR("(%d,%d,%d) "), - position_get_x_s16(&mainboard.pos), - position_get_y_s16(&mainboard.pos), - position_get_a_deg_s16(&mainboard.pos)); - printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"), - distance, angle); +#ifndef HOST_VERSION + struct cmd_time_monitor_result *res = parsed_result; + uint16_t seconds; - /* take some margin on dist to avoid deceleration */ - trajectory_d_a_rel(&mainboard.traj, distance + 250, angle); + 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 +} - /* circle exit condition */ - err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), - TRAJ_FLAGS_NO_NEAR); +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); - strat_set_speed(500, 500); - printf_P(PSTR("(%d,%d,%d) "), - position_get_x_s16(&mainboard.pos), - position_get_y_s16(&mainboard.pos), - position_get_a_deg_s16(&mainboard.pos)); - printf_P(PSTR("line\r\n")); - trajectory_line_abs(&mainboard.traj, - line2x1, line2y1, - line2x2, line2y2, 150.); +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) +{ + struct cmd_strat_event_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("on"))) + strat_event_enable(); + else + strat_event_disable(); } -static void halfturn(double line1x1, double line1y1, - double line1x2, double line1y2, - double line2x1, double line2y1, - double line2x2, double line2y2, - double radius, double dist, double dir) +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) { - uint8_t err; - double speed_d, speed_a; - double distance, angle; - - printf_P(PSTR("%s()\r\n"), __FUNCTION__); - - strat_set_speed(TEST_SPEED, TEST_SPEED); - quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC); - - circle_get_da_speed_from_radius(&mainboard.traj, radius, - &speed_d, &speed_a); - trajectory_line_abs(&mainboard.traj, - line1x1, line1y1, - line1x2, line1y2, 150.); - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) < - dist, TRAJ_FLAGS_NO_NEAR); - /* circle */ - strat_set_speed(speed_d, speed_a); - angle = dir * M_PI/2.; - distance = angle * radius; - if (distance < 0) - distance = -distance; - angle = simple_modulo_2pi(angle); - angle = DEG(angle); - - /* take some margin on dist to avoid deceleration */ - DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f", - distance, angle); - trajectory_d_a_rel(&mainboard.traj, distance + 500, angle); - - /* circle exit condition */ - err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), - TRAJ_FLAGS_NO_NEAR); - - DEBUG(E_USER_STRAT, "miniline"); - err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) < - dist, TRAJ_FLAGS_NO_NEAR); - DEBUG(E_USER_STRAT, "circle2"); - /* take some margin on dist to avoid deceleration */ - trajectory_d_a_rel(&mainboard.traj, distance + 500, angle); - - err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), - TRAJ_FLAGS_NO_NEAR); - - strat_set_speed(500, 500); - DEBUG(E_USER_STRAT, "line"); - trajectory_line_abs(&mainboard.traj, - line2x1, line2y1, - line2x2, line2y2, 150.); + 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 */ @@ -1398,97 +1267,18 @@ struct cmd_test_result { /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { - // struct cmd_test_result *res = parsed_result; -#ifdef HOST_VERSION - strat_reset_pos(400, 400, 90); - mainboard.angle.on = 1; - mainboard.distance.on = 1; -#endif - printf_P(PSTR("%s()\r\n"), __FUNCTION__); - while (!cmdline_keypressed()) { - /****** PASS1 */ - -#define DIST_HARD_TURN 260 -#define RADIUS_HARD_TURN 100 -#define DIST_EASY_TURN 190 -#define RADIUS_EASY_TURN 190 -#define DIST_HALF_TURN 225 -#define RADIUS_HALF_TURN 130 - - /* hard turn */ - line2line(375, 597, 375, 1847, - 375, 1847, 1050, 1472, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* easy left and easy right !*/ - line2line(825, 1596, 1050, 1472, - 1050, 1472, 1500, 1722, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(1050, 1472, 1500, 1722, - 1500, 1722, 2175, 1347, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(1500, 1722, 2175, 1347, - 2175, 1347, 2175, 847, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* half turns */ - halfturn(2175, 1347, 2175, 722, - 2625, 722, 2625, 1597, - RADIUS_HALF_TURN, DIST_HALF_TURN, 1.); - halfturn(2625, 847, 2625, 1722, - 2175, 1722, 2175, 1097, - RADIUS_HALF_TURN, DIST_HALF_TURN, 1.); - - /* easy turns */ - line2line(2175, 1597, 2175, 1097, - 2175, 1097, 1500, 722, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(2175, 1097, 1500, 722, - 1500, 722, 1050, 972, - RADIUS_EASY_TURN, DIST_EASY_TURN); - line2line(1500, 722, 1050, 972, - 1050, 972, 375, 597, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* hard turn */ - line2line(1050, 972, 375, 597, - 375, 597, 375, 1097, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /****** PASS2 */ - - /* easy turn */ - line2line(375, 597, 375, 1097, - 375, 1097, 1050, 1472, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* hard turn */ - line2line(375, 1097, 1050, 1472, - 1050, 1472, 375, 1847, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* hard turn */ - line2line(1050, 1472, 375, 1847, - 375, 1847, 375, 1347, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* easy turn */ - line2line(375, 1847, 375, 1347, - 375, 1347, 1050, 972, - RADIUS_EASY_TURN, DIST_EASY_TURN); - - /* hard turn */ - line2line(375, 1347, 1050, 972, - 1050, 972, 375, 597, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - /* hard turn */ - line2line(1050, 972, 375, 597, - 375, 597, 375, 1847, - RADIUS_HARD_TURN, DIST_HARD_TURN); - - } - 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"; @@ -1506,3 +1296,75 @@ parse_pgm_inst_t cmd_test = { NULL, }, }; + + + + +/**********************************************************/ +/* Climb */ + +/* this structure is filled when cmd_climb is parsed successfully */ +struct cmd_climb_result { + fixed_string_t arg0; + int32_t radius; + int32_t dist; +}; + + +//#define DIM_BALLS_DOWN 620 +//#define DIM_BALLS_UP 700 + +#define DIM_BALLS_DOWN 530 +#define DIM_BALLS_UP 700 + +/* function called when cmd_climb is parsed successfully */ +static void cmd_climb_parsed(void *parsed_result, void *data) +{ + uint8_t err; + + printf_P(PSTR("starting Clifenger\r\n")); + strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW ); + + trajectory_goto_xy_abs(&mainboard.traj, 250, 250); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + trajectory_a_abs(&mainboard.traj, 0); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + + + trajectory_goto_xy_abs(&mainboard.traj, 1250, 250); + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_DOWN, 0xFF); + printf_P(PSTR("ball_pack\r\n")); + support_balls_pack(); + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_UP, 0xFF); + printf_P(PSTR("ball_deploy\r\n")); + support_balls_deploy(); + err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); + + /* reach top, go down */ + trajectory_goto_xy_abs(&mainboard.traj, 250, 250); + err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)