#include "sensor.h"
#include "actuator.h"
-#if 0
/* return 1 if there is a corn near, and fill the index ptr */
-uint8_t corn_is_near(int8_t *corn_idx, uint8_t side)
+int8_t corn_is_near(uint8_t *corn_idx, uint8_t side)
{
+ /* XXX to be checked */
#define SENSOR_CORN_DIST 225
#define SENSOR_CORN_ANGLE 90
double x = position_get_x_double(&mainboard.pos);
y_corn_int = y_corn;
wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
- if (wp->corn.idx < 0)
+ if (wp == NULL)
return 0;
+ *corn_idx = wp->corn.idx;
return 1;
}
-#endif
/*
- * - send the correct commands to the spickles
- * - return 1 if we need to stop (cobboard is stucked)
-*/
-static uint8_t handle_spickles(void)
+ * return true if clitoid started
+ */
+static uint8_t clitoid_started(void)
{
- return 0;
-#if 0
- 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;
-#endif
+ return trajectory_get_state(&mainboard.traj) == RUNNING_CLITOID_CURVE;
}
-uint8_t line2line(uint8_t dir1, uint8_t num1,
- uint8_t dir2, uint8_t num2)
+/* XXX passer les flags de traj */
+uint8_t line2line(uint8_t num1, uint8_t dir1,
+ uint8_t num2, uint8_t dir2)
{
double line1_a_rad, line1_a_deg, line2_a_rad;
double diff_a_deg, diff_a_deg_abs, beta_deg;
line_t ll1, ll2;
point_t p;
uint8_t err;
- uint16_t a_speed, d_speed;
+ int8_t ret;
/* convert to 2 points */
num2line(&l1, dir1, num1);
/* printf_P(PSTR("diff_a_deg=%2.2f\r\n"), diff_a_deg_abs); */
/* printf_P(PSTR("inter=%2.2f,%2.2f\r\n"), p.x, p.y); */
+ /* small angle, 60 deg */
if (diff_a_deg_abs < 70.) {
- radius = 200;
+ radius = 150;
if (diff_a_deg > 0)
- beta_deg = 40;
+ beta_deg = 0;
else
- beta_deg = -40;
+ beta_deg = 0;
}
+ /* double 90 deg for half turn -- not used */
else if (diff_a_deg_abs < 100.) {
radius = 100;
if (diff_a_deg > 0)
else
beta_deg = -40;
}
+ /* hard turn, 120 deg */
else {
- radius = 120;
+ radius = 75;
if (diff_a_deg > 0)
- beta_deg = 60;
+ beta_deg = 0;
else
- beta_deg = -60;
+ beta_deg = 0;
}
/* XXX check return value !! */
- 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));
- /* disabled */
- if (0) {
- 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 err; /* XXX do something */
- }
- err = test_traj_end(0xFF);
- }
+ 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,
+ p.x, p.y));
+ if (ret < 0)
+ DEBUG(E_USER_STRAT, "clitoid failed");
+
+ /* XXX what to do if cobboard is stucked */
+
+ err = WAIT_COND_OR_TRAJ_END(clitoid_started(), 0xFF);
+ DEBUG(E_USER_STRAT, "clitoid started err=%d diff_a_deg_abs=%2.2f diff_a_deg=%2.2f",
+ err, diff_a_deg_abs, diff_a_deg);
+
+ /* when clitoid starts and angle is 60 deg, pack external
+ * spickle */
+ if (diff_a_deg_abs < 70. && err == 0) {
+ if (diff_a_deg > 0)
+ strat_rpack60 = 1;
+ else
+ strat_lpack60 = 1;
}
+ if (err == 0)
+ err = wait_traj_end(0xFF);
- err = WAIT_COND_OR_TRAJ_END(get_cob_count() == 5, 0xFF);
- strat_get_speed(&d_speed, &a_speed);
+ DEBUG(E_USER_STRAT, "clitoid finished");
/* XXX 600 -> cste */
- if (err == 0 && d_speed < 600 &&
- mainboard.traj.state == RUNNING_CLITOID_LINE)
- strat_set_speed(600, SPEED_ANGLE_FAST);
- err = wait_traj_end(0xFF);
+ /* 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); */
+ strat_rpack60 = 0;
+ strat_lpack60 = 0;
return err;
}
git.droids-corp.org / aversive.git / blobdiff