#include <math.h>
#include <aversive.h>
+#include <aversive/error.h>
#include <aversive/pgmspace.h>
#include <ax12.h>
#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
#endif
}
-uint8_t line2line(uint8_t dir1, uint8_t num1,
- uint8_t dir2, uint8_t num2)
+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);
num2line(&l2, dir2, num2);
- printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
- l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
- printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
- l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
+ DEBUG(E_USER_STRAT, "line1: (%2.2f, %2.2f) -> (%2.2f, %2.2f)",
+ l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
+ DEBUG(E_USER_STRAT, "line2: (%2.2f, %2.2f) -> (%2.2f, %2.2f)",
+ l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
/* convert to line eq and find intersection */
pts2line(&l1.p1, &l1.p2, &ll1);
line2_a_rad = atan2(l2.p2.y - l2.p1.y,
l2.p2.x - l2.p1.x);
diff_a_deg = DEG(line2_a_rad - line1_a_rad);
+ if (diff_a_deg < -180) {
+ diff_a_deg += 360;
+ }
+ else if (diff_a_deg > 180) {
+ diff_a_deg -= 360;
+ }
diff_a_deg_abs = fabs(diff_a_deg);
+/* 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); */
+
if (diff_a_deg_abs < 70.) {
radius = 200;
if (diff_a_deg > 0)
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));
+ /* 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,
+ p.x, p.y));
+ if (ret < 0)
+ DEBUG(E_USER_STRAT, "clitoid failed");
+
+ /* disabled */
if (0) {
err = 0;
while (err == 0) {
}
}
- err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ err = WAIT_COND_OR_TRAJ_END(get_cob_count() == 5, 0xFF);
+ strat_get_speed(&d_speed, &a_speed);
+
+ /* 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); */
+
+ err = wait_traj_end(0xFF);
+
return err;
}