+/* 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;
+}
+