better movements

diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c
index 879297a..255f26b 100644 (file)
--- a/projects/microb2010/mainboard/commands.c
+++ b/projects/microb2010/mainboard/commands.c
@@ -81,6 +81,7 @@ extern parse_pgm_inst_t cmd_ballboard_setmode3;
 extern parse_pgm_inst_t cmd_beacon_start;
 extern parse_pgm_inst_t cmd_servo_balls;
 extern parse_pgm_inst_t cmd_clitoid;
+extern parse_pgm_inst_t cmd_time_monitor;
 extern parse_pgm_inst_t cmd_test;
 
 /* commands_traj.c */
@@ -171,6 +172,7 @@ parse_pgm_ctx_t main_ctx[] = {
        (parse_pgm_inst_t *)&cmd_ballboard_setmode3,
        (parse_pgm_inst_t *)&cmd_servo_balls,
        (parse_pgm_inst_t *)&cmd_clitoid,
+       (parse_pgm_inst_t *)&cmd_time_monitor,
        (parse_pgm_inst_t *)&cmd_test,
 
        /* commands_traj.c */

diff --git a/projects/microb2010/mainboard/commands_cs.c b/projects/microb2010/mainboard/commands_cs.c
index c749c61..6a3cfc9 100644 (file)
--- a/projects/microb2010/mainboard/commands_cs.c
+++ b/projects/microb2010/mainboard/commands_cs.c
@@ -247,6 +247,7 @@ parse_pgm_inst_t cmd_speed_show = {
 /* this structure is filled when cmd_derivate_filter is parsed successfully */
 struct cmd_derivate_filter_result {
        struct cmd_cs_result cs;
+       fixed_string_t show;
        uint8_t size;
 };
 
@@ -273,12 +274,12 @@ static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
 
 prog_char str_derivate_filter_arg0[] = "derivate_filter";
 parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0);
-parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32);
+parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT8);
 
 prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)";
 parse_pgm_inst_t cmd_derivate_filter = {
        .f = cmd_derivate_filter_parsed,  /* function to call */
-       .data = (void *)1,      /* 2nd arg of func */
+       .data = NULL,      /* 2nd arg of func */
        .help_str = help_derivate_filter,
        .tokens = {        /* token list, NULL terminated */
                (prog_void *)&cmd_derivate_filter_arg0,
@@ -290,18 +291,13 @@ parse_pgm_inst_t cmd_derivate_filter = {
 
 /* show */
 
-struct cmd_derivate_filter_show_result {
-       struct cmd_cs_result cs;
-       fixed_string_t show;
-};
-
 prog_char str_derivate_filter_show_arg[] = "show";
-parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_show_result, show, str_derivate_filter_show_arg);
+parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, show, str_derivate_filter_show_arg);
 
 prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
 parse_pgm_inst_t cmd_derivate_filter_show = {
        .f = cmd_derivate_filter_parsed,  /* function to call */
-       .data = NULL,      /* 2nd arg of func */
+       .data = (void *)1,      /* 2nd arg of func */
        .help_str = help_derivate_filter_show,
        .tokens = {        /* token list, NULL terminated */
                (prog_void *)&cmd_derivate_filter_arg0,

diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
index adec77f..7c58bb9 100644 (file)
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -1123,6 +1123,47 @@ 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,
+       },
+};
+
 /**********************************************************/
 /* Test */
 

diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c
index 516f00b..9884bcf 100644 (file)
--- a/projects/microb2010/mainboard/cs.c
+++ b/projects/microb2010/mainboard/cs.c
@@ -224,7 +224,7 @@ void microb_cs_init(void)
        position_init(&mainboard.pos);
        position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM);
        position_set_related_robot_system(&mainboard.pos, &mainboard.rs);
-       position_set_centrifugal_coef(&mainboard.pos, 0.000016);
+       //      position_set_centrifugal_coef(&mainboard.pos, 0.000016);
        position_use_ext(&mainboard.pos);
 
        /* TRAJECTORY MANAGER */
