-/*
+/*
* 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 <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>
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);
return END_BLOCKING;
}
-/* reset position */
+/* reset position */
void strat_reset_pos(int16_t x, int16_t y, int16_t a)
{
int16_t posx = position_get_x_s16(&mainboard.pos);
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);
uint8_t err;
- pid_set_gains(&mainboard.angle.pid, 150, 0, 2000);
+ pid_set_maximums(&mainboard.distance.pid, 0, 20000, 1000);
+ pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
trajectory_d_rel(&mainboard.traj, dist);
err = wait_traj_end(flags);
pid_set_gains(&mainboard.angle.pid, p, i, d);
+ pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
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;
/* 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();
/* 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;
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())
/* opponent is in front of us */
if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) {
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",
+ 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;
}
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;
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