X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_corn.c;h=3ec6a2c5aef56a444c71219334e57754267f6b03;hp=31afd436463d3639cd7dc2804c0d0946eef2ad54;hb=5894a1a70f98504fee52e58f45f181aa684d99bf;hpb=e442b9c066de9b55eef70fdf9993fc3f6b8259e8 diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c index 31afd43..3ec6a2c 100644 --- a/projects/microb2010/mainboard/strat_corn.c +++ b/projects/microb2010/mainboard/strat_corn.c @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -65,10 +66,10 @@ #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); @@ -90,48 +91,23 @@ uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) 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; @@ -140,15 +116,16 @@ uint8_t line2line(uint8_t dir1, uint8_t num1, line_t ll1, ll2; point_t p; uint8_t err; + 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); @@ -172,13 +149,15 @@ uint8_t line2line(uint8_t dir1, uint8_t 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) @@ -186,34 +165,50 @@ uint8_t line2line(uint8_t dir1, uint8_t num1, 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); + + 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); */ - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + strat_rpack60 = 0; + strat_lpack60 = 0; return err; }