projects/microb2010/tests/test_board2008/strat_base.c

   1 /*
   2  *  Copyright Droids Corporation, Microb Technology (2009)
   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_base.c,v 1.6 2009-05-02 10:08:09 zer0 Exp $
  19  *
  20  */
  21
  22 #include <stdio.h>
  23 #include <stdlib.h>
  24 #include <string.h>
  25 #include <math.h>
  26
  27 #include <aversive/pgmspace.h>
  28 #include <aversive/wait.h>
  29 #include <aversive/error.h>
  30
  31 #include <uart.h>
  32 #include <pwm_ng.h>
  33 #include <clock_time.h>
  34
  35 #include <pid.h>
  36 #include <quadramp.h>
  37 #include <control_system_manager.h>
  38 #include <trajectory_manager.h>
  39 #include <blocking_detection_manager.h>
  40 #include <robot_system.h>
  41 #include <position_manager.h>
  42
  43 #include <rdline.h>
  44 #include <parse.h>
  45
  46 #include "main.h"
  47 #include "cmdline.h"
  48 #include "strat_base.h"
  49 #include "sensor.h"
  50
  51 /* true if we want to interrupt a trajectory */
  52 static uint8_t traj_intr=0;
  53
  54 /* filled when a END_OBSTACLE is returned */
  55 struct opponent_obstacle opponent_obstacle;
  56
  57 /* asked speed */
  58 static volatile int16_t strat_speed_a = SPEED_DIST_FAST;
  59 static volatile int16_t strat_speed_d = SPEED_ANGLE_FAST;
  60 static volatile uint16_t strat_limit_speed_a = 0; /* no limit */
  61 static volatile uint16_t strat_limit_speed_d = 0;
  62
  63 static volatile uint8_t strat_limit_speed_enabled = 1;
  64
  65
  66 /* Strings that match the end traj cause */
  67 /* /!\ keep it sync with stat_base.h */
  68 const char *err_tab []= {
  69         "END_TRAJ",
  70         "END_BLOCKING",
  71         "END_NEAR",
  72         "END_OBSTACLE",
  73         "END_ERROR",
  74         "END_INTR",
  75         "END_TIMER",
  76         "END_RESERVED",
  77 };
  78
  79 /* return string from end traj type num */
  80 const char *get_err(uint8_t err)
  81 {
  82         uint8_t i;
  83         if (err == 0)
  84                 return "SUCCESS";
  85         for (i=0 ; i<8; i++) {
  86                 if (err & (1 <<i))
  87                         return err_tab[i];
  88         }
  89         return "END_UNKNOWN";
  90 }
  91
  92 void strat_hardstop(void)
  93 {
  94         trajectory_hardstop(&mainboard.traj);
  95         pid_reset(&mainboard.angle.pid);
  96         pid_reset(&mainboard.distance.pid);
  97         bd_reset(&mainboard.angle.bd);
  98         bd_reset(&mainboard.distance.bd);
  99
 100         while ((ABS(mainboard.speed_d) > 200) ||
 101                (ABS(mainboard.speed_a) > 200))
 102
 103         trajectory_hardstop(&mainboard.traj);
 104         pid_reset(&mainboard.angle.pid);
 105         pid_reset(&mainboard.distance.pid);
 106         bd_reset(&mainboard.angle.bd);
 107         bd_reset(&mainboard.distance.bd);
 108 }
 109
 110 /* reset position */
 111 void strat_reset_pos(int16_t x, int16_t y, int16_t a)
 112 {
 113         int16_t posx = position_get_x_s16(&mainboard.pos);
 114         int16_t posy = position_get_y_s16(&mainboard.pos);
 115         int16_t posa = position_get_a_deg_s16(&mainboard.pos);
 116
 117         if (x == DO_NOT_SET_POS)
 118                 x = posx;
 119         if (y == DO_NOT_SET_POS)
 120                 y = posy;
 121         if (a == DO_NOT_SET_POS)
 122                 a = posa;
 123
 124         DEBUG(E_USER_STRAT, "reset pos (%s%s%s)",
 125               x == DO_NOT_SET_POS ? "" : "x",
 126               y == DO_NOT_SET_POS ? "" : "y",
 127               a == DO_NOT_SET_POS ? "" : "a");
 128         position_set(&mainboard.pos, x, y, a);
 129         DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__);
 130 }
 131
 132 /*
 133  * decrease gain on angle PID, and go forward until we reach the
 134  * border.
 135  */
 136 uint8_t strat_calib(int16_t dist, uint8_t flags)
 137 {
 138         int32_t p = pid_get_gain_P(&mainboard.angle.pid);
 139         int32_t i = pid_get_gain_I(&mainboard.angle.pid);
 140         int32_t d = pid_get_gain_D(&mainboard.angle.pid);
 141         uint8_t err;
 142
 143         pid_set_gains(&mainboard.angle.pid, 150, 0, 2000);
 144         trajectory_d_rel(&mainboard.traj, dist);
 145         err = wait_traj_end(flags);
 146         pid_set_gains(&mainboard.angle.pid, p, i, d);
 147         return err;
 148 }
 149
 150 static void strat_update_traj_speed(void)
 151 {
 152         uint16_t d, a;
 153
 154         d = strat_speed_d;
 155         if (strat_limit_speed_d && d > strat_limit_speed_d)
 156                 d = strat_limit_speed_d;
 157         a = strat_speed_a;
 158         if (strat_limit_speed_a && a > strat_limit_speed_a)
 159                 a = strat_limit_speed_a;
 160
 161         trajectory_set_speed(&mainboard.traj, d, a);
 162 }
 163
 164 void strat_set_speed(uint16_t d, uint16_t a)
 165 {
 166         uint8_t flags;
 167         IRQ_LOCK(flags);
 168         strat_speed_d = d;
 169         strat_speed_a = a;
 170         strat_update_traj_speed();
 171         IRQ_UNLOCK(flags);
 172 }
 173
 174 void strat_get_speed(uint16_t *d, uint16_t *a)
 175 {
 176         uint8_t flags;
 177         IRQ_LOCK(flags);
 178         *d = strat_speed_d;
 179         *a = strat_speed_a;
 180         IRQ_UNLOCK(flags);
 181 }
 182
 183
 184 void interrupt_traj(void)
 185 {
 186         traj_intr = 1;
 187 }
 188
 189 void interrupt_traj_reset(void)
 190 {
 191         traj_intr = 0;
 192 }
 193
 194 uint8_t test_traj_end(uint8_t why)
 195 {
 196         uint16_t cur_timer;
 197 /*      point_t robot_pt; */
 198
 199 /*      robot_pt.x = position_get_x_s16(&mainboard.pos); */
 200 /*      robot_pt.y = position_get_y_s16(&mainboard.pos); */
 201
 202         /* trigger an event at 3 sec before the end of the match if we
 203          * have balls in the barrel */
 204         cur_timer = time_get_s();
 205
 206         if ((mainboard.flags & DO_TIMER) && (why & END_TIMER)) {
 207                 /* end of match */
 208                 if (cur_timer >= MATCH_TIME)
 209                         return END_TIMER;
 210         }
 211
 212         if ((why & END_INTR) && traj_intr) {
 213                 interrupt_traj_reset();
 214                 return END_INTR;
 215         }
 216
 217         if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj))
 218                 return END_TRAJ;
 219
 220         /* we are near the destination point (depends on current
 221          * speed) AND the robot is in the area bounding box. */
 222         if (why & END_NEAR) {
 223                 int16_t d_near = 100;
 224
 225                 if (mainboard.speed_d > 2000)
 226                         d_near = 150;
 227
 228 /*              if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) && */
 229 /*                  is_in_boundingbox(&robot_pt)) */
 230 /*                      return END_NEAR; */
 231         }
 232
 233         if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) {
 234                 strat_hardstop();
 235                 return END_BLOCKING;
 236         }
 237
 238         if ((why & END_BLOCKING) && bd_get(&mainboard.distance.bd)) {
 239                 strat_hardstop();
 240                 return END_BLOCKING;
 241         }
 242
 243 /*      if ((why & END_OBSTACLE) && strat_obstacle()) { */
 244 /*              strat_hardstop(); */
 245 /*              return END_OBSTACLE; */
 246 /*      } */
 247
 248         return 0;
 249 }
 250
 251 uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line)
 252 {
 253         uint8_t ret = 0;
 254 /*      int16_t opp_x, opp_y, opp_d, opp_a; */
 255
 256         while (ret == 0)
 257                 ret = test_traj_end(why);
 258
 259 /*      if (ret == END_OBSTACLE) { */
 260 /*              if (get_opponent_xyda(&opp_x, &opp_y, */
 261 /*                                    &opp_d, &opp_a) == 0) */
 262 /*                      DEBUG(E_USER_STRAT, "Got %s at line %d" */
 263 /*                            " xy=(%d,%d) da=(%d,%d)", get_err(ret), */
 264 /*                            line, opp_x, opp_y, opp_d, opp_a); */
 265 /*      } */
 266 /*      else { */
 267 /*              DEBUG(E_USER_STRAT, "Got %s at line %d",  */
 268 /*                    get_err(ret), line); */
 269 /*      } */
 270         return ret;
 271 }