add climb fct
[aversive.git] / projects / microb2010 / mainboard / commands_mainboard.c
index a69208b..fc51e8e 100644 (file)
@@ -66,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"
 
@@ -295,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;
        }
 
@@ -326,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";
@@ -732,9 +730,9 @@ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
        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);
 }
 
 prog_char str_cobboard_setmode1_arg0[] = "cobboard";
@@ -765,7 +763,7 @@ 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)
 {
        struct cmd_cobboard_setmode2_result *res = parsed_result;
        uint8_t side = I2C_LEFT_SIDE;
@@ -775,17 +773,31 @@ 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);
+       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("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#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);
@@ -1063,139 +1075,6 @@ 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, &center) != 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(&center, &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)
 {
@@ -1253,6 +1132,85 @@ parse_pgm_inst_t cmd_clitoid = {
 };
 
 /**********************************************************/
+/* Time_Monitor */
+
+/* this structure is filled when cmd_time_monitor is parsed successfully */
+struct cmd_time_monitor_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_time_monitor is parsed successfully */
+static void cmd_time_monitor_parsed(void *parsed_result, void *data)
+{
+#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
+}
+
+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)
+{
+       struct cmd_strat_event_result *res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("on")))
+               strat_event_enable();
+       else
+               strat_event_disable();
+}
+
+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,
+       },
+};
+
+/**********************************************************/
 /* Test */
 
 /* this structure is filled when cmd_test is parsed successfully */
@@ -1265,7 +1223,18 @@ struct cmd_test_result {
 /* function called when cmd_test is parsed successfully */
 static void cmd_test_parsed(void *parsed_result, void *data)
 {
-       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";
@@ -1283,3 +1252,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)<DIM_BALLS_UP, 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_DOWN, 0xFF);
+       printf_P(PSTR("ball_deploy\r\n"));
+       support_balls_deploy();
+
+       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+}
+
+prog_char str_climb_arg0[] = "climb";
+parse_pgm_token_string_t cmd_climb_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_climb_result, arg0, str_climb_arg0);
+parse_pgm_token_num_t cmd_climb_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_climb_result, radius, INT32);
+parse_pgm_token_num_t cmd_climb_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_climb_result, dist, INT32);
+
+prog_char help_climb[] = "Climb function";
+parse_pgm_inst_t cmd_climb = {
+       .f = cmd_climb_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_climb,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_climb_arg0,
+               NULL,
+       },
+};