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 df8ee0e..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;
 
@@ -196,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)
 {
@@ -220,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;
@@ -241,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);
                }
        }