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_utils.h"
69 /* return 1 if there is a corn near, and fill the index ptr */
70 int8_t corn_is_near(uint8_t *corn_idx, uint8_t side)
72 #define SENSOR_CORN_DIST 225
73 #define SENSOR_CORN_ANGLE 90
74 double x = position_get_x_double(&mainboard.pos);
75 double y = position_get_y_double(&mainboard.pos);
76 double a_rad = position_get_a_rad_double(&mainboard.pos);
77 double x_corn, y_corn;
78 int16_t x_corn_int, y_corn_int;
79 struct waypoint_db *wp;
81 if (side == I2C_LEFT_SIDE) {
82 x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
83 y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
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;
92 wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
95 *corn_idx = wp->corn.idx;
100 * - send the correct commands to the spickles
101 * - return 1 if we need to stop (cobboard is stucked)
103 static uint8_t handle_spickles(void)
109 if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE))
110 i2c_cobboard_mode_deploy(I2C_LEFT_SIDE);
112 if (corn_table[corn_idx] == TYPE_WHITE_CORN)
113 i2c_cobboard_mode_harvest(I2C_LEFT_SIDE);
115 i2c_cobboard_mode_pack(I2C_LEFT_SIDE);
117 /* printf("%d %d\n", corn_idx, corn_table[corn_idx]); */
118 /* time_wait_ms(100); */
120 if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE))
121 i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE);
123 if (corn_table[corn_idx] == TYPE_WHITE_CORN)
124 i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE);
126 i2c_cobboard_mode_pack(I2C_RIGHT_SIDE);
133 uint8_t line2line(uint8_t dir1, uint8_t num1,
134 uint8_t dir2, uint8_t num2)
136 double line1_a_rad, line1_a_deg, line2_a_rad;
137 double diff_a_deg, diff_a_deg_abs, beta_deg;
139 struct line_2pts l1, l2;
143 uint16_t a_speed, d_speed;
145 /* convert to 2 points */
146 num2line(&l1, dir1, num1);
147 num2line(&l2, dir2, num2);
149 DEBUG(E_USER_STRAT, "line1: (%2.2f, %2.2f) -> (%2.2f, %2.2f)",
150 l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
151 DEBUG(E_USER_STRAT, "line2: (%2.2f, %2.2f) -> (%2.2f, %2.2f)",
152 l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
154 /* convert to line eq and find intersection */
155 pts2line(&l1.p1, &l1.p2, &ll1);
156 pts2line(&l2.p1, &l2.p2, &ll2);
157 intersect_line(&ll1, &ll2, &p);
159 line1_a_rad = atan2(l1.p2.y - l1.p1.y,
161 line1_a_deg = DEG(line1_a_rad);
162 line2_a_rad = atan2(l2.p2.y - l2.p1.y,
164 diff_a_deg = DEG(line2_a_rad - line1_a_rad);
165 if (diff_a_deg < -180) {
168 else if (diff_a_deg > 180) {
171 diff_a_deg_abs = fabs(diff_a_deg);
173 /* printf_P(PSTR("diff_a_deg=%2.2f\r\n"), diff_a_deg_abs); */
174 /* printf_P(PSTR("inter=%2.2f,%2.2f\r\n"), p.x, p.y); */
176 if (diff_a_deg_abs < 70.) {
183 else if (diff_a_deg_abs < 100.) {
198 /* XXX check return value !! */
199 trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
200 line1_a_deg, 150., diff_a_deg, beta_deg,
201 radius, xy_norm(l1.p1.x, l1.p1.y,
207 err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF);
209 /* cobboard is stucked */
210 trajectory_hardstop(&mainboard.traj);
211 return err; /* XXX do something */
213 err = test_traj_end(0xFF);
217 err = WAIT_COND_OR_TRAJ_END(get_cob_count() == 5, 0xFF);
218 strat_get_speed(&d_speed, &a_speed);
220 /* XXX 600 -> cste */
221 /* if (err == 0 && d_speed < 600 && */
222 /* mainboard.traj.state == RUNNING_CLITOID_LINE) */
223 /* strat_set_speed(600, SPEED_ANGLE_FAST); */
225 err = wait_traj_end(0xFF);
230 void num2line(struct line_2pts *l, uint8_t dir, uint8_t num)
237 l->p1.x = n * 450 + 375;
238 l->p1.y = COLOR_Y(0);
239 l->p2.x = n * 450 + 375;
240 l->p2.y = COLOR_Y(2100);
243 l->p1.x = n * 450 + 375;
244 l->p1.y = COLOR_Y(2100);
245 l->p2.x = n * 450 + 375;
246 l->p2.y = COLOR_Y(0);
250 l->p1.y = COLOR_Y(-n * 500 + 1472);
252 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
256 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
258 l->p2.y = COLOR_Y(-n * 500 + 1472);
262 l->p1.y = COLOR_Y(-n * 500 + 1472);
264 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
268 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
270 l->p2.y = COLOR_Y(-n * 500 + 1472);