fw.write("l")
elif k == "r":
fw.write("r")
+ elif k == "b":
+ fw.write("b")
elif k == "c":
robot_trail_list = []
elif k == "x":
#include "strat_utils.h"
#include "main.h"
+uint8_t robotsim_blocking = 0;
+
static int32_t l_pwm, r_pwm;
static int32_t l_enc, r_enc;
pertl = 1;
else if (cmd[0] == 'r')
pertr = 1;
+ else if (cmd[0] == 'b')
+ robotsim_blocking = 1;
if (cmd[0] == 'o') {
if (sscanf(cmd, "opp %d %d", &oppx, &oppy) == 2) {
abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
*
*/
+extern uint8_t robotsim_blocking;
+
int8_t robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size);
void robotsim_update(void);
void robotsim_pwm(void *arg, int32_t val);
static uint8_t prev_check_time;
uint8_t cur_time;
uint8_t need_lpack, need_rpack;
+ int16_t l_xspickle, l_yspickle;
+ int16_t r_xspickle, r_yspickle;
+ uint8_t l_y_too_high_pack = 0, r_y_too_high_pack = 0;
/* read sensors from ballboard */
IRQ_LOCK(flags);
}
/* detect cob on left side */
- lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE);
+ lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE,
+ &l_xspickle, &l_yspickle);
if (lcob_near && lcob != I2C_COB_NONE) {
if (strat_db.corn_table[lidx]->corn.color == I2C_COB_UNKNOWN)
DEBUG(E_USER_STRAT, "lcob %s %d",
lcob == I2C_COB_WHITE ? "white" : "black", lidx);
corn_set_color(strat_db.corn_table[lidx], lcob);
}
+ if (!__y_is_more_than(l_yspickle, 600))
+ l_y_too_high_pack = 1;
/* detect cob on right side */
- rcob_near = corn_is_near(&ridx, I2C_RIGHT_SIDE);
+ rcob_near = corn_is_near(&ridx, I2C_RIGHT_SIDE,
+ &r_xspickle, &r_yspickle);
if (rcob_near && rcob != I2C_COB_NONE) {
if (strat_db.corn_table[ridx]->corn.color == I2C_COB_UNKNOWN)
DEBUG(E_USER_STRAT, "rcob %s %d",
rcob == I2C_COB_WHITE ? "white" : "black", ridx);
corn_set_color(strat_db.corn_table[ridx], rcob);
}
+ if (!__y_is_more_than(r_yspickle, 600))
+ r_y_too_high_pack = 1;
/* control the cobboard mode for left spickle */
need_lpack = get_cob_count() >= 5 || strat_want_pack ||
- strat_lpack60 || strat_opponent_lpack;
+ strat_lpack60 || strat_opponent_lpack || l_y_too_high_pack;
+
if (lcob_near && strat_db.corn_table[lidx]->present) {
if (need_lpack) {
/* nothing */
/* control the cobboard mode for right spickle */
need_rpack = get_cob_count() >= 5 || strat_want_pack ||
- strat_rpack60 || strat_opponent_rpack;
+ strat_rpack60 || strat_opponent_rpack || r_y_too_high_pack;
if (rcob_near && strat_db.corn_table[ridx]->present) {
if (need_rpack) {
/* nothing */
case LINE_R_UP:
return COLOR_A(30);
case LINE_R_DOWN:
- return COLOR_A(-90);
+ return COLOR_A(-30);
case LINE_L_UP:
return COLOR_A(150);
case LINE_L_DOWN:
}
}
+int16_t get_nearest_dir_angle(int16_t a)
+{
+ uint8_t dir, min_dir = 0;
+ int16_t min_diff = 0x7FFF, diff;
+
+ for (dir = LINE_UP; dir <= LINE_R_UP; dir++) {
+ diff = abs(linedir2angle(dir) - a);
+ if (diff > 360)
+ diff -= 360;
+ if (diff > 360)
+ diff -= 360;
+ if (diff < min_diff) {
+ min_diff = diff;
+ min_dir = dir;
+ }
+ }
+ return linedir2angle(min_dir);
+}
+
/* return true if a waypoint belongs to a line */
uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir)
{
/* try to unblock in any situation */
uint8_t strat_unblock(void)
{
- int16_t x, y, posx, posy;
- uint8_t i, j, k;
+ int16_t x, y, posx, posy, posa;
+ uint8_t i, j, k, cpt;
uint16_t old_dspeed, old_aspeed;
uint8_t err;
uint16_t d_min = 0x7FFF, d;
strat_get_speed(&old_dspeed, &old_aspeed);
strat_hardstop();
+ posa = position_get_a_deg_s16(&mainboard.pos);
+
strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
posx = position_get_x_s16(&mainboard.pos);
posy = position_get_y_s16(&mainboard.pos);
DEBUG(E_USER_STRAT, "%s() unblock point is %d,%d",
__FUNCTION__, x, y);
- /* XXX if opponent is too close, go back, or wait ? */
+ for (cpt = 0; cpt < 2; cpt++) {
+
+ /* go to nearest waypoint */
+ trajectory_goto_xy_abs(&mainboard.traj, x, y);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (err == END_TIMER)
+ return err;
+
+ if (TRAJ_SUCCESS(err))
+ break;
+
+ if (cpt == 1)
+ break;
+
+ /* aie... do a S */
+ trajectory_d_a_rel(&mainboard.traj, 100, 20);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ trajectory_d_a_rel(&mainboard.traj, 100, -20);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ trajectory_d_a_rel(&mainboard.traj, -100, -20);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ trajectory_d_a_rel(&mainboard.traj, -100, 20);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ }
- /* go to nearest waypoint */
- trajectory_goto_xy_abs(&mainboard.traj, x, y);
+ trajectory_a_abs(&mainboard.traj, get_nearest_dir_angle(posa));
err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
if (err == END_TIMER)
return err;
#include "strat.h"
#include "sensor.h"
#include "i2c_protocol.h"
+#include "robotsim.h"
/* true if we want to interrupt a trajectory */
static uint8_t traj_intr=0;
+/* to debug state of strat_obstacle() */
+static uint8_t strat_obstacle_debug = 0;
+
/* filled when a END_OBSTACLE is returned */
struct opponent_obstacle opponent_obstacle;
strat_exit();
}
-uint8_t xxxdebug = 0;
/* return true if we have to brake due to an obstacle */
uint8_t strat_obstacle(void)
int16_t opp_x, opp_y, opp_d, opp_a;
/* too slow */
- if (ABS(mainboard.speed_d) < 0) {
- if (xxxdebug != 1) {
- DEBUG(E_USER_STRAT, "XXX too slow");
- xxxdebug = 1;
+ if (ABS(mainboard.speed_d) < 100) {
+ if (strat_obstacle_debug != 1) {
+ DEBUG(E_USER_STRAT, "disable opp, too slow");
+ strat_obstacle_debug = 1;
}
return 0;
}
/* no opponent detected */
- if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a)) {
- if (xxxdebug != 2) {
- DEBUG(E_USER_STRAT, "XXX no opp");
- DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
- "x=%d y=%d (speed_d=%d)",
- opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
- xxxdebug = 2;
+ if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a) < 0) {
+ if (strat_obstacle_debug != 2) {
+ DEBUG(E_USER_STRAT, "no opponent found");
+ strat_obstacle_debug = 2;
}
return 0;
}
opponent_obstacle.a = opp_a;
if (!is_in_area(opp_x, opp_y, 250)) {
- if (xxxdebug != 3) {
- DEBUG(E_USER_STRAT, "XXX not in area");
- DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+ if (strat_obstacle_debug != 3) {
+ DEBUG(E_USER_STRAT,
+ "opponent not in area : d=%d, a=%d "
"x=%d y=%d (speed_d=%d)",
opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
- xxxdebug = 3;
+ strat_obstacle_debug = 3;
}
return 0;
}
/* sensor are temporarily disabled */
if (sensor_obstacle_is_disabled()) {
- if (xxxdebug != 4) {
- DEBUG(E_USER_STRAT, "XXX disabled");
- DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+ if (strat_obstacle_debug != 4) {
+ DEBUG(E_USER_STRAT,
+ "sensor are disabled: opponent d=%d, a=%d "
"x=%d y=%d (speed_d=%d)",
opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
- xxxdebug = 4;
+ strat_obstacle_debug = 4;
}
return 0;
}
/* opponent too far */
if (opp_d > 650) {
- if (xxxdebug != 5) {
- DEBUG(E_USER_STRAT, "XXX too far");
- DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+ if (strat_obstacle_debug != 5) {
+ DEBUG(E_USER_STRAT, "opponent too far d=%d, a=%d "
"x=%d y=%d (speed_d=%d)",
opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
- xxxdebug = 5;
+ strat_obstacle_debug = 5;
}
return 0;
}
return 1;
}
- if (xxxdebug != 6) {
- DEBUG(E_USER_STRAT, "XXX not in cone");
- DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+ if (strat_obstacle_debug != 6) {
+ DEBUG(E_USER_STRAT, "opponent not in cone d=%d, a=%d "
"x=%d y=%d (speed_d=%d)",
opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
- xxxdebug = 6;
+ strat_obstacle_debug = 6;
}
return 0;
return END_OBSTACLE;
}
+#ifdef HOST_VERSION
+ if (robotsim_blocking) {
+ robotsim_blocking = 0;
+ return END_BLOCKING;
+ }
+#endif
return 0;
}
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)
}
else {
beta_deg = 0;
- *pack_spickles = I2C_RIGHT_SIDE;
+ *pack_spickles = I2C_LEFT_SIDE;
}
}
/* double 90 deg for half turn -- not used */
};
/* there is a corn near */
-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);
/* go from line num1,dir1 to line num2,dir2. Uses trjectory flags
* specified as argument and return END_xxx condition */
/* return 1 or 0 depending on which side of a line (y=cste) is the
* robot. works in yellow or blue color. */
-uint8_t y_is_more_than(int16_t y)
+uint8_t __y_is_more_than(int16_t posy, int16_t y)
{
- int16_t posy;
-
- posy = position_get_y_s16(&mainboard.pos);
if (mainboard.our_color == I2C_COLOR_YELLOW) {
if (posy > y)
return 1;
/* return 1 or 0 depending on which side of a line (x=cste) is the
* robot. works in yellow or blue color. */
-uint8_t x_is_more_than(int16_t x)
+uint8_t __x_is_more_than(int16_t posx, int16_t x)
{
- int16_t posx;
-
- posx = position_get_x_s16(&mainboard.pos);
if (posx > x)
return 1;
else
return 0;
}
+/* return 1 or 0 depending on which side of a line (y=cste) is the
+ * robot. works in yellow or blue color. */
+uint8_t y_is_more_than(int16_t y)
+{
+ int16_t posy;
+ posy = position_get_y_s16(&mainboard.pos);
+ return __y_is_more_than(posy, y);
+}
+
+/* return 1 or 0 depending on which side of a line (x=cste) is the
+ * robot. works in yellow or blue color. */
+uint8_t x_is_more_than(int16_t x)
+{
+ int16_t posx;
+ posx = position_get_x_s16(&mainboard.pos);
+ return __x_is_more_than(posx, x);
+}
+
int16_t sin_table[] = {
0,
3211,
uint8_t robot_is_near_disc(void);
uint8_t y_is_more_than(int16_t y);
uint8_t x_is_more_than(int16_t x);
+uint8_t __y_is_more_than(int16_t posy, int16_t y);
+uint8_t __x_is_more_than(int16_t posx, int16_t x);
int16_t fast_sin(int16_t deg);
int16_t fast_cos(int16_t deg);
uint8_t get_color(void);
git.droids-corp.org / aversive.git / commitdiff