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