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 0
  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 static volatile uint8_t opp_age = 0;
  73 static volatile int16_t opp_a = I2C_OPPONENT_NOT_THERE;
  74 static volatile int16_t opp_d = I2C_OPPONENT_NOT_THERE;
  75
  76 static volatile uint8_t pos_age = 0;
  77 static volatile int16_t pos_x = I2C_BEACON_NOT_FOUND;
  78 static volatile int16_t pos_y = I2C_BEACON_NOT_FOUND;
  79 static volatile int16_t pos_a = I2C_BEACON_NOT_FOUND;
  80
  81 int8_t beacon_get_pos(int16_t *x, int16_t *y, double *a)
  82 {
  83         uint8_t flags;
  84         int16_t tmpx, tmpy, tmpa;
  85
  86         IRQ_LOCK(flags);
  87         tmpx = beaconboard.posx;
  88         tmpy = beaconboard.posy;
  89         tmpa = beaconboard.posa;
  90         IRQ_UNLOCK(flags);
  91
  92         if (tmpx == I2C_BEACON_NOT_FOUND)
  93                 return -1;
  94
  95         *x = tmpx;
  96         *y = tmpy;
  97         *a = ((double)tmpa / 10.);
  98         return 0;
  99 }
 100
 101 #ifndef HOST_VERSION
 102 static void beacon_uart_cb(char c)
 103 {
 104         static uint8_t state;
 105         static uint16_t tmp_opp_d, tmp_opp_a;
 106         static uint16_t x, y, a;
 107
 108         /* init command */
 109         if ((c & 0x80) == 0)
 110                 state = INIT;
 111
 112         switch (state) {
 113         case INIT:
 114                 /* recv opp */
 115                 if (c == 0) {
 116                         state = OPP0;
 117                         tmp_opp_d = 0;
 118                         tmp_opp_a = 0;
 119                 }
 120                 /* recv opp */
 121                 else if (c == 1) {
 122                         state = STA0;
 123                         x = 0;
 124                         y = 0;
 125                         a = 0;
 126                 }
 127                 break;
 128         case OPP0:
 129                 tmp_opp_d = ((uint16_t)c) & 0x7F;
 130                 state = OPP1;
 131                 break;
 132         case OPP1:
 133                 tmp_opp_d |= (((uint16_t)c << 7) & 0x3F80);
 134                 state = OPP2;
 135                 break;
 136         case OPP2:
 137                 tmp_opp_a = ((uint16_t)c) & 0x7F;
 138                 state = OPP3;
 139                 break;
 140         case OPP3:
 141                 tmp_opp_a |= (((uint16_t)c << 7) & 0x3F80);
 142                 opp_a = tmp_opp_a;
 143                 opp_d = tmp_opp_d;
 144                 opp_age = 0;
 145                 state = INIT;
 146                 break;
 147         case STA0:
 148                 x = ((uint16_t)c) & 0x7F;
 149                 state = STA1;
 150                 break;
 151         case STA1:
 152                 x |= (((uint16_t)c << 7) & 0x3F80);
 153                 state = STA2;
 154                 break;
 155         case STA2:
 156                 y = ((uint16_t)c) & 0x7F;
 157                 state = STA3;
 158                 break;
 159         case STA3:
 160                 y |= (((uint16_t)c << 7) & 0x3F80);
 161                 state = STA4;
 162                 break;
 163         case STA4:
 164                 a = ((uint16_t)c) & 0x7F;
 165                 state = STA5;
 166                 break;
 167         case STA5:
 168                 a |= (((uint16_t)c << 7) & 0x3F80);
 169                 pos_x = x;
 170                 pos_y = y;
 171                 pos_a = a;
 172                 pos_age = 0;
 173                 state = INIT;
 174                 break;
 175         default:
 176                 state = INIT;
 177                 break;
 178         }
 179 }
 180 #endif
 181
 182 static void beacon_opponent_event(void)
 183 {
 184 #ifdef HOST_VERSION
 185         uint8_t flags;
 186         int16_t oppx, oppy;
 187         double oppa, oppd;
 188
 189         IRQ_LOCK(flags);
 190         if (beaconboard.oppx == I2C_OPPONENT_NOT_THERE) {
 191                 IRQ_UNLOCK(flags);
 192                 return;
 193         }
 194         oppx = beaconboard.oppx;
 195         oppy = beaconboard.oppy;
 196         abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
 197         beaconboard.oppa = DEG(oppa);
 198         if (beaconboard.oppa < 0)
 199                 beaconboard.oppa += 360;
 200         beaconboard.oppd = oppd;
 201         IRQ_UNLOCK(flags);
 202 #else
 203         uint8_t flags;
 204         double fd, fa, fx, fy;
 205         int16_t id, ia, ix, iy;
 206
 207         /* if beacon is too old, remove it */
 208         IRQ_LOCK(flags);
 209         if (opp_age < 3)
 210                 opp_age ++;
 211         else {
 212                 beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
 213                 IRQ_UNLOCK(flags);
 214                 return;
 215         }
 216
 217         ia = opp_a;
 218         id = opp_d;
 219         IRQ_UNLOCK(flags);
 220
 221         fa = ia;
 222         fa = RAD(fa);
 223         fd = id;
 224         rel_da_to_abs_xy(fd, fa, &fx, &fy);
 225
 226         ix = fx;
 227         iy = fy;
 228
 229         IRQ_LOCK(flags);
 230         beaconboard.oppx = ix;
 231         beaconboard.oppy = iy;
 232         beaconboard.oppa = ia;
 233         beaconboard.oppd = id;
 234         IRQ_UNLOCK(flags);
 235 #endif
 236 }
 237
 238 static void beacon_static_event(void)
 239 {
 240         uint8_t flags;
 241
 242         /* if beacon is too old, remove it */
 243         IRQ_LOCK(flags);
 244         if (pos_age < 3)
 245                 pos_age ++;
 246         else {
 247                 beaconboard.posx = I2C_BEACON_NOT_FOUND;
 248                 IRQ_UNLOCK(flags);
 249                 return;
 250         }
 251
 252         beaconboard.posx = pos_x;
 253         beaconboard.posy = pos_y;
 254         beaconboard.posa = pos_a;
 255         IRQ_UNLOCK(flags);
 256 }
 257
 258
 259 static void beacon_event(void *dummy)
 260 {
 261         beacon_opponent_event();
 262         beacon_static_event();
 263 }
 264
 265 void beacon_set_color(uint8_t color)
 266 {
 267         uart_send(BEACON_UART_NUM, color);
 268 }
 269
 270 void beacon_init(void)
 271 {
 272 #ifndef HOST_VERSION
 273         uart_register_rx_event(BEACON_UART_NUM, beacon_uart_cb);
 274 #endif
 275         scheduler_add_periodical_event_priority(beacon_event, NULL,
 276                                                 100000L / SCHEDULER_UNIT,
 277                                                 BEACON_PRIO);
 278 }