7 #include <aversive/pgmspace.h>
8 #include <aversive/wait.h>
9 #include <aversive/error.h>
17 #include <encoders_spi.h>
19 #include <scheduler.h>
21 #include <clock_time.h>
23 #include <control_system_manager.h>
27 #include <blocking_detection_manager.h>
31 #include "../common/i2c_commands.h"
37 #define BEACON_PWM_VALUE 1000
38 #define IR_SENSOR() (!!(PINK&(1<<5)))
39 #define MODULO_TIMER (1023L)
40 #define COEFF_TIMER (2)
41 #define COEFF_MULT (100L)
42 #define COEFF_MULT2 (1000L)
43 #define BEACON_SIZE (9)
44 #define BEACON_MAX_SAMPLE (3)
46 #define OPPONENT_POS_X (11)
47 #define OPPONENT_POS_Y (22)
49 #define BEACON_DEBUG(args...) DEBUG(E_USER_BEACON, args)
50 #define BEACON_NOTICE(args...) NOTICE(E_USER_BEACON, args)
51 #define BEACON_ERROR(args...) ERROR(E_USER_BEACON, args)
53 static volatile int32_t rising = -1;
54 static volatile int32_t falling = -1;
56 static int32_t get_dist(float size);
57 static int32_t get_angle(int32_t middle, int32_t ref);
59 static int32_t pos_ref = 0;
60 static int32_t invalid_count = 0;
62 static volatile int32_t beacon_speed;
63 static volatile int32_t beacon_save_count = 0;
64 static volatile int32_t beacon_prev_save_count = 0;
65 static volatile int32_t count = 0;
66 static volatile int32_t count_diff_rising = 0;
67 static volatile int32_t count_diff_falling = 0;
68 static int32_t beacon_coeff = 0;
70 static volatile int8_t valid_beacon = 0;
72 static volatile int32_t beacon_pos;
74 //static int32_t beacon_sample_size[BEACON_MAX_SAMPLE];
76 int32_t encoders_spi_get_value_beacon(void *number)
80 ret = encoders_spi_get_value(number);
84 void encoders_spi_set_value_beacon(void * number, int32_t v)
86 encoders_spi_set_value(number, v/4);
89 int32_t encoders_spi_update_beacon_speed(void * number)
95 ret = encoders_spi_get_value_beacon(number);
96 beacon_speed = ret - beacon_pos;
98 beacon_prev_save_count = beacon_save_count;
99 beacon_save_count = TCNT3;
102 beacon_coeff = COEFF_TIMER * COEFF_MULT;//beacon_speed * COEFF_MULT / ((beacon_prev_save_count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER);
108 void beacon_init(void)
113 pos_ref = encoders_spi_get_value_beacon(BEACON_ENCODER);
115 memset(&beacon, 0, sizeof(struct beacon));
116 beacon.opponent_x = I2C_OPPONENT_NOT_THERE;
120 /*for(i=0;i<BEACON_MAX_SAMPLE;i++)
121 beacon_sample_size[i] = 0;*/
123 /* set external interrupt (any edge) */
124 PCMSK2 = (1<<PCINT21);
130 void beacon_calibre_pos(void)
132 ballboard.flags &= ~DO_CS;
134 /* init beacon pos */
135 pwm_ng_set(BEACON_PWM, 100);
137 /* find rising edge of the mirror*/
139 while (sensor_get(BEACON_POS_SENSOR));
141 while (!sensor_get(BEACON_POS_SENSOR));
143 pwm_ng_set(BEACON_PWM, 0);
147 pid_reset(&ballboard.beacon.pid);
148 encoders_spi_set_value_beacon(BEACON_ENCODER, BEACON_OFFSET_CALIBRE);
150 cs_set_consign(&ballboard.beacon.cs, 0);
152 ballboard.flags |= DO_CS;
155 void beacon_start(void)
158 ballboard.beacon.on = 1;
159 cs_set_consign(&ballboard.beacon.cs, 600);
162 void beacon_stop(void)
164 ballboard.beacon.on = 0;
165 pwm_ng_set(BEACON_PWM, 0);
168 void beacon_reset_pos(void)
170 pwm_ng_set(BEACON_PWM, 0);
171 encoders_spi_set_value(BEACON_ENCODER, 0);
176 int32_t encoders_spi_get_beacon_speed(void * dummy)
183 /* motor speed (top tour) */
184 SIGNAL(SIG_PIN_CHANGE2)
193 count_diff_rising = (count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER;
202 falling = beacon_pos;
203 count_diff_falling = (count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER;
209 void beacon_calc(void *dummy)
212 static int32_t local_rising, local_falling;
213 static int32_t middle;
214 static float size = 0;
218 int32_t local_count_diff_rising ;
219 int32_t local_count_diff_falling ;
220 int32_t local_beacon_coeff;
226 //int32_t total_size=0;
240 /* 0.5 second timeout */
241 if (invalid_count < 25)
245 beacon.opponent_x = I2C_OPPONENT_NOT_THERE;
253 local_valid = valid_beacon;
254 local_count_diff_rising = count_diff_rising;
255 local_count_diff_falling = count_diff_falling ;
256 local_rising = rising;
257 local_falling = falling;
258 local_beacon_coeff = beacon_coeff;
263 //BEACON_DEBUG("rising= %ld\t",local_rising);
264 //BEACON_DEBUG("falling= %ld\r\n",local_falling);
266 /* recalculate number of pulse by adding the value of the counter, then put value back into motor's round range */
267 local_rising = ((local_rising + (local_count_diff_rising * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR);
268 local_falling = ((local_falling + (local_count_diff_falling * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR);
270 //BEACON_DEBUG("rising1= %ld\t",local_rising);
271 //BEACON_DEBUG("falling1= %ld\r\n",local_falling);
273 //BEACON_DEBUG("count diff rising= %ld\t",local_count_diff_rising);
274 //BEACON_DEBUG("count diff falling= %ld\r\n",local_count_diff_falling);
276 /* if around 360 deg, rising > falling, so invert both and recalculate size and middle */
277 if(local_falling < local_rising){
279 local_rising = local_falling;
280 local_falling = temp;
281 size = BEACON_STEP_TOUR - local_falling + local_rising;
282 middle = (local_falling + ((int32_t)(size)/2) + BEACON_STEP_TOUR) %(BEACON_STEP_TOUR);
283 edge = local_falling;
285 /* else rising > falling */
287 size = local_falling - local_rising;
288 middle = local_rising + (size / 2);
292 //for(i=BEACON_MAX_SAMPLE-1;i>0;i--){
293 // beacon_sample_size[i] = beacon_sample_size[i-1];
294 // total_size += beacon_sample_size[i];
296 //beacon_sample_size[0] = size;
297 //total_size += size;
298 //total_size /= BEACON_MAX_SAMPLE;
300 //BEACON_DEBUG("rising2= %ld\t",local_rising);
301 //BEACON_DEBUG("falling2= %ld\r\n",local_falling);
302 /* BEACON_DEBUG("size= %ld %ld\t",size, total_size); */
303 BEACON_DEBUG("size= %f\r\n",size);
304 //BEACON_DEBUG("middle= %ld\r\n",middle);
306 local_angle = get_angle(middle,0);
307 BEACON_NOTICE("opponent angle= %ld\t",local_angle);
309 local_dist = get_dist(size);
310 BEACON_NOTICE("opponent dist= %ld\r\n",local_dist);
312 beacon_angle_dist_to_x_y(local_angle, local_dist, &result_x, &result_y);
315 beacon.opponent_x = result_x;
316 beacon.opponent_y = result_y;
317 beacon.opponent_angle = local_angle;
318 beacon.opponent_dist = local_dist;
320 //beacon.opponent_x = OPPONENT_POS_X;
321 //beacon.opponent_y = OPPONENT_POS_Y;
324 BEACON_NOTICE("opponent x= %ld\t",beacon.opponent_x);
325 BEACON_NOTICE("opponent y= %ld\r\n\n",beacon.opponent_y);
328 BEACON_NOTICE("non valid\r\n\n");
334 static int32_t get_dist(float size)
339 //alpha = (size*2*M_PI*COEFF_MULT2);
340 //dist = ((2*BEACON_SIZE*BEACON_STEP_TOUR*COEFF_MULT2)/alpha)/2;
342 /* function found by measuring points up to 80cm */
343 //dist = ((size - 600)*(size - 600)) / 2400 +28;
346 /* dist = a0 + a1*x + a2*x² + a3x³ */
347 dist = 1157.3 + (1.4146*size) + (-0.013508*size*size) + (0.00001488*size*size*size);
353 static int32_t get_angle(int32_t middle, int32_t ref)
357 ret_angle = (middle - ref) * 360 / BEACON_STEP_TOUR;
365 void beacon_angle_dist_to_x_y(int32_t angle, int32_t dist, int32_t *x, int32_t *y)
373 int32_t local_robot_angle;
376 local_x = beacon.robot_x;
377 local_y = beacon.robot_y;
378 local_robot_angle = beacon.robot_angle;
381 if (local_robot_angle < 0)
382 local_robot_angle += 360;
384 x_opponent = cos((local_robot_angle + angle)* 2 * M_PI / 360)* dist;
385 y_opponent = sin((local_robot_angle + angle)* 2 * M_PI / 360)* dist;
387 //BEACON_DEBUG("x_op= %ld\t",x_opponent);
388 //BEACON_DEBUG("y_op= %ld\r\n",y_opponent);
389 //BEACON_NOTICE("robot_x= %ld\t",local_x);
390 //BEACON_NOTICE("robot_y= %ld\t",local_y);
391 //BEACON_NOTICE("robot_angle= %ld\r\n",local_robot_angle);
393 *x = local_x + x_opponent;
394 *y = local_y + y_opponent;