projects/microb2010/mainboard/strat.c

   1 /*
   2  *  Copyright Droids, 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.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23 #include <stdio.h>
  24 #include <stdlib.h>
  25 #include <string.h>
  26 #include <math.h>
  27
  28 #include <aversive/pgmspace.h>
  29 #include <aversive/queue.h>
  30 #include <aversive/wait.h>
  31 #include <aversive/error.h>
  32
  33 #include <ax12.h>
  34 #include <uart.h>
  35 #include <pwm_ng.h>
  36 #include <clock_time.h>
  37 #include <spi.h>
  38
  39 #include <pid.h>
  40 #include <quadramp.h>
  41 #include <control_system_manager.h>
  42 #include <trajectory_manager.h>
  43 #include <vect_base.h>
  44 #include <lines.h>
  45 #include <polygon.h>
  46 #include <obstacle_avoidance.h>
  47 #include <blocking_detection_manager.h>
  48 #include <robot_system.h>
  49 #include <position_manager.h>
  50
  51 #include <diagnostic.h>
  52
  53 #include <rdline.h>
  54 #include <parse.h>
  55
  56 #include "../common/i2c_commands.h"
  57 #include "i2c_protocol.h"
  58 #include "main.h"
  59 #include "strat.h"
  60 #include "strat_base.h"
  61 #include "strat_utils.h"
  62 #include "sensor.h"
  63 #include "actuator.h"
  64
  65 #define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */
  66 #define COL_SCAN_PRE_MARGIN 250
  67
  68 struct strat_infos strat_infos = {
  69         /* conf */
  70         .conf = {
  71                 .flags = 0,
  72         },
  73         /* status */
  74         .status = {
  75                 .flags = 0,
  76         },
  77 };
  78
  79 /*************************************************************/
  80
  81 /*                  INIT                                     */
  82
  83 /*************************************************************/
  84
  85 void strat_set_bounding_box(void)
  86 {
  87         if (get_color() == I2C_COLOR_YELLOW) {
  88                 strat_infos.area_bbox.x1 = 300;
  89                 strat_infos.area_bbox.y1 = 200;
  90                 strat_infos.area_bbox.x2 = 2720; /* needed for c1 */
  91                 strat_infos.area_bbox.y2 = 1800;
  92         }
  93         else {
  94                 strat_infos.area_bbox.x1 = 200;
  95                 strat_infos.area_bbox.y1 = 300;
  96                 strat_infos.area_bbox.x2 = 2720; /* needed for c1 */
  97                 strat_infos.area_bbox.y2 = 1900;
  98         }
  99
 100         polygon_set_boundingbox(strat_infos.area_bbox.x1,
 101                                 strat_infos.area_bbox.y1,
 102                                 strat_infos.area_bbox.x2,
 103                                 strat_infos.area_bbox.y2);
 104 }
 105
 106 /* called before each strat, and before the start switch */
 107 void strat_preinit(void)
 108 {
 109         time_reset();
 110         interrupt_traj_reset();
 111         mainboard.flags =  DO_ENCODERS | DO_CS | DO_RS |
 112                 DO_POS | DO_BD | DO_POWER;
 113
 114         i2c_cobboard_mode_init();
 115         strat_dump_conf();
 116         strat_dump_infos(__FUNCTION__);
 117 }
 118
 119 void strat_dump_conf(void)
 120 {
 121         if (!strat_infos.dump_enabled)
 122                 return;
 123
 124         printf_P(PSTR("-- conf --\r\n"));
 125
 126 }
 127
 128 /* display current information about the state of the game */
 129 void strat_dump_infos(const char *caller)
 130 {
 131         if (!strat_infos.dump_enabled)
 132                 return;
 133
 134         printf_P(PSTR("%s() dump strat infos:\r\n"), caller);
 135 }
 136
 137 /* init current area state before a match. Dump update user conf
 138  * here */
 139 void strat_reset_infos(void)
 140 {
 141 }
 142
 143 /* call it just before launching the strat */
 144 void strat_init(void)
 145 {
 146         /* XXX init rollers, .. */
 147         strat_reset_infos();
 148
 149         /* we consider that the color is correctly set */
 150
 151         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 152         time_reset();
 153         interrupt_traj_reset();
 154
 155         /* used in strat_base for END_TIMER */
 156         mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
 157                 DO_POS | DO_BD | DO_TIMER | DO_POWER;
 158 }
 159
 160
 161 /* call it after each strat */
 162 void strat_exit(void)
 163 {
 164         uint8_t flags;
 165
 166         mainboard.flags &= ~(DO_TIMER);
 167         strat_hardstop();
 168         time_reset();
 169         wait_ms(1000);
 170         IRQ_LOCK(flags);
 171         mainboard.flags &= ~(DO_CS);
 172         pwm_ng_set(LEFT_PWM, 0);
 173         pwm_ng_set(RIGHT_PWM, 0);
 174         IRQ_UNLOCK(flags);
 175 }
 176
 177 /* called periodically */
 178 void strat_event(void *dummy)
 179 {
 180 #if 0
 181         /* pack or deploy spickle */
 182         if (strat_infos.status.flags & STRAT_STATUS_LHARVEST) {
 183                 if (sensor_get(S_LCOB_PRESENT)) {
 184                         if (sensor_get(S_LCOB_WHITE))
 185                                 i2c_ballboard_set_mode();
 186                         else
 187                                 ;
 188                 }
 189         }
 190 #endif
 191         /* limit speed when opponent is close */
 192         strat_limit_speed();
 193 }
 194
 195 static uint8_t strat_beginning(void)
 196 {
 197         return END_TRAJ;
 198 }
 199
 200 /* dump state (every 5 s max) */
 201 #define DUMP_RATE_LIMIT(dump, last_print)               \
 202         do {                                            \
 203                 if (time_get_s() - last_print > 5) {    \
 204                         dump();                         \
 205                         last_print = time_get_s();      \
 206                 }                                       \
 207         } while (0)
 208
 209
 210 uint8_t strat_main(void)
 211 {
 212         uint8_t err;
 213
 214         /* */
 215         err = strat_beginning();
 216
 217         return err;
 218 }