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

diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c
index 42a33d9..6971155 100644
--- 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,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;
 
@@ -104,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)
 {
@@ -158,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;
 	}
@@ -185,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)
 {
@@ -193,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 */
@@ -203,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;
@@ -213,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);
@@ -250,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 */