X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_mainboard.c;h=7c42411a5a613bd97bb6c740b7a811f0e29caf42;hp=8a79c35f3f95b589e4d39ff998e8d3090146523f;hb=b022f257a5ee568737e1a684a83d0154397fffcb;hpb=aed049ab6be4e1916457743d53f6b610a21b4854

diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
index 8a79c35..7c42411 100644
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -66,6 +66,7 @@
 #include "strat.h"
 #include "strat_utils.h"
 #include "strat_base.h"
+#include "strat_corn.h"
 #include "i2c_protocol.h"
 #include "actuator.h"
 
@@ -1202,6 +1203,65 @@ static void reverse_line(struct line_2pts *l)
 }
 #endif
 
+/* return 1 if there is a corn near, and fill the index ptr */
+static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side)
+{
+#define SENSOR_CORN_DIST  225
+#define SENSOR_CORN_ANGLE 90
+	double x = position_get_x_double(&mainboard.pos);
+	double y = position_get_y_double(&mainboard.pos);
+	double a_rad = position_get_a_rad_double(&mainboard.pos);
+	double x_corn, y_corn;
+	int16_t x_corn_int, y_corn_int;
+
+	if (side == I2C_LEFT_SIDE) {
+		x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
+		y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
+	}
+	else {
+		x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
+		y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
+	}
+	x_corn_int = x_corn;
+	y_corn_int = y_corn;
+
+	*corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
+	if (*corn_idx < 0)
+		return 0;
+	return 1;
+}
+
+/*
+ * - send the correct commands to the spickles
+ * - return 1 if we need to stop (cobboard is stucked)
+*/
+static uint8_t handle_spickles(void)
+{
+	int8_t corn_idx;
+
+	if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE))
+		i2c_cobboard_mode_deploy(I2C_LEFT_SIDE);
+	else {
+		if (corn_table[corn_idx] == TYPE_WHITE_CORN)
+			i2c_cobboard_mode_harvest(I2C_LEFT_SIDE);
+		else
+			i2c_cobboard_mode_pack(I2C_LEFT_SIDE);
+	}
+/* 	printf("%d %d\n", corn_idx, corn_table[corn_idx]); */
+/* 	time_wait_ms(100); */
+
+	if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE))
+		i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE);
+	else {
+		if (corn_table[corn_idx] == TYPE_WHITE_CORN)
+			i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE);
+		else
+			i2c_cobboard_mode_pack(I2C_RIGHT_SIDE);
+	}
+
+	return 0;
+}
+
 static void line2line(uint8_t dir1, uint8_t num1,
 		      uint8_t dir2, uint8_t num2)
 {
@@ -1211,6 +1271,7 @@ static void line2line(uint8_t dir1, uint8_t num1,
 	struct line_2pts l1, l2;
 	line_t ll1, ll2;
 	point_t p;
+	uint8_t err;
 
 	/* convert to 2 points */
 	num2line(&l1, dir1, num1);
@@ -1255,11 +1316,22 @@ static void line2line(uint8_t dir1, uint8_t num1,
 		else
 			beta_deg = -60;
 	}
+
 	trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
 			   line1_a_deg, 150., diff_a_deg, beta_deg,
 			   radius, xy_norm(l1.p1.x, l1.p1.y,
 					   p.x, p.y));
-	wait_traj_end(0xFF);
+	err = 0;
+	while (err == 0) {
+		err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF);
+		if (err == 0) {
+			/* cobboard is stucked */
+			trajectory_hardstop(&mainboard.traj);
+			return; /* XXX do something */
+		}
+		err = test_traj_end(0xFF);
+	}
+	return;
 }
 
 /* function called when cmd_test is parsed successfully */
@@ -1269,11 +1341,14 @@ static void cmd_test_parsed(void *parsed_result, void *data)
 	strat_reset_pos(298.48, 309.21, 70.02);
 	mainboard.angle.on = 1;
 	mainboard.distance.on = 1;
+	strat_set_speed(250, SPEED_ANGLE_FAST);
 #endif
+	init_corn_table(0, 0);
 	time_wait_ms(100);
 
 	line2line(LINE_UP, 0, LINE_R_DOWN, 2);
-	line2line(LINE_R_DOWN, 2, LINE_R_UP, 1);
+	line2line(LINE_R_DOWN, 2, LINE_R_UP, 2);
+	line2line(LINE_R_UP, 2, LINE_UP, 5);
 
 	trajectory_hardstop(&mainboard.traj);
 }