3694bcdab01e70367ac9c2ac3a52358e7ed776d6
[aversive.git] / strat_corn.c
1 /*
2  *  Copyright Droids, Microb Technology (2010)
3  *
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.
8  *
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.
13  *
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
17  *
18  *  Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
19  *
20  *  Olivier MATZ <zer0@droids-corp.org>
21  */
22
23
24 #include <stdio.h>
25 #include <stdlib.h>
26 #include <stdint.h>
27 #include <math.h>
28
29 #include <aversive.h>
30 #include <aversive/pgmspace.h>
31
32 #include <ax12.h>
33 #include <uart.h>
34 #include <pwm_ng.h>
35 #include <clock_time.h>
36 #include <spi.h>
37
38 #include <pid.h>
39 #include <quadramp.h>
40 #include <control_system_manager.h>
41 #include <trajectory_manager.h>
42 #include <trajectory_manager_utils.h>
43 #include <trajectory_manager_core.h>
44 #include <vect_base.h>
45 #include <lines.h>
46 #include <polygon.h>
47 #include <obstacle_avoidance.h>
48 #include <blocking_detection_manager.h>
49 #include <robot_system.h>
50 #include <position_manager.h>
51
52 #include <diagnostic.h>
53
54 #include <rdline.h>
55 #include <parse.h>
56
57 #include "../common/i2c_commands.h"
58 #include "i2c_protocol.h"
59 #include "main.h"
60 #include "strat.h"
61 #include "strat_db.h"
62 #include "strat_base.h"
63 #include "strat_corn.h"
64 #include "strat_utils.h"
65 #include "sensor.h"
66 #include "actuator.h"
67
68 #if 0
69 /* return 1 if there is a corn near, and fill the index ptr */
70 uint8_t corn_is_near(int8_t *corn_idx, uint8_t side)
71 {
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;
80
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;
84         }
85         else {
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;
88         }
89         x_corn_int = x_corn;
90         y_corn_int = y_corn;
91
92         wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
93         if (wp->corn.idx < 0)
94                 return 0;
95         return 1;
96 }
97 #endif
98
99 /*
100  * - send the correct commands to the spickles
101  * - return 1 if we need to stop (cobboard is stucked)
102 */
103 static uint8_t handle_spickles(void)
104 {
105         return 0;
106 #if 0
107         int8_t corn_idx;
108
109         if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE))
110                 i2c_cobboard_mode_deploy(I2C_LEFT_SIDE);
111         else {
112                 if (corn_table[corn_idx] == TYPE_WHITE_CORN)
113                         i2c_cobboard_mode_harvest(I2C_LEFT_SIDE);
114                 else
115                         i2c_cobboard_mode_pack(I2C_LEFT_SIDE);
116         }
117 /*      printf("%d %d\n", corn_idx, corn_table[corn_idx]); */
118 /*      time_wait_ms(100); */
119
120         if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE))
121                 i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE);
122         else {
123                 if (corn_table[corn_idx] == TYPE_WHITE_CORN)
124                         i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE);
125                 else
126                         i2c_cobboard_mode_pack(I2C_RIGHT_SIDE);
127         }
128
129         return 0;
130 #endif
131 }
132
133 uint8_t line2line(uint8_t dir1, uint8_t num1,
134                   uint8_t dir2, uint8_t num2)
135 {
136         double line1_a_rad, line1_a_deg, line2_a_rad;
137         double diff_a_deg, diff_a_deg_abs, beta_deg;
138         double radius;
139         struct line_2pts l1, l2;
140         line_t ll1, ll2;
141         point_t p;
142         uint8_t err;
143
144         /* convert to 2 points */
145         num2line(&l1, dir1, num1);
146         num2line(&l2, dir2, num2);
147
148         printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
149                  l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
150         printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
151                  l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
152
153         /* convert to line eq and find intersection */
154         pts2line(&l1.p1, &l1.p2, &ll1);
155         pts2line(&l2.p1, &l2.p2, &ll2);
156         intersect_line(&ll1, &ll2, &p);
157
158         line1_a_rad = atan2(l1.p2.y - l1.p1.y,
159                             l1.p2.x - l1.p1.x);
160         line1_a_deg = DEG(line1_a_rad);
161         line2_a_rad = atan2(l2.p2.y - l2.p1.y,
162                             l2.p2.x - l2.p1.x);
163         diff_a_deg = DEG(line2_a_rad - line1_a_rad);
164         diff_a_deg_abs = fabs(diff_a_deg);
165
166         if (diff_a_deg_abs < 70.) {
167                 radius = 200;
168                 if (diff_a_deg > 0)
169                         beta_deg = 40;
170                 else
171                         beta_deg = -40;
172         }
173         else if (diff_a_deg_abs < 100.) {
174                 radius = 100;
175                 if (diff_a_deg > 0)
176                         beta_deg = 40;
177                 else
178                         beta_deg = -40;
179         }
180         else {
181                 radius = 120;
182                 if (diff_a_deg > 0)
183                         beta_deg = 60;
184                 else
185                         beta_deg = -60;
186         }
187
188         trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
189                            line1_a_deg, 150., diff_a_deg, beta_deg,
190                            radius, xy_norm(l1.p1.x, l1.p1.y,
191                                            p.x, p.y));
192         if (0) {
193                 err = 0;
194                 while (err == 0) {
195                         err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF);
196                         if (err == 0) {
197                                 /* cobboard is stucked */
198                                 trajectory_hardstop(&mainboard.traj);
199                                 return err; /* XXX do something */
200                         }
201                         err = test_traj_end(0xFF);
202                 }
203         }
204
205         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
206         return err;
207 }
208
209 void num2line(struct line_2pts *l, uint8_t dir, uint8_t num)
210 {
211         float n = num;
212
213         switch (dir) {
214
215         case LINE_UP:
216                 l->p1.x = n * 450 + 375;
217                 l->p1.y = COLOR_Y(0);
218                 l->p2.x = n * 450 + 375;
219                 l->p2.y = COLOR_Y(2100);
220                 break;
221         case LINE_DOWN:
222                 l->p1.x = n * 450 + 375;
223                 l->p1.y = COLOR_Y(2100);
224                 l->p2.x = n * 450 + 375;
225                 l->p2.y = COLOR_Y(0);
226                 break;
227         case LINE_R_UP:
228                 l->p1.x = 150;
229                 l->p1.y = COLOR_Y(-n * 500 + 1472);
230                 l->p2.x = 2850;
231                 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
232                 break;
233         case LINE_L_DOWN:
234                 l->p1.x = 2850;
235                 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
236                 l->p2.x = 150;
237                 l->p2.y = COLOR_Y(-n * 500 + 1472);
238                 break;
239         case LINE_L_UP:
240                 l->p1.x = 2850;
241                 l->p1.y = COLOR_Y(-n * 500 + 1472);
242                 l->p2.x = 150;
243                 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
244                 break;
245         case LINE_R_DOWN:
246                 l->p1.x = 150;
247                 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
248                 l->p2.x = 2850;
249                 l->p2.y = COLOR_Y(-n * 500 + 1472);
250                 break;
251         default:
252                 break;
253         }
254 }