-/*
+/*
* Copyright Droids Corporation, Microb Technology (2009)
- *
+ *
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <spi.h>
#include <pid.h>
#include <quadramp.h>
#include <control_system_manager.h>
#include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
#include <vect_base.h>
#include <lines.h>
#include <polygon.h>
#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;
return "END_UNKNOWN";
}
-void strat_hardstop(void)
+void strat_hardstop(void)
{
+ DEBUG(E_USER_STRAT, "strat_hardstop");
+
trajectory_hardstop(&mainboard.traj);
pid_reset(&mainboard.angle.pid);
pid_reset(&mainboard.distance.pid);
{
uint8_t i, err;
-#ifdef HOMOLOGATION
- int8_t serr;
- uint8_t hardstop = 0;
- microseconds us = time_get_us2();
- int16_t opp_a, opp_d, opp_x, opp_y;
-
- while (1) {
- serr = get_opponent_xyda(&opp_x, &opp_y,
- &opp_d, &opp_a);
- if (serr == -1)
- break;
- if (opp_d < 600)
- break;
- if (hardstop == 0) {
- strat_hardstop();
- hardstop = 1;
- }
- if ((time_get_us2() - us) > 3000000L)
- break;
- }
-#endif
for (i=0; i<3; i++) {
trajectory_goto_xy_abs(&mainboard.traj, x, y);
err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
return END_BLOCKING;
}
-/* reset position */
-void strat_reset_pos(int16_t x, int16_t y, int16_t a)
+/* reset position */
+void strat_reset_pos(int16_t x, int16_t y, double a)
{
- int16_t posx = position_get_x_s16(&mainboard.pos);
- int16_t posy = position_get_y_s16(&mainboard.pos);
- int16_t posa = position_get_a_deg_s16(&mainboard.pos);
+ double posx = position_get_x_double(&mainboard.pos);
+ double posy = position_get_y_double(&mainboard.pos);
+ double posa = position_get_a_rad_double(&mainboard.pos);
if (x == DO_NOT_SET_POS)
x = posx;
if (y == DO_NOT_SET_POS)
y = posy;
if (a == DO_NOT_SET_POS)
- a = posa;
-
+ a = DEG(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",
DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__);
}
-/*
+/*
* decrease gain on angle PID, and go forward until we reach the
* border.
*/
int32_t p = pid_get_gain_P(&mainboard.angle.pid);
int32_t i = pid_get_gain_I(&mainboard.angle.pid);
int32_t d = pid_get_gain_D(&mainboard.angle.pid);
+ 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;
- pid_set_gains(&mainboard.angle.pid, 150, 0, 2000);
+ /* 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, 500, 10, 4000);
trajectory_d_rel(&mainboard.traj, dist);
err = wait_traj_end(flags);
- pid_set_gains(&mainboard.angle.pid, p, i, d);
- return err;
-}
+ if (err != END_BLOCKING)
+ goto fail;
-/* escape from zone, and don't brake, so we can continue with another
- * traj */
-uint8_t strat_escape(struct build_zone *zone, uint8_t flags)
-{
- uint8_t err;
- uint16_t old_spdd, old_spda;
-
- strat_get_speed(&old_spdd, &old_spda);
-
- err = WAIT_COND_OR_TIMEOUT(!opponent_is_behind(), 1000);
- if (err == 0) {
- strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
- trajectory_d_rel(&mainboard.traj, -150);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- strat_set_speed(old_spdd, old_spda);
- return err;
- }
-
- strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+ /* 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(500);
+ 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
- if (zone->flags & ZONE_F_DISC) {
- trajectory_d_rel(&mainboard.traj, -350);
- err = WAIT_COND_OR_TRAJ_END(!robot_is_near_disc(), flags);
- }
- else {
- trajectory_d_rel(&mainboard.traj, -300);
- err = wait_traj_end(flags);
- }
+ 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);
- strat_set_speed(old_spdd, old_spda);
- if (err == 0)
- return END_NEAR;
return err;
}
a = strat_speed_a;
if (strat_limit_speed_a && a > strat_limit_speed_a)
a = strat_limit_speed_a;
-
+
trajectory_set_speed(&mainboard.traj, d, a);
}
IRQ_UNLOCK(flags);
}
+void strat_set_acc(double d, double a)
+{
+ trajectory_set_acc(&mainboard.traj, d, a);
+}
+
void strat_get_speed(uint16_t *d, uint16_t *a)
{
uint8_t flags;
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;
if (get_opponent_da(&opp_d, &opp_a) != 0)
goto update;
-#ifdef HOMOLOGATION
- if (opp_d < 600) {
- lim_d = 150;
- lim_a = 200;
- }
-#else
if (opp_d < 500) {
if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) {
lim_d = SPEED_DIST_VERY_SLOW;
lim_a = SPEED_ANGLE_VERY_SLOW;
}
}
-#endif
else if (opp_d < 800) {
if (mainboard.speed_d > 0 && (opp_a > 290 || opp_a < 70)) {
lim_d = SPEED_DIST_SLOW;
/* start the strat */
void strat_start(void)
-{
- int8_t i;
+{
uint8_t err;
strat_preinit();
+#ifndef HOST_VERSION
/* if start sw not plugged */
if (sensor_get(S_START_SWITCH)) {
+ int8_t i;
+
printf_P(PSTR("No start switch, press a key or plug it\r\n"));
/* while start sw not plugged */
break;
}
}
-
+
/* if start sw plugged */
if (!sensor_get(S_START_SWITCH)) {
printf_P(PSTR("Ready, unplug start switch to start\r\n"));
/* while start sw plugged */
while (!sensor_get(S_START_SWITCH));
}
+#endif
strat_init();
err = strat_main();
strat_exit();
}
+
/* 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) < 150)
+ 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 (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;
+ }
/* save obstacle position */
opponent_obstacle.x = opp_x;
opponent_obstacle.d = opp_d;
opponent_obstacle.a = opp_a;
+ if (!is_in_area(opp_x, opp_y, 250)) {
+ 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);
+ strat_obstacle_debug = 3;
+ }
+ return 0;
+ }
+
/* sensor are temporarily disabled */
- if (sensor_obstacle_is_disabled())
+ if (sensor_obstacle_is_disabled()) {
+ 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);
+ strat_obstacle_debug = 4;
+ }
return 0;
+ }
/* relative position */
x_rel = cos(RAD(opp_a)) * (double)opp_d;
y_rel = sin(RAD(opp_a)) * (double)opp_d;
/* opponent too far */
- if (opp_d > 600)
+ if (opp_d > 650) {
+ 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);
+ strat_obstacle_debug = 5;
+ }
return 0;
+ }
/* opponent is in front of us */
- if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) {
+ if ((mainboard.speed_d >= 0 && opp_d < 600 && (opp_a > 315 || opp_a < 45)) ||
+ (mainboard.speed_d >= 0 && opp_d < 800 && (opp_a > 330 || opp_a < 30))) {
DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d "
- "xrel=%d yrel=%d (speed_d=%d)",
+ "xrel=%d yrel=%d (speed_d=%d)",
opp_d, opp_a, x_rel, y_rel, mainboard.speed_d);
sensor_obstacle_disable();
return 1;
}
/* opponent is behind us */
- if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) {
- DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d",
+ if ((mainboard.speed_d < 0 && opp_d < 600 && (opp_a < 225 && opp_a > 135)) ||
+ (mainboard.speed_d < 0 && opp_d < 800 && (opp_a < 210 && opp_a > 150))) {
+ DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d",
opp_d, opp_a, x_rel, y_rel);
sensor_obstacle_disable();
return 1;
}
+ 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);
+ strat_obstacle_debug = 6;
+ }
+
return 0;
}
}
uint8_t test_traj_end(uint8_t why)
-{
+{
uint16_t cur_timer;
point_t robot_pt;
}
if ((why & END_INTR) && traj_intr) {
- interrupt_traj_reset();
+ interrupt_traj_reset();
return END_INTR;
}
if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj))
return END_TRAJ;
-
+
/* we are near the destination point (depends on current
* speed) AND the robot is in the area bounding box. */
if (why & END_NEAR) {
- int16_t d_near = 100;
-
+ int16_t d_near = 100;
+
if (mainboard.speed_d > 2000)
d_near = 150;
-
+
if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) &&
is_in_boundingbox(&robot_pt))
return END_NEAR;
}
-
+
if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) {
strat_hardstop();
return END_BLOCKING;
return END_OBSTACLE;
}
+#ifdef HOST_VERSION
+ if (robotsim_blocking) {
+ robotsim_blocking = 0;
+ return END_BLOCKING;
+ }
+#endif
return 0;
}
line, opp_x, opp_y, opp_d, opp_a);
}
else {
- DEBUG(E_USER_STRAT, "Got %s at line %d",
+ DEBUG(E_USER_STRAT, "Got %s at line %d",
get_err(ret), line);
}
return ret;
git.droids-corp.org / aversive.git / blobdiff