+/**********************************************************/
+/* Ballboard_Show */
+
+/* this structure is filled when cmd_ballboard_show is parsed successfully */
+struct cmd_ballboard_show_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_ballboard_show is parsed successfully */
+static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
+{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
+ 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);
+#endif
+}
+
+prog_char str_ballboard_show_arg0[] = "ballboard";
+parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0);
+prog_char str_ballboard_show_arg1[] = "show";
+parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1);
+
+prog_char help_ballboard_show[] = "show ballboard status";
+parse_pgm_inst_t cmd_ballboard_show = {
+ .f = cmd_ballboard_show_parsed, /* function to call */
+ .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,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Ballboard_Setmode1 */
+
+/* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */
+struct cmd_ballboard_setmode1_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* 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")))
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT);
+ else if (!strcmp_P(res->arg1, PSTR("off")))
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF);
+ else if (!strcmp_P(res->arg1, PSTR("eject")))
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
+ else if (!strcmp_P(res->arg1, PSTR("harvest")))
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
+
+ /* other commands */
+#endif
+}
+
+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";
+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)";
+parse_pgm_inst_t cmd_ballboard_setmode1 = {
+ .f = cmd_ballboard_setmode1_parsed, /* function to call */
+ .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,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Ballboard_Setmode2 */
+
+/* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */
+struct cmd_ballboard_setmode2_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+/* 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;
+
+ 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);
+#endif
+}
+
+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";
+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);
+
+prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)";
+parse_pgm_inst_t cmd_ballboard_setmode2 = {
+ .f = cmd_ballboard_setmode2_parsed, /* function to call */
+ .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,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Ballboard_Setmode3 */
+
+/* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */
+struct cmd_ballboard_setmode3_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t level;
+};
+
+/* 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";
+parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0);
+prog_char str_ballboard_setmode3_arg1[] = "xxx";
+parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1);
+parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8);
+
+prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)";
+parse_pgm_inst_t cmd_ballboard_setmode3 = {
+ .f = cmd_ballboard_setmode3_parsed, /* function to call */
+ .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,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Servo_Balls */
+
+/* this structure is filled when cmd_servo_balls is parsed successfully */
+struct cmd_servo_balls_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_servo_balls is parsed successfully */
+static void cmd_servo_balls_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_servo_balls_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("deploy")))
+ support_balls_deploy();
+ else if (!strcmp_P(res->arg1, PSTR("pack")))
+ support_balls_pack();
+}
+
+prog_char str_servo_balls_arg0[] = "support_balls";
+parse_pgm_token_string_t cmd_servo_balls_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0);
+prog_char str_servo_balls_arg1[] = "deploy#pack";
+parse_pgm_token_string_t cmd_servo_balls_arg1 =
+ TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1);
+
+prog_char help_servo_balls[] = "control support balls";
+parse_pgm_inst_t cmd_servo_balls = {
+ .f = cmd_servo_balls_parsed, /* function to call */
+ .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,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Clitoid */
+
+/* this structure is filled when cmd_clitoid is parsed successfully */
+struct cmd_clitoid_result {
+ fixed_string_t arg0;
+ float alpha_deg;
+ float beta_deg;
+ float R_mm;
+ float Vd;
+ float Amax;
+ 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);
+}
+
+prog_char str_clitoid_arg0[] = "clitoid";
+parse_pgm_token_string_t cmd_clitoid_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_clitoid_result,
+ arg0, str_clitoid_arg0);
+parse_pgm_token_num_t cmd_clitoid_alpha_deg =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ alpha_deg, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_beta_deg =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ beta_deg, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_R_mm =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ R_mm, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_Vd =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ Vd, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_Amax =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ Amax, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_d_inter_mm =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ d_inter_mm, FLOAT);
+
+prog_char help_clitoid[] = "do a clitoid (alpha, beta, R, Vd, Amax, d_inter)";
+parse_pgm_inst_t cmd_clitoid = {
+ .f = cmd_clitoid_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_clitoid,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_clitoid_arg0,
+ (prog_void *)&cmd_clitoid_alpha_deg,
+ (prog_void *)&cmd_clitoid_beta_deg,
+ (prog_void *)&cmd_clitoid_R_mm,
+ (prog_void *)&cmd_clitoid_Vd,
+ (prog_void *)&cmd_clitoid_Amax,
+ (prog_void *)&cmd_clitoid_d_inter_mm,
+ NULL,
+ },
+};
+
+//////////////////////
+
+// 500 -- 5
+// 400 -- 3
+#define TEST_SPEED 400
+#define TEST_ACC 3
+
+static void line2line(double line1x1, double line1y1,
+ double line1x2, double line1y2,
+ double line2x1, double line2y1,
+ double line2x2, double line2y2,
+ double radius, double dist)
+{
+ 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);
+
+ /* take some margin on dist to avoid deceleration */
+ trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
+
+ /* circle exit condition */
+ err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
+ TRAJ_FLAGS_NO_NEAR);
+
+ 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.);
+}
+
+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)
+{
+ 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.);
+}
+