@@ -240,8 +240,8 @@ void microb_cs_init(void)
        /* ---- CS angle */
        /* PID */
        pid_init(&mainboard.angle.pid);
-       pid_set_gains(&mainboard.angle.pid, 500, 10, 7000);
-       pid_set_maximums(&mainboard.angle.pid, 0, 20000, 4095);
+       pid_set_gains(&mainboard.angle.pid, 850, 30, 15000);
+       pid_set_maximums(&mainboard.angle.pid, 0, 10000, 4095);
        pid_set_out_shift(&mainboard.angle.pid, 10);
        pid_set_derivate_filter(&mainboard.angle.pid, 4);
 
@@ -266,8 +266,8 @@ void microb_cs_init(void)
        /* ---- CS distance */
        /* PID */
        pid_init(&mainboard.distance.pid);
-       pid_set_gains(&mainboard.distance.pid, 500, 10, 7000);
-       pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095);
+       pid_set_gains(&mainboard.distance.pid, 850, 30, 15000);
+       pid_set_maximums(&mainboard.distance.pid, 0, 10000, 4095);
        pid_set_out_shift(&mainboard.distance.pid, 10);
        pid_set_derivate_filter(&mainboard.distance.pid, 6);
 

diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c
index 6997139..6ce17cf 100644 (file)
--- a/projects/microb2010/mainboard/i2c_protocol.c
+++ b/projects/microb2010/mainboard/i2c_protocol.c
@@ -274,6 +274,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
                /* status */
                cobboard.mode = ans->mode;
                cobboard.status = ans->status;
+               cobboard.cob_count = ans->cob_count;
                cobboard.left_cobroller_speed = ans->left_cobroller_speed;
                cs_set_consign(&mainboard.left_cobroller.cs, cobboard.left_cobroller_speed);
                cobboard.right_cobroller_speed = ans->right_cobroller_speed;

diff --git a/projects/microb2010/mainboard/pid_config.h b/projects/microb2010/mainboard/pid_config.h
index fa95f08..251cca8 100755 (executable)
--- a/projects/microb2010/mainboard/pid_config.h
+++ b/projects/microb2010/mainboard/pid_config.h
@@ -25,6 +25,6 @@
 /** the derivate term can be filtered to remove the noise. This value
  * is the maxium sample count to keep in memory to do this
  * filtering. For an instance of pid, this count is defined o*/
-#define PID_DERIVATE_FILTER_MAX_SIZE 4
+#define PID_DERIVATE_FILTER_MAX_SIZE 6
 
 #endif

diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c
index db161b8..2d0d7be 100644 (file)
--- a/projects/microb2010/mainboard/robotsim.c
+++ b/projects/microb2010/mainboard/robotsim.c
@@ -57,9 +57,23 @@ static int32_t l_pwm, r_pwm;
 static int32_t l_enc, r_enc;
 
 static int fdr, fdw;
+/*
+ * Debug with GDB:
+ *
+ * (gdb) handle SIGUSR1 pass
+ * Signal        Stop  Print   Pass to program Description
+ * SIGUSR1       Yes   Yes     Yes             User defined signal 1
+ * (gdb) handle SIGUSR2 pass
+ * Signal        Stop  Print   Pass to program Description
+ * SIGUSR2       Yes   Yes     Yes             User defined signal 2
+ * (gdb) handle SIGUSR1 noprint
+ * Signal        Stop  Print   Pass to program Description
+ * SIGUSR1       No    No      Yes             User defined signal 1
+ * (gdb) handle SIGUSR2 noprint
+ */
 
 /* */
-#define FILTER  97
+#define FILTER  98
 #define FILTER2 (100-FILTER)
 #define SHIFT   4
 

diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index e08c4af..52b151e 100644 (file)
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -100,6 +100,9 @@ void strat_conf_dump(const char *caller)
 /* call it just before launching the strat */
 void strat_init(void)
 {
+       position_set(&mainboard.pos, 298.16,
+                    COLOR_Y(308.78), COLOR_A(70.00));
+
        /* XXX init rollers, .. */
 
        strat_db_init();
@@ -110,9 +113,9 @@ void strat_init(void)
        time_reset();
        interrupt_traj_reset();
 
-       i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
-       i2c_cobboard_harvest(I2C_LEFT_SIDE);
-       i2c_cobboard_harvest(I2C_RIGHT_SIDE);
+       //i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+       //i2c_cobboard_harvest(I2C_LEFT_SIDE);
+       //i2c_cobboard_harvest(I2C_RIGHT_SIDE);
        i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
 
        /* used in strat_base for END_TIMER */
@@ -163,9 +166,29 @@ static uint8_t strat_beginning(void)
 {
        uint8_t err;
 
-       strat_set_speed(250, SPEED_ANGLE_FAST);
+       strat_set_acc(ACC_DIST, ACC_ANGLE);
+#ifdef HOST_VERSION
+       strat_set_speed(600, SPEED_ANGLE_FAST);
+#else
+       /* 250 */
+       strat_set_speed(600, SPEED_ANGLE_FAST);
+#endif
+
+
+       strat_set_speed(600, 60); /* OK */
+       // strat_set_speed(250, 28); /* OK */
 
+       trajectory_d_a_rel(&mainboard.traj, 500, COLOR_A(20));
+       err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
+                                   TRAJ_FLAGS_STD);
+
+       strat_set_acc(ACC_DIST, ACC_ANGLE);
+
+#if 0
  l1:
+       if (get_cob_count() >= 5)
+               strat_set_speed(600, SPEED_ANGLE_FAST);
+
        err = line2line(LINE_UP, 0, LINE_R_DOWN, 2);
        if (!TRAJ_SUCCESS(err)) {
                trajectory_hardstop(&mainboard.traj);
@@ -174,6 +197,9 @@ static uint8_t strat_beginning(void)
        }
 
  l2:
+       if (get_cob_count() >= 5)
+               strat_set_speed(600, SPEED_ANGLE_FAST);
+
        err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2);
        if (!TRAJ_SUCCESS(err)) {
                trajectory_hardstop(&mainboard.traj);
@@ -182,12 +208,30 @@ static uint8_t strat_beginning(void)
        }
 
  l3:
+       if (get_cob_count() >= 5)
+               strat_set_speed(600, SPEED_ANGLE_FAST);
+
        err = line2line(LINE_R_UP, 2, LINE_UP, 5);
        if (!TRAJ_SUCCESS(err)) {
                trajectory_hardstop(&mainboard.traj);
                time_wait_ms(2000);
                goto l3;
        }
+#else
+       strat_set_speed(600, SPEED_ANGLE_FAST);
+       err = line2line(LINE_UP, 0, LINE_R_DOWN, 3);
+       err = line2line(LINE_R_DOWN, 3, LINE_R_UP, 2);
+       err = line2line(LINE_R_UP, 2, LINE_R_DOWN, 2);
+       err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 3);
+       err = line2line(LINE_R_UP, 3, LINE_UP, 5);
+       err = line2line(LINE_UP, 5, LINE_L_DOWN, 2);
+       err = line2line(LINE_L_DOWN, 2, LINE_L_UP, 1);
+       err = line2line(LINE_L_UP, 1, LINE_L_DOWN, 1);
+       err = line2line(LINE_L_DOWN, 1, LINE_DOWN, 0);
+       wait_ms(500);
+       trajectory_hardstop(&mainboard.traj);
+       return END_TRAJ;
+#endif
 
        trajectory_hardstop(&mainboard.traj);
 

diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h
index 42b47dd..ba51434 100644 (file)
--- a/projects/microb2010/mainboard/strat.h
+++ b/projects/microb2010/mainboard/strat.h
@@ -98,8 +98,8 @@
 #define TRAJ_FLAGS_SMALL_DIST (END_TRAJ|END_BLOCKING|END_INTR)
 
 /* default acc */
-#define ACC_DIST  10.
-#define ACC_ANGLE 10.
+#define ACC_DIST  15.
+#define ACC_ANGLE 15.
 
 /* default speeds */
 #define SPEED_DIST_FAST 2500.

diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c
index e661ac1..87bcf9f 100644 (file)
--- a/projects/microb2010/mainboard/strat_avoid.c
+++ b/projects/microb2010/mainboard/strat_avoid.c
@@ -215,7 +215,7 @@ static struct djpoint *get_neigh(struct djpoint *pt,
        default:
                return NULL;
        }
-       if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
+       if (i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
                return NULL;
 
        if (is_reachable(i, j) == 0)

diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c
index 1f3b6e5..6075271 100644 (file)
--- a/projects/microb2010/mainboard/strat_base.c
+++ b/projects/microb2010/mainboard/strat_base.c
@@ -103,6 +103,8 @@ const char *get_err(uint8_t err)
 
 void strat_hardstop(void)
 {
+       DEBUG(E_USER_STRAT, "strat_hardstop");
+
        trajectory_hardstop(&mainboard.traj);
        pid_reset(&mainboard.angle.pid);
        pid_reset(&mainboard.distance.pid);
@@ -209,6 +211,11 @@ void strat_set_speed(uint16_t d, uint16_t a)
        IRQ_UNLOCK(flags);
 }
 
+void strat_set_acc(double d, double a)
+{
+       trajectory_set_acc(&mainboard.traj, d, a);
+}
+
 void strat_get_speed(uint16_t *d, uint16_t *a)
 {
        uint8_t flags;
@@ -312,7 +319,6 @@ void strat_start(void)
 
        strat_init();
        err = strat_main();
-       printf("coucou\n");
        NOTICE(E_USER_STRAT, "Finished !! returned %s", get_err(err));
        strat_exit();
 }
@@ -344,9 +350,12 @@ uint8_t strat_obstacle(void)
        double opp_x, opp_y;
 
 #ifdef HOST_VERSION
+       return 0;
        if (time_get_s() >= 12 && time_get_s() <= 30)
                return 1;
 #endif
+       return 0; /* XXX disabled */
+
        if (!sensor_get(S_RCOB_WHITE))
                return 0;
 

diff --git a/projects/microb2010/mainboard/strat_base.h b/projects/microb2010/mainboard/strat_base.h
index d8b61e5..908a2f6 100644 (file)
--- a/projects/microb2010/mainboard/strat_base.h
+++ b/projects/microb2010/mainboard/strat_base.h
@@ -66,6 +66,7 @@ uint8_t strat_obstacle(void);
 /* set/get user strat speed */
 void strat_set_speed(uint16_t d, uint16_t a);
 void strat_get_speed(uint16_t *d, uint16_t *a);
+void strat_set_acc(double d, double a);
 
 /* when user type ctrl-c we can interrupt traj */
 void interrupt_traj(void);

diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index 3694bcd..31afd43 100644 (file)
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -161,8 +161,17 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
        line2_a_rad = atan2(l2.p2.y - l2.p1.y,
                            l2.p2.x - l2.p1.x);
        diff_a_deg = DEG(line2_a_rad - line1_a_rad);
+       if (diff_a_deg < -180) {
+               diff_a_deg += 360;
+       }
+       else if (diff_a_deg > 180) {
+               diff_a_deg -= 360;
+       }
        diff_a_deg_abs = fabs(diff_a_deg);
 
+/*     printf_P(PSTR("diff_a_deg=%2.2f\r\n"), diff_a_deg_abs); */
+/*     printf_P(PSTR("inter=%2.2f,%2.2f\r\n"), p.x, p.y); */
+
        if (diff_a_deg_abs < 70.) {
                radius = 200;
                if (diff_a_deg > 0)
@@ -185,10 +194,12 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
                        beta_deg = -60;
        }
 
+       /* XXX check return value !! */
        trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
                           line1_a_deg, 150., diff_a_deg, beta_deg,
                           radius, xy_norm(l1.p1.x, l1.p1.y,
                                           p.x, p.y));
+       /* disabled */
        if (0) {
                err = 0;
                while (err == 0) {