X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Frobotsim.c;h=69711558b03c4e3f87fffc7bf8825c78b1bd6b21;hp=df8ee0ebcc2c9dac32903b6aea6c2fd47953b214;hb=HEAD;hpb=b699727efceb661be65fbd22e975565b800c628e

diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c
index df8ee0e..6971155 100644
--- 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);
 		}
 	}