projects/microb2010/sensorboard/beacon.c

   1
   2 #include <stdio.h>
   3 #include <string.h>
   4 #include <math.h>
   5
   6 #include <aversive.h>
   7 #include <aversive/pgmspace.h>
   8 #include <aversive/wait.h>
   9 #include <aversive/error.h>
  10
  11 #include <uart.h>
  12 #include <i2c.h>
  13 #include <ax12.h>
  14 #include <parse.h>
  15 #include <rdline.h>
  16 #include <pwm_ng.h>
  17 #include <encoders_spi.h>
  18 #include <timer.h>
  19 #include <scheduler.h>
  20 #include <pid.h>
  21 #include <time.h>
  22 #include <quadramp.h>
  23 #include <control_system_manager.h>
  24 #include <adc.h>
  25 #include <spi.h>
  26
  27 #include <blocking_detection_manager.h>
  28
  29 #include "sensor.h"
  30
  31 #include "../common/i2c_commands.h"
  32 #include "main.h"
  33 #include "beacon.h"
  34
  35 struct beacon beacon;
  36
  37 #define BEACON_PWM_VALUE 1000
  38 #define IR_SENSOR() (!!(PINK&(1<<2)))
  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)
  45
  46 #define OPPONENT_POS_X (11)
  47 #define OPPONENT_POS_Y (22)
  48
  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)
  52
  53 static volatile int32_t rising = -1;
  54 static volatile int32_t falling = -1;
  55
  56 static int32_t get_dist(float size);
  57 static int32_t get_angle(int32_t middle, int32_t ref);
  58
  59 static int32_t pos_ref = 0;
  60 static int32_t invalid_count = 0;
  61
  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;
  69
  70 static volatile int8_t valid_beacon = 0;
  71
  72 static volatile int32_t beacon_pos;
  73
  74 //static int32_t beacon_sample_size[BEACON_MAX_SAMPLE];
  75
  76 int32_t encoders_spi_get_value_beacon(void *number)
  77 {
  78         int32_t ret;
  79
  80         ret = encoders_spi_get_value(number);
  81         return ret*4;
  82 }
  83
  84
  85 void encoders_spi_set_value_beacon(void * number, int32_t v)
  86 {
  87         encoders_spi_set_value(number, v/4);
  88 }
  89
  90 int32_t encoders_spi_update_beacon_speed(void * number)
  91 {
  92         int32_t ret;
  93         uint8_t flags;
  94
  95         IRQ_LOCK(flags);
  96         ret = encoders_spi_get_value_beacon(number);
  97         beacon_speed = ret - beacon_pos;
  98         beacon_pos = ret;
  99         beacon_prev_save_count = beacon_save_count;
 100         beacon_save_count = TCNT3;
 101         IRQ_UNLOCK(flags);
 102
 103         beacon_coeff = COEFF_TIMER  * COEFF_MULT;//beacon_speed * COEFF_MULT / ((beacon_prev_save_count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER);
 104
 105         return beacon_speed;
 106 }
 107
 108
 109 void beacon_init(void)
 110 {
 111         //int8_t i;
 112
 113         beacon_reset_pos();
 114         pos_ref = encoders_spi_get_value_beacon(BEACON_ENCODER);
 115
 116         memset(&beacon, 0, sizeof(struct beacon));
 117         beacon.opponent_x = I2C_OPPONENT_NOT_THERE;
 118
 119         beacon_speed = 0;
 120
 121         /*for(i=0;i<BEACON_MAX_SAMPLE;i++)
 122                 beacon_sample_size[i] = 0;*/
 123
 124         /* set external interrupt (any edge) */
 125         PCMSK2 = (1<<PCINT18);
 126         PCICR  = (1<<PCIE2);
 127
 128
 129 }
 130
 131 void beacon_calibre_pos(void)
 132 {
 133         sensorboard.flags &= ~DO_CS;
 134
 135         /* init beacon pos */
 136         pwm_ng_set(BEACON_PWM, 100);
 137
 138         /* find rising edge of the mirror*/
 139         wait_ms(100);
 140         while (sensor_get(BEACON_POS_SENSOR));
 141         wait_ms(100);
 142         while (!sensor_get(BEACON_POS_SENSOR));
 143
 144         pwm_ng_set(BEACON_PWM, 0);
 145
 146
 147         beacon_reset_pos();
 148         pid_reset(&sensorboard.beacon.pid);
 149         encoders_spi_set_value_beacon(BEACON_ENCODER, BEACON_OFFSET_CALIBRE);
 150
 151         cs_set_consign(&sensorboard.beacon.cs, 0);
 152
 153         sensorboard.flags |= DO_CS;
 154 }
 155
 156 void beacon_start(void)
 157 {
 158         beacon_reset_pos();
 159         sensorboard.beacon.on = 1;
 160         cs_set_consign(&sensorboard.beacon.cs, 600);
 161 }
 162
 163 void beacon_stop(void)
 164 {
 165         sensorboard.beacon.on = 0;
 166         pwm_ng_set(BEACON_PWM, 0);
 167 }
 168
 169 void beacon_reset_pos(void)
 170 {
 171         pwm_ng_set(BEACON_PWM, 0);
 172         encoders_spi_set_value(BEACON_ENCODER, 0);
 173 }
 174
 175
 176
 177 int32_t encoders_spi_get_beacon_speed(void * dummy)
 178 {
 179         return beacon_speed;
 180 }
 181
 182
 183 //port K bit 2
 184 /* motor speed (top tour) */
 185 SIGNAL(SIG_PIN_CHANGE2)
 186 {
 187         uint8_t flags;
 188
 189         /* rising edge */
 190         if ( IR_SENSOR()) {
 191                 IRQ_LOCK(flags);
 192                 count = TCNT3;
 193                 rising = beacon_pos;
 194                 count_diff_rising = (count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER;
 195                 valid_beacon = 0;
 196                 IRQ_UNLOCK(flags);
 197
 198         }
 199         /* falling edge */
 200         else {
 201                 IRQ_LOCK(flags);
 202                 count = TCNT3;
 203                 falling = beacon_pos;
 204                 count_diff_falling = (count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER;
 205                 valid_beacon = 1;
 206                 IRQ_UNLOCK(flags);
 207         }
 208 }
 209
 210 void beacon_calc(void *dummy)
 211 {
 212         static uint8_t a=0;
 213         static int32_t local_rising, local_falling;
 214         static int32_t middle;
 215         static float size = 0;
 216         int32_t local_angle;
 217         int32_t local_dist;
 218
 219         int32_t local_count_diff_rising ;
 220         int32_t local_count_diff_falling ;
 221         int32_t local_beacon_coeff;
 222
 223         int32_t result_x=0;
 224         int32_t result_y=0;
 225         int32_t temp=0;
 226         int32_t edge=0;
 227         //int32_t total_size=0;
 228
 229         uint8_t flags;
 230         //uint8_t i;
 231         int8_t local_valid;
 232
 233         if(a)
 234                 LED4_ON();
 235         else
 236                 LED4_OFF();
 237
 238         a = !a;
 239
 240         if (falling == -1){
 241                 /* 0.5 second timeout */
 242                 if (invalid_count < 25)
 243                         invalid_count++;
 244                 else {
 245                         IRQ_LOCK(flags);
 246                         beacon.opponent_x = I2C_OPPONENT_NOT_THERE;
 247                         IRQ_UNLOCK(flags);
 248                 }
 249                 return;
 250         }
 251
 252         invalid_count = 0;
 253         IRQ_LOCK(flags);
 254         local_valid = valid_beacon;
 255         local_count_diff_rising  = count_diff_rising;
 256         local_count_diff_falling = count_diff_falling ;
 257         local_rising = rising;
 258         local_falling = falling;
 259         local_beacon_coeff = beacon_coeff;
 260         IRQ_UNLOCK(flags);
 261
 262         if (local_valid){
 263                 invalid_count = 0;
 264                 //BEACON_DEBUG("rising= %ld\t",local_rising);
 265                 //BEACON_DEBUG("falling= %ld\r\n",local_falling);
 266
 267                 /* recalculate number of pulse by adding the value of the counter, then put value back into motor's round range */
 268                 local_rising  = ((local_rising + (local_count_diff_rising * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR);
 269                 local_falling = ((local_falling + (local_count_diff_falling * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR);
 270
 271                 //BEACON_DEBUG("rising1= %ld\t",local_rising);
 272                 //BEACON_DEBUG("falling1= %ld\r\n",local_falling);
 273
 274                 //BEACON_DEBUG("count diff rising= %ld\t",local_count_diff_rising);
 275                 //BEACON_DEBUG("count diff falling= %ld\r\n",local_count_diff_falling);
 276
 277                 /* if around 360 deg, rising > falling, so invert both and recalculate size and middle */
 278                 if(local_falling < local_rising){
 279                         temp          = local_rising;
 280                         local_rising  = local_falling;
 281                         local_falling = temp;
 282                         size          = BEACON_STEP_TOUR - local_falling + local_rising;
 283                         middle        = (local_falling + ((int32_t)(size)/2) + BEACON_STEP_TOUR) %(BEACON_STEP_TOUR);
 284                         edge = local_falling;
 285                 }
 286                 /* else rising > falling */
 287                 else{
 288                         size   = local_falling - local_rising;
 289                         middle = local_rising + (size / 2);
 290                         edge   = local_rising;
 291                 }
 292
 293                 //for(i=BEACON_MAX_SAMPLE-1;i>0;i--){
 294                 //      beacon_sample_size[i] = beacon_sample_size[i-1];
 295                 //      total_size += beacon_sample_size[i];
 296                 //}
 297                 //beacon_sample_size[0] = size;
 298                 //total_size += size;
 299                 //total_size /= BEACON_MAX_SAMPLE;
 300
 301                 //BEACON_DEBUG("rising2= %ld\t",local_rising);
 302                 //BEACON_DEBUG("falling2= %ld\r\n",local_falling);
 303                 /*                      BEACON_DEBUG("size= %ld %ld\t",size, total_size); */
 304                 BEACON_DEBUG("size= %f\r\n",size);
 305                 //BEACON_DEBUG("middle= %ld\r\n",middle);
 306
 307                 local_angle = get_angle(middle,0);
 308                 BEACON_NOTICE("opponent angle= %ld\t",local_angle);
 309
 310                 local_dist = get_dist(size);
 311                 BEACON_NOTICE("opponent dist= %ld\r\n",local_dist);
 312
 313                 beacon_angle_dist_to_x_y(local_angle, local_dist, &result_x, &result_y);
 314
 315                 IRQ_LOCK(flags);
 316                 beacon.opponent_x = result_x;
 317                 beacon.opponent_y = result_y;
 318                 beacon.opponent_angle = local_angle;
 319                 beacon.opponent_dist = local_dist;
 320                 /* for I2C test */
 321                 //beacon.opponent_x = OPPONENT_POS_X;
 322                 //beacon.opponent_y = OPPONENT_POS_Y;
 323                 IRQ_UNLOCK(flags);
 324
 325                 BEACON_NOTICE("opponent x= %ld\t",beacon.opponent_x);
 326                 BEACON_NOTICE("opponent y= %ld\r\n\n",beacon.opponent_y);
 327         }
 328         else {
 329                 BEACON_NOTICE("non valid\r\n\n");
 330         }
 331
 332         falling = -1;
 333 }
 334
 335 static int32_t get_dist(float size)
 336 {
 337         int32_t dist=0;
 338         //int32_t alpha=0;
 339
 340         //alpha = (size*2*M_PI*COEFF_MULT2);
 341         //dist = ((2*BEACON_SIZE*BEACON_STEP_TOUR*COEFF_MULT2)/alpha)/2;
 342
 343         /* function found by measuring points up to 80cm */
 344         //dist = ((size - 600)*(size - 600)) / 2400 +28;
 345
 346         /* new function */
 347         /* dist = a0 + a1*x + a2*x² + a3x³ */
 348         dist = 1157.3 + (1.4146*size) + (-0.013508*size*size) + (0.00001488*size*size*size);
 349
 350         return dist;
 351
 352 }
 353
 354 static int32_t get_angle(int32_t middle, int32_t ref)
 355 {
 356         int32_t ret_angle;
 357
 358         ret_angle = (middle - ref) * 360 / BEACON_STEP_TOUR;
 359
 360         if(ret_angle > 360)
 361                 ret_angle -= 360;
 362
 363         return ret_angle;
 364 }
 365
 366 void beacon_angle_dist_to_x_y(int32_t angle, int32_t dist, int32_t *x, int32_t *y)
 367 {
 368         uint8_t flags;
 369
 370         int32_t local_x;
 371         int32_t local_y;
 372         int32_t x_opponent;
 373         int32_t y_opponent;
 374         int32_t local_robot_angle;
 375
 376         IRQ_LOCK(flags);
 377         local_x           = beacon.robot_x;
 378         local_y           = beacon.robot_y;
 379         local_robot_angle = beacon.robot_angle;
 380         IRQ_UNLOCK(flags);
 381
 382         if (local_robot_angle < 0)
 383                 local_robot_angle += 360;
 384
 385         x_opponent = cos((local_robot_angle + angle)* 2 * M_PI / 360)* dist;
 386         y_opponent = sin((local_robot_angle + angle)* 2 * M_PI / 360)* dist;
 387
 388         //BEACON_DEBUG("x_op= %ld\t",x_opponent);
 389         //BEACON_DEBUG("y_op= %ld\r\n",y_opponent);
 390         //BEACON_NOTICE("robot_x= %ld\t",local_x);
 391         //BEACON_NOTICE("robot_y= %ld\t",local_y);
 392         //BEACON_NOTICE("robot_angle= %ld\r\n",local_robot_angle);
 393
 394         *x = local_x + x_opponent;
 395         *y = local_y + y_opponent;
 396
 397 }