projects/microb2010/mainboard/robotsim.c

   1 /*
   2  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
   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: main.c,v 1.9.4.5 2007-06-01 09:37:22 zer0 Exp $
  19  *
  20  */
  21
  22 #include <stdio.h>
  23 #include <string.h>
  24 #include <stdint.h>
  25 #include <unistd.h>
  26 #include <sys/stat.h>
  27 #include <sys/types.h>
  28 #include <fcntl.h>
  29 #include <unistd.h>
  30
  31 #include <aversive.h>
  32 #include <aversive/error.h>
  33
  34 #include <timer.h>
  35 #include <scheduler.h>
  36 #include <time.h>
  37
  38 #include <ax12.h>
  39 #include <pwm_ng.h>
  40 #include <pid.h>
  41 #include <quadramp.h>
  42 #include <control_system_manager.h>
  43 #include <trajectory_manager.h>
  44 #include <blocking_detection_manager.h>
  45 #include <robot_system.h>
  46 #include <position_manager.h>
  47 #include <trajectory_manager_utils.h>
  48
  49 #include <parse.h>
  50 #include <rdline.h>
  51
  52 #include "../common/i2c_commands.h"
  53 #include "strat.h"
  54 #include "strat_utils.h"
  55 #include "main.h"
  56
  57 uint8_t robotsim_blocking = 0;
  58
  59 static int32_t l_pwm, r_pwm;
  60 static int32_t l_enc, r_enc;
  61
  62 static int fdr, fdw;
  63 /*
  64  * Debug with GDB:
  65  *
  66  * (gdb) handle SIGUSR1 pass
  67  * Signal        Stop   Print   Pass to program Description
  68  * SIGUSR1       Yes    Yes     Yes             User defined signal 1
  69  * (gdb) handle SIGUSR2 pass
  70  * Signal        Stop   Print   Pass to program Description
  71  * SIGUSR2       Yes    Yes     Yes             User defined signal 2
  72  * (gdb) handle SIGUSR1 noprint
  73  * Signal        Stop   Print   Pass to program Description
  74  * SIGUSR1       No     No      Yes             User defined signal 1
  75  * (gdb) handle SIGUSR2 noprint
  76  */
  77
  78 /* */
  79 #define FILTER  98
  80 #define FILTER2 (100-FILTER)
  81 #define SHIFT   4
  82
  83 void robotsim_dump(void)
  84 {
  85         char buf[BUFSIZ];
  86         int len;
  87         int16_t x, y, a;
  88
  89         x = position_get_x_s16(&mainboard.pos);
  90         y = position_get_y_s16(&mainboard.pos);
  91         a = position_get_a_deg_s16(&mainboard.pos);
  92 /*      y = COLOR_Y(y); */
  93 /*      a = COLOR_A(a); */
  94
  95         len = snprintf(buf, sizeof(buf), "pos=%d,%d,%d\n",
  96                        x, y, a);
  97         hostsim_lock();
  98         write(fdw, buf, len);
  99         hostsim_unlock();
 100 }
 101
 102 static int8_t
 103 robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd)
 104 {
 105         char buf[BUFSIZ];
 106         int len;
 107
 108         ballboard.mode = cmd->mode;
 109         len = snprintf(buf, sizeof(buf), "ballboard=%d\n", cmd->mode);
 110         if (cmd->mode == I2C_BALLBOARD_MODE_EJECT)
 111                 ballboard.ball_count = 0;
 112         hostsim_lock();
 113         write(fdw, buf, len);
 114         hostsim_unlock();
 115         return 0;
 116 }
 117
 118 static int8_t
 119 robotsim_i2c_cobboard_set_mode(struct i2c_cmd_cobboard_set_mode *cmd)
 120 {
 121         if (cmd->mode == I2C_COBBOARD_MODE_EJECT)
 122                 cobboard.cob_count = 0;
 123         return 0;
 124 }
 125
 126 int8_t
 127 robotsim_i2c_cobboard_set_spickles(uint8_t side, uint8_t flags)
 128 {
 129         char buf[BUFSIZ];
 130         int len;
 131
 132         if (side == I2C_LEFT_SIDE) {
 133                 if (cobboard.lspickle == flags)
 134                         return 0;
 135                 else
 136                         cobboard.lspickle = flags;
 137         }
 138         if (side == I2C_RIGHT_SIDE) {
 139                 if (cobboard.rspickle == flags)
 140                         return 0;
 141                 else
 142                         cobboard.rspickle = flags;
 143         }
 144
 145         len = snprintf(buf, sizeof(buf), "cobboard=%d,%d\n", side, flags);
 146         hostsim_lock();
 147         write(fdw, buf, len);
 148         hostsim_unlock();
 149         return 0;
 150 }
 151
 152 static int8_t
 153 robotsim_i2c_ballboard(uint8_t addr, uint8_t *buf, uint8_t size)
 154 {
 155         void *void_cmd = buf;
 156
 157         switch (buf[0]) {
 158         case I2C_CMD_BALLBOARD_SET_MODE:
 159                 {
 160                         struct i2c_cmd_ballboard_set_mode *cmd = void_cmd;
 161                         robotsim_i2c_ballboard_set_mode(cmd);
 162                         break;
 163                 }
 164
 165         default:
 166                 break;
 167         }
 168         return 0;
 169 }
 170
 171 static int8_t
 172 robotsim_i2c_cobboard(uint8_t addr, uint8_t *buf, uint8_t size)
 173 {
 174         void *void_cmd = buf;
 175
 176         switch (buf[0]) {
 177
 178         case I2C_CMD_COBBOARD_SET_MODE:
 179                 {
 180                         struct i2c_cmd_cobboard_set_mode *cmd = void_cmd;
 181                         robotsim_i2c_cobboard_set_mode(cmd);
 182                         break;
 183                 }
 184
 185         default:
 186                 break;
 187         }
 188         return 0;
 189 }
 190
 191 int8_t
 192 robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size)
 193 {
 194         if (addr == I2C_BALLBOARD_ADDR)
 195                 return robotsim_i2c_ballboard(addr, buf, size);
 196         else if (addr == I2C_COBBOARD_ADDR)
 197                 return robotsim_i2c_cobboard(addr, buf, size);
 198         return 0;
 199 }
 200
 201 static void beacon_update(void)
 202 {
 203        uint8_t flags;
 204        int16_t oppx, oppy;
 205        double oppa, oppd;
 206
 207        IRQ_LOCK(flags);
 208        if (ballboard.opponent_x == I2C_OPPONENT_NOT_THERE) {
 209                IRQ_UNLOCK(flags);
 210                return;
 211        }
 212        oppx = ballboard.opponent_x;
 213        oppy = ballboard.opponent_y;
 214        abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
 215        ballboard.opponent_a = DEG(oppa);
 216        if (ballboard.opponent_a < 0)
 217                ballboard.opponent_a += 360;
 218        ballboard.opponent_d = oppd;
 219        IRQ_UNLOCK(flags);
 220 }
 221
 222 /* must be called periodically */
 223 void robotsim_update(void)
 224 {
 225         static int32_t l_pwm_shift[SHIFT];
 226         static int32_t r_pwm_shift[SHIFT];
 227         static int32_t l_speed, r_speed;
 228         static unsigned i = 0;
 229         static unsigned cpt = 0;
 230
 231         uint8_t flags;
 232         int32_t local_l_pwm, local_r_pwm;
 233         double x, y, a, a2, d;
 234         char cmd[BUFSIZ];
 235         int n, pertl = 0, pertr = 0;
 236
 237         /* corners of the robot */
 238         double xfl, yfl; /* front left */
 239         double xrl, yrl; /* rear left */
 240         double xrr, yrr; /* rear right */
 241         double xfr, yfr; /* front right */
 242
 243         int oppx, oppy;
 244         double oppa, oppd;
 245
 246         beacon_update();
 247
 248         /* time shift the command */
 249         l_pwm_shift[i] = l_pwm;
 250         r_pwm_shift[i] = r_pwm;
 251         i ++;
 252         i %= SHIFT;
 253         local_l_pwm = l_pwm_shift[i];
 254         local_r_pwm = r_pwm_shift[i];
 255
 256         /* read command */
 257         if (((cpt ++) & 0x7) == 0) {
 258                 n = read(fdr, &cmd, BUFSIZ - 1);
 259                 if (n < 1)
 260                         n = 0;
 261                 cmd[n] = 0;
 262         }
 263
 264         /* perturbation */
 265         if (cmd[0] == 'l')
 266                 pertl = 1;
 267         else if (cmd[0] == 'r')
 268                 pertr = 1;
 269         else if (cmd[0] == 'b')
 270                 robotsim_blocking = 1;
 271         if (cmd[0] == 'o') {
 272                 if (sscanf(cmd, "opp %d %d", &oppx, &oppy) == 2) {
 273                         abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
 274                         IRQ_LOCK(flags);
 275                         ballboard.opponent_x = oppx;
 276                         ballboard.opponent_y = oppy;
 277                         ballboard.opponent_a = DEG(oppa);
 278                         if (ballboard.opponent_a < 0)
 279                                 ballboard.opponent_a += 360;
 280                         ballboard.opponent_d = oppd;
 281                         IRQ_UNLOCK(flags);
 282                 }
 283         }
 284
 285         x = position_get_x_double(&mainboard.pos);
 286         y = position_get_y_double(&mainboard.pos);
 287         a = position_get_a_rad_double(&mainboard.pos);
 288
 289         l_speed = ((l_speed * FILTER) / 100) +
 290                 ((local_l_pwm * 1000 * FILTER2)/1000);
 291         r_speed = ((r_speed * FILTER) / 100) +
 292                 ((local_r_pwm * 1000 * FILTER2)/1000);
 293
 294         /* basic collision detection */
 295         a2 = atan2(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_REAR);
 296         d = norm(ROBOT_WIDTH/2, ROBOT_HALF_LENGTH_REAR);
 297
 298         xfl = x + cos(a+a2) * d;
 299         yfl = y + sin(a+a2) * d;
 300         if (!is_in_area(xfl, yfl, 0) && l_speed > 0)
 301                 l_speed = 0;
 302
 303         xrl = x + cos(a+M_PI-a2) * d;
 304         yrl = y + sin(a+M_PI-a2) * d;
 305         if (!is_in_area(xrl, yrl, 0) && l_speed < 0)
 306                 l_speed = 0;
 307
 308         xrr = x + cos(a+M_PI+a2) * d;
 309         yrr = y + sin(a+M_PI+a2) * d;
 310         if (!is_in_area(xrr, yrr, 0) && r_speed < 0)
 311                 r_speed = 0;
 312
 313         xfr = x + cos(a-a2) * d;
 314         yfr = y + sin(a-a2) * d;
 315         if (!is_in_area(xfr, yfr, 0) && r_speed > 0)
 316                 r_speed = 0;
 317
 318         if (pertl)
 319                 l_enc += 5000; /* push 1 cm */
 320         if (pertr)
 321                 r_enc += 5000; /* push 1 cm */
 322
 323         /* XXX should lock */
 324         l_enc += (l_speed / 1000);
 325         r_enc += (r_speed / 1000);
 326 }
 327
 328 void robotsim_pwm(void *arg, int32_t val)
 329 {
 330         //      printf("%p, %d\n", arg, val);
 331         if (arg == LEFT_PWM)
 332                 l_pwm = (val / 1.55);
 333         else if (arg == RIGHT_PWM)
 334                 r_pwm = (val / 1.55);
 335 }
 336
 337 int32_t robotsim_encoder_get(void *arg)
 338 {
 339         if (arg == LEFT_ENCODER)
 340                 return l_enc;
 341         else if (arg == RIGHT_ENCODER)
 342                 return r_enc;
 343         return 0;
 344 }
 345
 346 int robotsim_init(void)
 347 {
 348         mkfifo("/tmp/.robot_sim2dis", 0600);
 349         mkfifo("/tmp/.robot_dis2sim", 0600);
 350         fdw = open("/tmp/.robot_sim2dis", O_WRONLY, 0);
 351         if (fdw < 0)
 352                 return -1;
 353         fdr = open("/tmp/.robot_dis2sim", O_RDONLY | O_NONBLOCK, 0);
 354         if (fdr < 0) {
 355                 close(fdw);
 356                 return -1;
 357         }
 358         return 0;
 359 }