2 * Copyright Droids, Microb Technology (2010)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
30 #include <aversive/error.h>
31 #include <aversive/pgmspace.h>
36 #include <clock_time.h>
41 #include <control_system_manager.h>
42 #include <trajectory_manager.h>
43 #include <trajectory_manager_utils.h>
44 #include <trajectory_manager_core.h>
45 #include <vect_base.h>
48 #include <obstacle_avoidance.h>
49 #include <blocking_detection_manager.h>
50 #include <robot_system.h>
51 #include <position_manager.h>
53 #include <diagnostic.h>
58 #include "../common/i2c_commands.h"
59 #include "i2c_protocol.h"
63 #include "strat_base.h"
64 #include "strat_corn.h"
65 #include "strat_avoid.h"
66 #include "strat_utils.h"
70 static volatile uint8_t clitoid_slow = 0;
72 /* return 1 if there is a corn near, and fill the index ptr */
73 int8_t corn_is_near(uint8_t *corn_idx, uint8_t side)
75 /* XXX to be checked */
76 #define SENSOR_CORN_DIST 225
77 #define SENSOR_CORN_ANGLE 90
78 double x = position_get_x_double(&mainboard.pos);
79 double y = position_get_y_double(&mainboard.pos);
80 double a_rad = position_get_a_rad_double(&mainboard.pos);
81 double x_corn, y_corn;
82 int16_t x_corn_int, y_corn_int;
83 struct waypoint_db *wp;
85 if (side == I2C_LEFT_SIDE) {
86 x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
87 y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
90 x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
91 y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
96 wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
99 *corn_idx = wp->corn.idx;
103 /* fill 2 points that are on the line (num, dir) */
104 static void num2line(struct line_2pts *l, uint8_t num, uint8_t dir)
111 l->p1.x = n * 450 + 375;
112 l->p1.y = COLOR_Y(0);
113 l->p2.x = n * 450 + 375;
114 l->p2.y = COLOR_Y(2100);
117 l->p1.x = n * 450 + 375;
118 l->p1.y = COLOR_Y(2100);
119 l->p2.x = n * 450 + 375;
120 l->p2.y = COLOR_Y(0);
124 l->p1.y = COLOR_Y(-n * 500 + 1472);
126 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
130 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
132 l->p2.y = COLOR_Y(-n * 500 + 1472);
136 l->p1.y = COLOR_Y(-n * 500 + 1472);
138 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
142 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
144 l->p2.y = COLOR_Y(-n * 500 + 1472);
151 /* return true if we must go slow */
152 static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1,
153 uint8_t num2, uint8_t dir2)
157 uint8_t i2, i3, j2, j3; /* next wp */
159 x = position_get_x_s16(&mainboard.pos);
160 y = position_get_y_s16(&mainboard.pos);
162 if (get_cob_count() >= 5)
165 if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
166 DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
171 /* if (time_get_s() > 39) */
172 /* DEBUG(E_USER_STRAT, "i,j = (%d %d), count=%d", i, j, corn_count_neigh(i, j)); */
174 if (corn_count_neigh(i, j) == 2)
177 if (wp_belongs_to_line(i, j, num2, dir2))
181 if (wp_get_neigh(i, j, &i2, &j2, dir1) < 0) {
182 DEBUG(E_USER_STRAT, "%s(): cannot get neigh1",
187 /* if (i2, j2) belongs to next line, check corn in opposition */
188 if (wp_belongs_to_line(i2, j2, num2, dir2)) {
189 if (corn_count_neigh(i2, j2) > 0)
196 if (wp_get_neigh(i2, j2, &i3, &j3, dir1) < 0) {
197 DEBUG(E_USER_STRAT, "%s(): cannot get neigh2",
202 /* if (i3, j3) belongs to next line, check corn in opposition */
203 if (wp_belongs_to_line(i3, j3, num2, dir2)) {
204 if (corn_count_neigh(i2, j2) > 0 ||
205 corn_count_neigh(i3, j3) > 0)
216 * handle speed before clitoid (on the line), depending on strat_db.
217 * return true if clitoid started
219 #define NORETURN_DIST 300
220 static uint8_t speedify_clitoid(uint8_t num1, uint8_t dir1,
221 uint8_t num2, uint8_t dir2)
226 slow = clitoid_select_speed(num1, dir1, num2, dir2);
227 if (slow != clitoid_slow) {
228 turnx = mainboard.traj.target.line.turn_pt.x;
229 turny = mainboard.traj.target.line.turn_pt.y;
230 if (distance_from_robot(turnx, turny) > NORETURN_DIST) {
236 return trajectory_get_state(&mainboard.traj) == RUNNING_CLITOID_CURVE;
239 /* process the clitoid parameters, return 0 on success or -1 if
240 * clitoid cannot be executed. pack_spickles is set to I2C_LEFT_SIDE,
241 * I2C_RIGHT_SIDE or I2C_NO_SIDE to tell if we need to pack a specific
243 static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1,
244 uint8_t num2, uint8_t dir2,
245 uint8_t *pack_spickles)
247 double line1_a_rad, line1_a_deg, line2_a_rad;
248 double diff_a_deg, diff_a_deg_abs, beta_deg;
250 struct line_2pts l1, l2;
255 /* convert to 2 points */
256 num2line(&l1, num1, dir1);
257 num2line(&l2, num2, dir2);
259 DEBUG(E_USER_STRAT, "line1: (%2.2f, %2.2f) -> (%2.2f, %2.2f)",
260 l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
261 DEBUG(E_USER_STRAT, "line2: (%2.2f, %2.2f) -> (%2.2f, %2.2f)",
262 l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
264 /* convert to line eq and find intersection */
265 pts2line(&l1.p1, &l1.p2, &ll1);
266 pts2line(&l2.p1, &l2.p2, &ll2);
267 intersect_line(&ll1, &ll2, &p);
269 line1_a_rad = atan2(l1.p2.y - l1.p1.y,
271 line1_a_deg = DEG(line1_a_rad);
272 line2_a_rad = atan2(l2.p2.y - l2.p1.y,
274 diff_a_deg = DEG(line2_a_rad - line1_a_rad);
275 if (diff_a_deg < -180) {
278 else if (diff_a_deg > 180) {
281 diff_a_deg_abs = fabs(diff_a_deg);
283 /* printf_P(PSTR("diff_a_deg=%2.2f\r\n"), diff_a_deg_abs); */
284 /* printf_P(PSTR("inter=%2.2f,%2.2f\r\n"), p.x, p.y); */
286 *pack_spickles = I2C_NO_SIDE;
288 /* small angle, 60 deg */
289 if (diff_a_deg_abs < 70.) {
291 if (diff_a_deg > 0) {
293 *pack_spickles = I2C_RIGHT_SIDE;
297 *pack_spickles = I2C_RIGHT_SIDE;
300 /* double 90 deg for half turn -- not used */
301 else if (diff_a_deg_abs < 100.) {
308 /* hard turn, 120 deg */
317 clitoid_slow = clitoid_select_speed(num1, dir1, num2, dir2);
319 DEBUG(E_USER_STRAT, "slow clito\n");
320 strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
323 DEBUG(E_USER_STRAT, "fast clito\n");
324 strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
327 /* XXX check return value !! */
328 ret = trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
329 line1_a_deg, 150., diff_a_deg, beta_deg,
330 radius, xy_norm(l1.p1.x, l1.p1.y,
335 /* go from line num1,dir1 to line num2,dir2. Uses trjectory flags
336 * specified as argument and return END_xxx condition */
337 uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2,
338 uint8_t dir2, uint8_t flags)
341 uint8_t err, pack_spickles;
344 ret = strat_calc_clitoid(num1, dir1, num2, dir2, &pack_spickles);
346 DEBUG(E_USER_STRAT, "clitoid failed");
348 /* XXX what to do if cobboard is stucked */
350 /* wait beginning of clitoid or changing of speed */
351 err = WAIT_COND_OR_TRAJ_END(speedify_clitoid(num1, dir1,
355 /* the speed has to change */
356 if (trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE)
359 DEBUG(E_USER_STRAT, "clitoid started err=%d pack_spickles=%d",
362 /* when clitoid starts and angle is 60 deg, pack external
365 if (pack_spickles == I2C_LEFT_SIDE)
367 else if (pack_spickles == I2C_RIGHT_SIDE)
370 /* wait end of clitoid */
371 err = wait_traj_end(flags);
374 DEBUG(E_USER_STRAT, "clitoid finished");
376 /* XXX 600 -> cste */
377 /* XXX does not work, do better */
378 /* if (err == 0 && d_speed < 600 && */
379 /* mainboard.traj.state == RUNNING_CLITOID_LINE) */
380 /* strat_set_speed(600, SPEED_ANGLE_FAST); */