projects/microb2010/mainboard/beacon.c

   1 /*
   2  *  Copyright Droids Corporation (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.h,v 1.7 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  */
  21
  22 #include <stdio.h>
  23 #include <string.h>
  24
  25 #include <aversive/pgmspace.h>
  26 #include <aversive/wait.h>
  27 #include <aversive/error.h>
  28
  29 #include <ax12.h>
  30 #include <uart.h>
  31 #include <pwm_ng.h>
  32 #include <i2c.h>
  33 #include <clock_time.h>
  34
  35 #include <scheduler.h>
  36 #include <pid.h>
  37 #include <quadramp.h>
  38 #include <control_system_manager.h>
  39 #include <trajectory_manager.h>
  40 #include <trajectory_manager_utils.h>
  41 #include <vect_base.h>
  42 #include <lines.h>
  43 #include <polygon.h>
  44 #include <obstacle_avoidance.h>
  45 #include <blocking_detection_manager.h>
  46 #include <robot_system.h>
  47 #include <position_manager.h>
  48
  49 #include <rdline.h>
  50 #include <parse.h>
  51 #include <parse_string.h>
  52 #include <parse_num.h>
  53
  54 #include "../common/i2c_commands.h"
  55 #include "main.h"
  56 #include "strat_utils.h"
  57
  58 #define BEACON_UART_NUM 2
  59
  60 #define INIT 0
  61 #define OPP0 1
  62 #define OPP1 2
  63 #define OPP2 3
  64 #define OPP3 4
  65 #define STA0 5
  66 #define STA1 6
  67 #define STA2 7
  68 #define STA3 8
  69 #define STA4 9
  70 #define STA5 10
  71
  72 #define BEACON_ANGLE_OFFSET 449
  73
  74 static volatile uint8_t opp_age = 0;
  75 static volatile int16_t opp_a = I2C_OPPONENT_NOT_THERE;
  76 static volatile int16_t opp_d = I2C_OPPONENT_NOT_THERE;
  77
  78 static volatile uint8_t pos_age = 0;
  79 static volatile int16_t pos_x = I2C_BEACON_NOT_FOUND;
  80 static volatile int16_t pos_y = I2C_BEACON_NOT_FOUND;
  81 static volatile int16_t pos_a = I2C_BEACON_NOT_FOUND;
  82
  83 #define BEACON_POS_OFFSET (-50.)
  84 int8_t beacon_get_pos_double(double *x, double *y, double *a_rad)
  85 {
  86         uint8_t flags;
  87         int16_t tmpx, tmpy, tmpa;
  88         double dtmpx, dtmpy, dtmpa;
  89
  90         IRQ_LOCK(flags);
  91         tmpx = beaconboard.posx;
  92         tmpy = beaconboard.posy;
  93         tmpa = beaconboard.posa;
  94         IRQ_UNLOCK(flags);
  95
  96         if (tmpx == I2C_BEACON_NOT_FOUND)
  97                 return -1;
  98
  99         dtmpx = tmpx;
 100         dtmpy = tmpy;
 101         dtmpa = RAD((double)tmpa / 10.);
 102
 103         dtmpx += cos(dtmpa) * BEACON_POS_OFFSET;
 104         dtmpx += sin(dtmpa) * BEACON_POS_OFFSET;
 105
 106         *x = dtmpx;
 107         *y = dtmpy;
 108         *a_rad = dtmpa;
 109         return 0;
 110 }
 111
 112 #ifndef HOST_VERSION
 113 static void beacon_uart_cb(char c)
 114 {
 115         static uint8_t state;
 116         static uint16_t tmp_opp_d, tmp_opp_a;
 117         static uint16_t x, y, a;
 118
 119         /* init command */
 120         if ((c & 0x80) == 0)
 121                 state = INIT;
 122
 123         switch (state) {
 124         case INIT:
 125                 /* recv opp */
 126                 if (c == 0) {
 127                         state = OPP0;
 128                         tmp_opp_d = 0;
 129                         tmp_opp_a = 0;
 130                 }
 131                 /* recv opp */
 132                 else if (c == 1) {
 133                         state = STA0;
 134                         x = 0;
 135                         y = 0;
 136                         a = 0;
 137                 }
 138                 break;
 139         case OPP0:
 140                 tmp_opp_d = ((uint16_t)c) & 0x7F;
 141                 state = OPP1;
 142                 break;
 143         case OPP1:
 144                 tmp_opp_d |= (((uint16_t)c << 7) & 0x3F80);
 145                 state = OPP2;
 146                 break;
 147         case OPP2:
 148                 tmp_opp_a = ((uint16_t)c) & 0x7F;
 149                 state = OPP3;
 150                 break;
 151         case OPP3:
 152                 tmp_opp_a |= (((uint16_t)c << 7) & 0x3F80);
 153                 opp_a = tmp_opp_a;
 154                 opp_d = tmp_opp_d;
 155                 opp_age = 0;
 156                 state = INIT;
 157                 break;
 158         case STA0:
 159                 x = ((uint16_t)c) & 0x7F;
 160                 state = STA1;
 161                 break;
 162         case STA1:
 163                 x |= (((uint16_t)c << 7) & 0x3F80);
 164                 state = STA2;
 165                 break;
 166         case STA2:
 167                 y = ((uint16_t)c) & 0x7F;
 168                 state = STA3;
 169                 break;
 170         case STA3:
 171                 y |= (((uint16_t)c << 7) & 0x3F80);
 172                 state = STA4;
 173                 break;
 174         case STA4:
 175                 a = ((uint16_t)c) & 0x7F;
 176                 state = STA5;
 177                 break;
 178         case STA5:
 179                 a |= (((uint16_t)c << 7) & 0x3F80);
 180                 pos_x = x;
 181                 pos_y = y;
 182                 pos_a = a;
 183                 pos_age = 0;
 184                 state = INIT;
 185                 break;
 186         default:
 187                 state = INIT;
 188                 break;
 189         }
 190 }
 191 #endif
 192
 193 static void beacon_opponent_event(void)
 194 {
 195 #ifdef HOST_VERSION
 196         uint8_t flags;
 197         int16_t oppx, oppy;
 198         double oppa, oppd;
 199
 200         IRQ_LOCK(flags);
 201         if (beaconboard.oppx == I2C_OPPONENT_NOT_THERE) {
 202                 IRQ_UNLOCK(flags);
 203                 return;
 204         }
 205         oppx = beaconboard.oppx;
 206         oppy = beaconboard.oppy;
 207         abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
 208         beaconboard.oppa = DEG(oppa);
 209         if (beaconboard.oppa < 0)
 210                 beaconboard.oppa += 360;
 211         beaconboard.oppd = oppd;
 212         IRQ_UNLOCK(flags);
 213 #else
 214         uint8_t flags;
 215         double fd, fa, fx, fy;
 216         int16_t id, ia, ix, iy;
 217
 218         /* if beacon is too old, remove it */
 219         IRQ_LOCK(flags);
 220         if (opp_age < 50)
 221                 opp_age ++;
 222         else {
 223                 beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
 224                 IRQ_UNLOCK(flags);
 225                 return;
 226         }
 227
 228         ia = opp_a;
 229         id = opp_d;
 230         IRQ_UNLOCK(flags);
 231
 232         ia = (ia + BEACON_ANGLE_OFFSET);
 233         if (ia > 3600)
 234                 ia -= 3600;
 235         fa = ia;
 236         fa = RAD(fa);
 237         fd = id;
 238         rel_da_to_abs_xy(fd, fa, &fx, &fy);
 239
 240         ix = fx;
 241         iy = fy;
 242
 243         IRQ_LOCK(flags);
 244         beaconboard.oppx = ix;
 245         beaconboard.oppy = iy;
 246         beaconboard.oppa = ia / 10;
 247         beaconboard.oppd = id;
 248         IRQ_UNLOCK(flags);
 249 #endif
 250 }
 251
 252 static void beacon_static_event(void)
 253 {
 254         uint8_t flags;
 255
 256         /* if beacon is too old, remove it */
 257         IRQ_LOCK(flags);
 258         if (pos_age < 3)
 259                 pos_age ++;
 260         else {
 261                 beaconboard.posx = I2C_BEACON_NOT_FOUND;
 262                 IRQ_UNLOCK(flags);
 263                 return;
 264         }
 265
 266         beaconboard.posx = pos_x;
 267         beaconboard.posy = pos_y;
 268         beaconboard.posa = pos_a;
 269         IRQ_UNLOCK(flags);
 270 }
 271
 272
 273 static void beacon_event(void *dummy)
 274 {
 275         beacon_opponent_event();
 276         beacon_static_event();
 277 }
 278
 279 void beacon_set_color(uint8_t color)
 280 {
 281         uart_send(BEACON_UART_NUM, color);
 282 }
 283
 284 void beacon_init(void)
 285 {
 286 #ifndef HOST_VERSION
 287         uart_register_rx_event(BEACON_UART_NUM, beacon_uart_cb);
 288 #endif
 289         scheduler_add_periodical_event_priority(beacon_event, NULL,
 290                                                 100000L / SCHEDULER_UNIT,
 291                                                 BEACON_PRIO);
 292 }