X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Frobotsim.c;h=db161b89cb52d205dfb17ab14f142b5b8edf87dc;hp=15b107563e38e59085e6b8dbc40ab6f00d11e8b5;hb=78150017ab8c5615af414df706a0525fe7c262ae;hpb=ebfaaedd491e61696cc93b353471be15408d23e4

diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c
index 15b1075..db161b8 100644
--- a/projects/microb2010/mainboard/robotsim.c
+++ b/projects/microb2010/mainboard/robotsim.c
@@ -48,6 +48,7 @@
 #include <parse.h>
 #include <rdline.h>
 
+#include "../common/i2c_commands.h"
 #include "strat.h"
 #include "strat_utils.h"
 #include "main.h"
@@ -66,14 +67,99 @@ void robotsim_dump(void)
 {
 	char buf[BUFSIZ];
 	int len;
+	int16_t x, y, a;
 
-	len = snprintf(buf, sizeof(buf), "%d %d %d\n",
-		      position_get_x_s16(&mainboard.pos),
-		      position_get_y_s16(&mainboard.pos),
-		      position_get_a_deg_s16(&mainboard.pos));
+	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",
+		       x, y, a);
+	hostsim_lock();
+	write(fdw, buf, len);
+	hostsim_unlock();
+}
+
+static int8_t
+robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd)
+{
+	char buf[BUFSIZ];
+	int len;
+
+	ballboard.mode = cmd->mode;
+	len = snprintf(buf, sizeof(buf), "ballboard=%d\n", cmd->mode);
 	hostsim_lock();
 	write(fdw, buf, len);
 	hostsim_unlock();
+	return 0;
+}
+
+int8_t
+robotsim_i2c_cobboard_set_mode(uint8_t mode)
+{
+	char buf[BUFSIZ];
+	int len;
+
+	if (cobboard.mode == mode)
+		return 0;
+
+	cobboard.mode = mode;
+	len = snprintf(buf, sizeof(buf), "cobboard=%d\n", mode);
+	hostsim_lock();
+	write(fdw, buf, len);
+	hostsim_unlock();
+	return 0;
+}
+
+static int8_t
+robotsim_i2c_ballboard(uint8_t addr, uint8_t *buf, uint8_t size)
+{
+	void *void_cmd = buf;
+
+	switch (buf[0]) {
+	case I2C_CMD_BALLBOARD_SET_MODE:
+		{
+			struct i2c_cmd_ballboard_set_mode *cmd = void_cmd;
+			robotsim_i2c_ballboard_set_mode(cmd);
+			break;
+		}
+
+	default:
+		break;
+	}
+	return 0;
+}
+
+static int8_t
+robotsim_i2c_cobboard(uint8_t addr, uint8_t *buf, uint8_t size)
+{
+	//	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;
+	}
+	return 0;
+}
+
+int8_t
+robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size)
+{
+	if (addr == I2C_BALLBOARD_ADDR)
+		return robotsim_i2c_ballboard(addr, buf, size);
+	else if (addr == I2C_COBBOARD_ADDR)
+		return robotsim_i2c_cobboard(addr, buf, size);
+	return 0;
 }
 
 /* must be called periodically */
@@ -118,8 +204,8 @@ void robotsim_update(void)
 		((local_r_pwm * 1000 * FILTER2)/1000);
 
 	/* basic collision detection */
-	a2 = atan2(ROBOT_WIDTH/2, ROBOT_LENGTH/2);
-	d = norm(ROBOT_WIDTH/2, ROBOT_LENGTH/2);
+	a2 = atan2(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_REAR);
+	d = norm(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_REAR);
 
 	xfl = x + cos(a+a2) * d;
 	yfl = y + sin(a+a2) * d;
@@ -156,9 +242,9 @@ void robotsim_pwm(void *arg, int32_t val)
 {
 	//	printf("%p, %d\n", arg, val);
 	if (arg == LEFT_PWM)
-		l_pwm = val;
+		l_pwm = (val / 1.55);
 	else if (arg == RIGHT_PWM)
-		r_pwm = val;
+		r_pwm = (val / 1.55);
 }
 
 int32_t robotsim_encoder_get(void *arg)