static volatile uint8_t clitoid_slow = 0;
/* return 1 if there is a corn near, and fill the index ptr */
-int8_t corn_is_near(uint8_t *corn_idx, uint8_t side)
+int8_t corn_is_near(uint8_t *corn_idx, uint8_t side,
+ int16_t *xspickle, int16_t *yspickle)
{
/* XXX to be checked */
#define SENSOR_CORN_DIST 225
}
x_corn_int = x_corn;
y_corn_int = y_corn;
+ *xspickle = x_corn_int;
+ *yspickle = y_corn_int;
wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
if (wp == NULL)
if (get_cob_count() >= 5)
return 0; /* fast */
- if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
+ if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) {
DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
__FUNCTION__, x, y);
return 1;
}
-/* if (time_get_s() > 32) */
-/* DEBUG(E_USER_STRAT, "i,j = (%d %d), count=%d", i, j, */
-/* corn_count_neigh(i, j)); */
-
if (corn_count_neigh(i, j) == 2)
return 1;
- /* we are on intersection, let's go slow... but as we enter in
- * the curve-part of the clitoid, we should not go there */
+
+ /* we are on intersection, keep the same speed... but as we
+ * enter in the curve-part of the clitoid, we should not go
+ * there */
if (wp_belongs_to_line(i, j, num2, dir2))
- return 0;
+ return clitoid_slow;
/* we can ge fast if it's a 60deg angle and if we checked the
* current point */
}
else {
beta_deg = 0;
- *pack_spickles = I2C_RIGHT_SIDE;
+ *pack_spickles = I2C_LEFT_SIDE;
}
}
/* double 90 deg for half turn -- not used */
strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
}
- /* XXX check return value !! */
ret = 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,
reprocess:
ret = strat_calc_clitoid(num1, dir1, num2, dir2, &pack_spickles);
- if (ret < 0)
+ if (ret < 0) {
DEBUG(E_USER_STRAT, "clitoid failed");
+ return END_ERROR;
+ }
/* XXX what to do if cobboard is stucked */
num2, dir2),
flags);
+ /* error during traj, or traj finished */
+ if (err != 0)
+ return err;
+
/* the speed has to change */
- if (trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE)
+ if (err == 0 &&
+ trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE)
goto reprocess;
DEBUG(E_USER_STRAT, "clitoid started err=%d pack_spickles=%d",
err = wait_traj_end(flags);
}
- DEBUG(E_USER_STRAT, "clitoid finished");
-
- /* XXX 600 -> cste */
- /* XXX does not work, do better */
-/* if (err == 0 && d_speed < 600 && */
-/* mainboard.traj.state == RUNNING_CLITOID_LINE) */
-/* strat_set_speed(600, SPEED_ANGLE_FAST); */
+ DEBUG(E_USER_STRAT, "clitoid finished, err=%d", err);
strat_rpack60 = 0;
strat_lpack60 = 0;