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
  70 static volatile uint8_t opp_age = 0;
  71 static volatile int16_t opp_a = I2C_OPPONENT_NOT_THERE;
  72 static volatile int16_t opp_d = I2C_OPPONENT_NOT_THERE;
  73
  74 #ifndef HOST_VERSION
  75 static void beacon_uart_cb(char c)
  76 {
  77         static uint8_t state;
  78         static uint16_t d, a, x, y;
  79
  80         /* init command */
  81         if ((c & 0x80) == 0)
  82                 state = INIT;
  83
  84         switch (state) {
  85         case INIT:
  86                 /* recv opp */
  87                 if (c == 0) {
  88                         state = OPP0;
  89                         d = 0;
  90                         a = 0;
  91                 }
  92                 /* recv opp */
  93                 else if (c == 0) {
  94                         state = STA0;
  95                         x = 0;
  96                         y = 0;
  97                 }
  98                 break;
  99         case OPP0:
 100                 d = ((uint16_t)c) & 0x7F;
 101                 break;
 102         case OPP1:
 103                 d |= (((uint16_t)c << 7) & 0x3F80);
 104                 break;
 105         case OPP2:
 106                 a = ((uint16_t)c) & 0x7F;
 107                 break;
 108         case OPP3:
 109                 a |= (((uint16_t)c << 7) & 0x3F80);
 110                 opp_a = a;
 111                 opp_d = d;
 112                 opp_age = 0;
 113                 break;
 114         case STA0:
 115                 x = ((uint16_t)c) & 0x7F;
 116                 break;
 117         case STA1:
 118                 x |= (((uint16_t)c << 7) & 0x3F80);
 119                 break;
 120         case STA2:
 121                 y = ((uint16_t)c) & 0x7F;
 122                 break;
 123         case STA3:
 124                 y |= (((uint16_t)c << 7) & 0x3F80);
 125                 beaconboard.posx = x;
 126                 beaconboard.posy = y;
 127                 break;
 128                 /* XXX STA4 with angle */
 129         default:
 130                 state = INIT;
 131                 break;
 132         }
 133 }
 134 #endif
 135
 136 static void beacon_event(void *dummy)
 137 {
 138 #ifdef HOST_VERSION
 139         uint8_t flags;
 140         int16_t oppx, oppy;
 141         double oppa, oppd;
 142
 143         IRQ_LOCK(flags);
 144         if (beaconboard.oppx == I2C_OPPONENT_NOT_THERE) {
 145                 IRQ_UNLOCK(flags);
 146                 return;
 147         }
 148         oppx = beaconboard.oppx;
 149         oppy = beaconboard.oppy;
 150         abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
 151         beaconboard.oppa = DEG(oppa);
 152         if (beaconboard.oppa < 0)
 153                 beaconboard.oppa += 360;
 154         beaconboard.oppd = oppd;
 155         IRQ_UNLOCK(flags);
 156 #else
 157         uint8_t flags;
 158         double fd, fa, fx, fy;
 159         int16_t id, ia, ix, iy;
 160
 161         /* if beacon is too old, remove it */
 162         IRQ_LOCK(flags);
 163         if (opp_age < 3)
 164                 opp_age ++;
 165         else
 166                 beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
 167
 168         ia = opp_a;
 169         id = opp_d;
 170         IRQ_UNLOCK(flags);
 171
 172         fa = ia;
 173         fa = RAD(fa);
 174         fd = id;
 175         rel_da_to_abs_xy(fd, fa, &fx, &fy);
 176
 177         ix = fx;
 178         iy = fy;
 179
 180         IRQ_LOCK(flags);
 181         beaconboard.oppx = ix;
 182         beaconboard.oppy = iy;
 183         beaconboard.oppa = ia;
 184         beaconboard.oppd = id;
 185         IRQ_UNLOCK(flags);
 186 #endif
 187 }
 188
 189 void beacon_init(void)
 190 {
 191 #ifndef HOST_VERSION
 192         uart_register_rx_event(BEACON_UART_NUM, beacon_uart_cb);
 193 #endif
 194         scheduler_add_periodical_event_priority(beacon_event, NULL,
 195                                                 100000L / SCHEDULER_UNIT,
 196                                                 BEACON_PRIO);
 197 }