work on trajectory, update cobboard and ballboard too

[aversive.git] / projects / microb2010 / mainboard / commands_traj.c

diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c
index 0d83688..7bf6057 100644 (file)
--- a/projects/microb2010/mainboard/commands_traj.c
+++ b/projects/microb2010/mainboard/commands_traj.c
@@ -23,6 +23,7 @@
 #include <stdio.h>
 #include <string.h>
 
 #include <stdio.h>
 #include <string.h>
 

+#include <hostsim.h>

 #include <aversive/pgmspace.h>
 #include <aversive/wait.h>
 #include <aversive/error.h>
 #include <aversive/pgmspace.h>
 #include <aversive/wait.h>
 #include <aversive/error.h>


@@ -38,6 +39,7 @@
 #include <quadramp.h>
 #include <control_system_manager.h>
 #include <trajectory_manager.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>
 #include <vect_base.h>
 #include <lines.h>
 #include <polygon.h>


@@ -144,7 +146,7 @@ static void cmd_circle_coef_parsed(void *parsed_result, void *data)
                trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
        }
 
                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";
 }
 
 prog_char str_circle_coef_arg0[] = "circle_coef";


@@ -264,6 +266,9 @@ struct cmd_rs_gains_result {
 /* function called when cmd_rs_gains is parsed successfully */
 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
 {
 /* 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"))) {
        struct cmd_rs_gains_result * res = parsed_result;
 
        if (!strcmp_P(res->arg1, PSTR("set"))) {


@@ -274,6 +279,7 @@ static void cmd_rs_gains_parsed(void * parsed_result, void * data)
        }
        printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
                 mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
        }
        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";
 }
 
 prog_char str_rs_gains_arg0[] = "rs_gains";


@@ -395,14 +401,13 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data)
        struct cmd_pt_list_result * res = parsed_result;
        uint8_t i, why=0;
        
        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("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"))) {
 
        if (!strcmp_P(res->arg1, PSTR("insert")) ||
            !strcmp_P(res->arg1, PSTR("append"))) {


@@ -442,12 +447,17 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data)
                printf_P(PSTR("List empty\r\n"));
                return;
        }
                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 */
                }
        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) {
 #if 0
                else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
                        while (1) {


@@ -460,7 +470,13 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data)
 #endif
                if (why & (~(END_TRAJ | END_NEAR)))
                        trajectory_stop(&mainboard.traj);
 #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";
 }
 
 prog_char str_pt_list_arg0[] = "pt_list";


@@ -524,7 +540,7 @@ parse_pgm_inst_t cmd_pt_list_del = {
 };
 /* show */
 
 };
 /* 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";
 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";


@@ -686,43 +702,46 @@ static void auto_position(void)
        strat_get_speed(&old_spdd, &old_spda);
        strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
 
        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);
        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;
 
        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;
 
        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);
        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);
        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);
        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;
 
        strat_set_speed(old_spdd, old_spda);
        return;
 


@@ -735,7 +754,7 @@ intr:
 static void cmd_position_parsed(void * parsed_result, void * data)
 {
        struct cmd_position_result * res = parsed_result;
 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);
        /* display raw position values */
        if (!strcmp_P(res->arg1, PSTR("reset"))) {
                position_set(&mainboard.pos, 0, 0, 0);


@@ -743,16 +762,20 @@ static void cmd_position_parsed(void * parsed_result, void * data)
        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("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();
        }
                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();
        }
 
                auto_position();
        }
 


@@ -765,7 +788,7 @@ static void cmd_position_parsed(void * parsed_result, void * data)
 
 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_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";
 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";