vt100: include pgmspace.h as we use PROGMEM macro

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

diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c
index 5fc2e37..6971155 100644 (file)
--- a/projects/microb2010/mainboard/robotsim.c
+++ b/projects/microb2010/mainboard/robotsim.c
@@ -44,6 +44,7 @@
 #include <blocking_detection_manager.h>
 #include <robot_system.h>
 #include <position_manager.h>
+#include <trajectory_manager_utils.h>
 
 #include <parse.h>
 #include <rdline.h>
@@ -53,13 +54,29 @@
 #include "strat_utils.h"
 #include "main.h"
 
+uint8_t robotsim_blocking = 0;
+
 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
 
@@ -67,11 +84,16 @@ void robotsim_dump(void)
 {
        char buf[BUFSIZ];
        int len;
+       int16_t x, y, a;
+
+       x = position_get_x_s16(&mainboard.pos);
+       y = position_get_y_s16(&mainboard.pos);
+       a = position_get_a_deg_s16(&mainboard.pos);
+/*     y = COLOR_Y(y); */
+/*     a = COLOR_A(a); */
 
        len = snprintf(buf, sizeof(buf), "pos=%d,%d,%d\n",
-                     position_get_x_s16(&mainboard.pos),
-                     position_get_y_s16(&mainboard.pos),
-                     position_get_a_deg_s16(&mainboard.pos));
+                      x, y, a);
        hostsim_lock();
        write(fdw, buf, len);
        hostsim_unlock();
@@ -85,6 +107,8 @@ robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd)
 
        ballboard.mode = cmd->mode;
        len = snprintf(buf, sizeof(buf), "ballboard=%d\n", cmd->mode);
+       if (cmd->mode == I2C_BALLBOARD_MODE_EJECT)
+               ballboard.ball_count = 0;
        hostsim_lock();
        write(fdw, buf, len);
        hostsim_unlock();
@@ -93,12 +117,32 @@ robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd)
 
 static int8_t
 robotsim_i2c_cobboard_set_mode(struct i2c_cmd_cobboard_set_mode *cmd)
+{
+       if (cmd->mode == I2C_COBBOARD_MODE_EJECT)
+               cobboard.cob_count = 0;
+       return 0;
+}
+
+int8_t
+robotsim_i2c_cobboard_set_spickles(uint8_t side, uint8_t flags)
 {
        char buf[BUFSIZ];
        int len;
 
-       cobboard.mode = cmd->mode;
-       len = snprintf(buf, sizeof(buf), "cobboard=%d\n", cmd->mode);
+       if (side == I2C_LEFT_SIDE) {
+               if (cobboard.lspickle == flags)
+                       return 0;
+               else
+                       cobboard.lspickle = flags;
+       }
+       if (side == I2C_RIGHT_SIDE) {
+               if (cobboard.rspickle == flags)
+                       return 0;
+               else
+                       cobboard.rspickle = flags;
+       }
+
+       len = snprintf(buf, sizeof(buf), "cobboard=%d,%d\n", side, flags);
        hostsim_lock();
        write(fdw, buf, len);
        hostsim_unlock();
@@ -130,12 +174,14 @@ robotsim_i2c_cobboard(uint8_t addr, uint8_t *buf, uint8_t size)
        void *void_cmd = buf;
 
        switch (buf[0]) {
+
        case I2C_CMD_COBBOARD_SET_MODE:
                {
                        struct i2c_cmd_cobboard_set_mode *cmd = void_cmd;
                        robotsim_i2c_cobboard_set_mode(cmd);
                        break;
                }
+
        default:
                break;
        }
@@ -152,6 +198,27 @@ robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size)
        return 0;
 }
 
+static void beacon_update(void)
+{
+       uint8_t flags;
+       int16_t oppx, oppy;
+       double oppa, oppd;
+
+       IRQ_LOCK(flags);
+       if (ballboard.opponent_x == I2C_OPPONENT_NOT_THERE) {
+               IRQ_UNLOCK(flags);
+               return;
+       }
+       oppx = ballboard.opponent_x;
+       oppy = ballboard.opponent_y;
+       abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
+       ballboard.opponent_a = DEG(oppa);
+       if (ballboard.opponent_a < 0)
+               ballboard.opponent_a += 360;
+       ballboard.opponent_d = oppd;
+       IRQ_UNLOCK(flags);
+}
+
 /* must be called periodically */
 void robotsim_update(void)
 {
@@ -160,9 +227,12 @@ void robotsim_update(void)
        static int32_t l_speed, r_speed;
        static unsigned i = 0;
        static unsigned cpt = 0;
+
+       uint8_t flags;
        int32_t local_l_pwm, local_r_pwm;
        double x, y, a, a2, d;
-       char cmd = 0;
+       char cmd[BUFSIZ];
+       int n, pertl = 0, pertr = 0;
 
        /* corners of the robot */
        double xfl, yfl; /* front left */
@@ -170,6 +240,11 @@ void robotsim_update(void)
        double xrr, yrr; /* rear right */
        double xfr, yfr; /* front right */
 
+       int oppx, oppy;
+       double oppa, oppd;
+
+       beacon_update();
+
        /* time shift the command */
        l_pwm_shift[i] = l_pwm;
        r_pwm_shift[i] = r_pwm;
@@ -180,8 +255,31 @@ void robotsim_update(void)
 
        /* read command */
        if (((cpt ++) & 0x7) == 0) {
-               if (read(fdr, &cmd, 1) != 1)
-                       cmd = 0;
+               n = read(fdr, &cmd, BUFSIZ - 1);
+               if (n < 1)
+                       n = 0;
+               cmd[n] = 0;
+       }
+
+       /* perturbation */
+       if (cmd[0] == 'l')
+               pertl = 1;
+       else if (cmd[0] == 'r')
+               pertr = 1;
+       else if (cmd[0] == 'b')
+               robotsim_blocking = 1;
+       if (cmd[0] == 'o') {
+               if (sscanf(cmd, "opp %d %d", &oppx, &oppy) == 2) {
+                       abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
+                       IRQ_LOCK(flags);
+                       ballboard.opponent_x = oppx;
+                       ballboard.opponent_y = oppy;
+                       ballboard.opponent_a = DEG(oppa);
+                       if (ballboard.opponent_a < 0)
+                               ballboard.opponent_a += 360;
+                       ballboard.opponent_d = oppd;
+                       IRQ_UNLOCK(flags);
+               }
        }
 
        x = position_get_x_double(&mainboard.pos);
@@ -217,10 +315,9 @@ void robotsim_update(void)
        if (!is_in_area(xfr, yfr, 0) && r_speed > 0)
                r_speed = 0;
 
-       /* perturbation */
-       if (cmd == 'l')
+       if (pertl)
                l_enc += 5000; /* push 1 cm */
-       if (cmd == 'r')
+       if (pertr)
                r_enc += 5000; /* push 1 cm */
 
        /* XXX should lock */