if (a == DO_NOT_SET_POS)
a = posa;
+ /* some issues with blocking on simulator */
+#ifdef HOST_VERSION
+ if (x == ROBOT_HALF_LENGTH_REAR)
+ x = ROBOT_HALF_LENGTH_REAR + 10;
+ if (x == AREA_X - ROBOT_HALF_LENGTH_REAR)
+ x = AREA_X - ROBOT_HALF_LENGTH_REAR - 10;
+ if (y == ROBOT_HALF_LENGTH_REAR)
+ y = ROBOT_HALF_LENGTH_REAR + 10;
+ if (y == AREA_Y - ROBOT_HALF_LENGTH_REAR)
+ y = AREA_Y - ROBOT_HALF_LENGTH_REAR - 10;
+ if (x == ROBOT_HALF_LENGTH_FRONT)
+ x = ROBOT_HALF_LENGTH_FRONT + 10;
+ if (x == AREA_X - ROBOT_HALF_LENGTH_FRONT)
+ x = AREA_X - ROBOT_HALF_LENGTH_FRONT - 10;
+ if (y == ROBOT_HALF_LENGTH_FRONT)
+ y = ROBOT_HALF_LENGTH_FRONT + 10;
+ if (y == AREA_Y - ROBOT_HALF_LENGTH_FRONT)
+ y = AREA_Y - ROBOT_HALF_LENGTH_FRONT - 10;
+#endif
DEBUG(E_USER_STRAT, "reset pos (%s%s%s)",
x == DO_NOT_SET_POS ? "" : "x",
y == DO_NOT_SET_POS ? "" : "y",
int32_t max_in = pid_get_max_in(&mainboard.angle.pid);
int32_t max_i = pid_get_max_I(&mainboard.angle.pid);
int32_t max_out = pid_get_max_out(&mainboard.angle.pid);
+ uint32_t i_thres = mainboard.distance.bd.i_thres;
+ int32_t k1 = mainboard.distance.bd.k1;
+ int32_t k2 = mainboard.distance.bd.k2;
+ uint16_t cpt_thres = mainboard.distance.bd.cpt_thres;
+
uint8_t err;
+ /* go with a lower angle pid, and with a sensible blocking
+ * detection */
+ bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 400000, 20);
pid_set_maximums(&mainboard.distance.pid, 0, 20000, 1000);
- pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
+ pid_set_gains(&mainboard.angle.pid, 500, 10, 4000);
trajectory_d_rel(&mainboard.traj, dist);
err = wait_traj_end(flags);
+ if (err != END_BLOCKING)
+ goto fail;
+
+ /* we detected a blocking, reset bd, remove angle pid and
+ * continue */
+ trajectory_d_rel(&mainboard.traj, dist);
+ pid_set_maximums(&mainboard.distance.pid, max_in, max_i, 4095);
+ pid_set_gains(&mainboard.angle.pid, 1, 0, 0);
+ time_wait_ms(300);
+ strat_hardstop();
+
+#ifdef HOST_VERSION
+ /* issue with block on simulator */
+ if (dist > 0)
+ trajectory_d_rel(&mainboard.traj, -10);
+ else
+ trajectory_d_rel(&mainboard.traj, 10);
+ wait_traj_end(END_TRAJ);
+#endif
+
+ fail:
pid_set_gains(&mainboard.angle.pid, p, i, d);
pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
+ bd_set_current_thresholds(&mainboard.distance.bd, k1, k2, i_thres, cpt_thres);
+
return err;
}
IRQ_UNLOCK(flags);
}
+void strat_get_acc(double *d, double *a)
+{
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ *d = mainboard.traj.d_acc;
+ *a = mainboard.traj.a_acc;
+ IRQ_UNLOCK(flags);
+}
+
void strat_limit_speed_enable(void)
{
strat_limit_speed_enabled = 1;
strat_limit_speed_enabled = 0;
}
-/* called periodically */
+/* called periodically (note: disabled in 2010) */
void strat_limit_speed(void)
{
uint16_t lim_d = 0, lim_a = 0;
/* return true if we have to brake due to an obstacle */
uint8_t strat_obstacle(void)
{
-#if 0
int16_t x_rel, y_rel;
int16_t opp_x, opp_y, opp_d, opp_a;
return 0;
/* no opponent detected */
- if (get_opponent_xyda(&opp_x, &opp_y,
- &opp_d, &opp_a))
+ if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a))
return 0;
/* save obstacle position */
opponent_obstacle.y = opp_y;
opponent_obstacle.d = opp_d;
opponent_obstacle.a = opp_a;
-#else /* belgium cup only */
- int16_t x_rel, y_rel;
- int16_t opp_d, opp_a;
- double opp_x, opp_y;
-#ifdef HOST_VERSION
- return 0;
- if (time_get_s() >= 12 && time_get_s() <= 30)
- return 1;
-#endif
- return 0; /* XXX disabled */
-
- if (!sensor_get(S_RCOB_WHITE))
- return 0;
-
- opp_a = 0;
- opp_d = 300;
-
- rel_da_to_abs_xy(opp_d, RAD(opp_a), &opp_x, &opp_y);
if (!is_in_area(opp_x, opp_y, 250))
return 0;
-#endif
/* sensor are temporarily disabled */
if (sensor_obstacle_is_disabled())
y_rel = sin(RAD(opp_a)) * (double)opp_d;
/* opponent too far */
- if (opp_d > 600)
+ if (opp_d > 500)
return 0;
/* opponent is in front of us */