#include <stdio.h>
#include <string.h>
+#include <hostsim.h>
#include <aversive/pgmspace.h>
#include <aversive/wait.h>
#include <aversive/error.h>
#include <quadramp.h>
#include <control_system_manager.h>
#include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
#include <vect_base.h>
#include <lines.h>
#include <polygon.h>
trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
}
- printf_P(PSTR("circle_coef %2.2f\r\n"), mainboard.traj.circle_coef);
+ printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef);
}
prog_char str_circle_coef_arg0[] = "circle_coef";
/* function called when cmd_rs_gains is parsed successfully */
static void cmd_rs_gains_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_rs_gains_result * res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("set"))) {
}
printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
+#endif
}
prog_char str_rs_gains_arg0[] = "rs_gains";
struct cmd_pt_list_result * res = parsed_result;
uint8_t i, why=0;
- if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
- printf_P(PSTR("not implemented\r\n"));
- return;
- }
-
if (!strcmp_P(res->arg1, PSTR("append"))) {
res->arg2 = pt_list_len;
}
+ if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
+ printf_P(PSTR("removed\r\n"));
+ return;
+ }
if (!strcmp_P(res->arg1, PSTR("insert")) ||
!strcmp_P(res->arg1, PSTR("append"))) {
printf_P(PSTR("List empty\r\n"));
return;
}
+ restart:
for (i=0 ; i<pt_list_len ; i++) {
printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
if (!strcmp_P(res->arg1, PSTR("start"))) {
trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
why = wait_traj_end(0xFF); /* all */
}
+ else if (!strcmp_P(res->arg1, PSTR("loop_start"))) {
+ trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
+ why = wait_traj_end(0xFF); /* all */
+ }
#if 0
else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
while (1) {
#endif
if (why & (~(END_TRAJ | END_NEAR)))
trajectory_stop(&mainboard.traj);
+ if (why & END_INTR)
+ break;
}
+ if (why & END_INTR)
+ return;
+ if (!strcmp_P(res->arg1, PSTR("loop_start")))
+ goto restart;
}
prog_char str_pt_list_arg0[] = "pt_list";
};
/* show */
-prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start";
+prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start";
parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
prog_char help_pt_list_show[] = "Show, start or reset pt_list";
strat_get_speed(&old_spdd, &old_spda);
strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
- trajectory_d_rel(&mainboard.traj, -300);
+ trajectory_d_rel(&mainboard.traj, 300);
err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
if (err == END_INTR)
goto intr;
wait_ms(100);
- strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
+ strat_reset_pos(ROBOT_WIDTH/2,
+ COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
+ COLOR_A(-90));
- trajectory_d_rel(&mainboard.traj, 120);
+ trajectory_d_rel(&mainboard.traj, -180);
err = wait_traj_end(END_INTR|END_TRAJ);
if (err == END_INTR)
goto intr;
- trajectory_a_rel(&mainboard.traj, COLOR_A(90));
+ trajectory_a_rel(&mainboard.traj, COLOR_A(-90));
err = wait_traj_end(END_INTR|END_TRAJ);
if (err == END_INTR)
goto intr;
- trajectory_d_rel(&mainboard.traj, -300);
+ trajectory_d_rel(&mainboard.traj, 300);
err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
if (err == END_INTR)
goto intr;
wait_ms(100);
- strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2),
- COLOR_A(90));
+ strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
+ DO_NOT_SET_POS,
+ 180);
- trajectory_d_rel(&mainboard.traj, 120);
+ trajectory_d_rel(&mainboard.traj, -170);
err = wait_traj_end(END_INTR|END_TRAJ);
if (err == END_INTR)
goto intr;
wait_ms(100);
-
- trajectory_a_rel(&mainboard.traj, COLOR_A(-40));
+
+ trajectory_a_rel(&mainboard.traj, COLOR_A(-110));
err = wait_traj_end(END_INTR|END_TRAJ);
if (err == END_INTR)
goto intr;
wait_ms(100);
-
+
strat_set_speed(old_spdd, old_spda);
return;
static void cmd_position_parsed(void * parsed_result, void * data)
{
struct cmd_position_result * res = parsed_result;
-
+
/* display raw position values */
if (!strcmp_P(res->arg1, PSTR("reset"))) {
position_set(&mainboard.pos, 0, 0, 0);
else if (!strcmp_P(res->arg1, PSTR("set"))) {
position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
}
- else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) {
- mainboard.our_color = I2C_COLOR_GREEN;
- i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_GREEN);
- i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_GREEN);
+ else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
+ mainboard.our_color = I2C_COLOR_BLUE;
+#ifndef HOST_VERSION
+ i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
+ i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
+#endif
auto_position();
}
- else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
- mainboard.our_color = I2C_COLOR_RED;
- i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_RED);
- i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_RED);
+ else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) {
+ mainboard.our_color = I2C_COLOR_YELLOW;
+#ifndef HOST_VERSION
+ i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
+ i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
+#endif
auto_position();
}
prog_char str_position_arg0[] = "position";
parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
-prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red";
+prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow";
parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
prog_char help_position[] = "Show/reset (x,y,a) position";