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 1b103d8..6971155 100644 (file)
--- a/projects/microb2010/mainboard/robotsim.c
+++ b/projects/microb2010/mainboard/robotsim.c
@@ -54,6 +54,8 @@
 #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;
 
@@ -105,12 +107,22 @@ 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();
        return 0;
 }
 
+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)
 {
@@ -159,17 +171,17 @@ robotsim_i2c_ballboard(uint8_t addr, uint8_t *buf, uint8_t size)
 static int8_t
 robotsim_i2c_cobboard(uint8_t addr, uint8_t *buf, uint8_t size)
 {
-       //      void *void_cmd = buf;
+       void *void_cmd = buf;
 
        switch (buf[0]) {
-#if 0 /* deleted */
+
        case I2C_CMD_COBBOARD_SET_MODE:
                {
                        struct i2c_cmd_cobboard_set_mode *cmd = void_cmd;
                        robotsim_i2c_cobboard_set_mode(cmd);
                        break;
                }
-#endif
+
        default:
                break;
        }
@@ -186,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)
 {
@@ -210,6 +243,8 @@ void robotsim_update(void)
        int oppx, oppy;
        double oppa, oppd;
 
+       beacon_update();
+
        /* time shift the command */
        l_pwm_shift[i] = l_pwm;
        r_pwm_shift[i] = r_pwm;
@@ -231,16 +266,18 @@ void robotsim_update(void)
                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);
-                       beaconboard.oppx = oppx;
-                       beaconboard.oppy = oppy;
-                       beaconboard.oppa = DEG(oppa);
-                       if (beaconboard.oppa < 0)
-                               beaconboard.oppa += 360;
-                       beaconboard.oppd = oppd;
+                       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);
                }
        }