projects/microb2010/mainboard/strat_corn.c

   1 /*
   2  *  Copyright Droids, Microb Technology (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.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23
  24 #include <stdio.h>
  25 #include <stdlib.h>
  26 #include <stdint.h>
  27 #include <math.h>
  28
  29 #include <aversive.h>
  30 #include <aversive/error.h>
  31 #include <aversive/pgmspace.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 <trajectory_manager_utils.h>
  44 #include <trajectory_manager_core.h>
  45 #include <vect_base.h>
  46 #include <lines.h>
  47 #include <polygon.h>
  48 #include <obstacle_avoidance.h>
  49 #include <blocking_detection_manager.h>
  50 #include <robot_system.h>
  51 #include <position_manager.h>
  52
  53 #include <diagnostic.h>
  54
  55 #include <rdline.h>
  56 #include <parse.h>
  57
  58 #include "../common/i2c_commands.h"
  59 #include "i2c_protocol.h"
  60 #include "main.h"
  61 #include "strat.h"
  62 #include "strat_db.h"
  63 #include "strat_base.h"
  64 #include "strat_corn.h"
  65 #include "strat_utils.h"
  66 #include "sensor.h"
  67 #include "actuator.h"
  68
  69 /* return 1 if there is a corn near, and fill the index ptr */
  70 int8_t corn_is_near(uint8_t *corn_idx, uint8_t side)
  71 {
  72         /* XXX to be checked */
  73 #define SENSOR_CORN_DIST  225
  74 #define SENSOR_CORN_ANGLE 90
  75         double x = position_get_x_double(&mainboard.pos);
  76         double y = position_get_y_double(&mainboard.pos);
  77         double a_rad = position_get_a_rad_double(&mainboard.pos);
  78         double x_corn, y_corn;
  79         int16_t x_corn_int, y_corn_int;
  80         struct waypoint_db *wp;
  81
  82         if (side == I2C_LEFT_SIDE) {
  83                 x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
  84                 y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
  85         }
  86         else {
  87                 x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
  88                 y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
  89         }
  90         x_corn_int = x_corn;
  91         y_corn_int = y_corn;
  92
  93         wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
  94         if (wp == NULL)
  95                 return 0;
  96         *corn_idx = wp->corn.idx;
  97         return 1;
  98 }
  99
 100 /*
 101  * - send the correct commands to the spickles
 102  * - return 1 if we need to stop (cobboard is stucked)
 103 */
 104 static uint8_t handle_spickles(void)
 105 {
 106         return 0;
 107 #if 0
 108         int8_t corn_idx;
 109
 110         if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE))
 111                 i2c_cobboard_mode_deploy(I2C_LEFT_SIDE);
 112         else {
 113                 if (corn_table[corn_idx] == TYPE_WHITE_CORN)
 114                         i2c_cobboard_mode_harvest(I2C_LEFT_SIDE);
 115                 else
 116                         i2c_cobboard_mode_pack(I2C_LEFT_SIDE);
 117         }
 118 /*      printf("%d %d\n", corn_idx, corn_table[corn_idx]); */
 119 /*      time_wait_ms(100); */
 120
 121         if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE))
 122                 i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE);
 123         else {
 124                 if (corn_table[corn_idx] == TYPE_WHITE_CORN)
 125                         i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE);
 126                 else
 127                         i2c_cobboard_mode_pack(I2C_RIGHT_SIDE);
 128         }
 129
 130         return 0;
 131 #endif
 132 }
 133
 134 uint8_t line2line(uint8_t num1, uint8_t dir1,
 135                   uint8_t num2, uint8_t dir2)
 136 {
 137         double line1_a_rad, line1_a_deg, line2_a_rad;
 138         double diff_a_deg, diff_a_deg_abs, beta_deg;
 139         double radius;
 140         struct line_2pts l1, l2;
 141         line_t ll1, ll2;
 142         point_t p;
 143         uint8_t err;
 144         uint16_t a_speed, d_speed;
 145         int8_t ret;
 146
 147         /* convert to 2 points */
 148         num2line(&l1, dir1, num1);
 149         num2line(&l2, dir2, num2);
 150
 151         DEBUG(E_USER_STRAT, "line1: (%2.2f, %2.2f) -> (%2.2f, %2.2f)",
 152               l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
 153         DEBUG(E_USER_STRAT, "line2: (%2.2f, %2.2f) -> (%2.2f, %2.2f)",
 154               l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
 155
 156         /* convert to line eq and find intersection */
 157         pts2line(&l1.p1, &l1.p2, &ll1);
 158         pts2line(&l2.p1, &l2.p2, &ll2);
 159         intersect_line(&ll1, &ll2, &p);
 160
 161         line1_a_rad = atan2(l1.p2.y - l1.p1.y,
 162                             l1.p2.x - l1.p1.x);
 163         line1_a_deg = DEG(line1_a_rad);
 164         line2_a_rad = atan2(l2.p2.y - l2.p1.y,
 165                             l2.p2.x - l2.p1.x);
 166         diff_a_deg = DEG(line2_a_rad - line1_a_rad);
 167         if (diff_a_deg < -180) {
 168                 diff_a_deg += 360;
 169         }
 170         else if (diff_a_deg > 180) {
 171                 diff_a_deg -= 360;
 172         }
 173         diff_a_deg_abs = fabs(diff_a_deg);
 174
 175 /*      printf_P(PSTR("diff_a_deg=%2.2f\r\n"), diff_a_deg_abs); */
 176 /*      printf_P(PSTR("inter=%2.2f,%2.2f\r\n"), p.x, p.y); */
 177
 178         if (diff_a_deg_abs < 70.) {
 179                 radius = 200;
 180                 if (diff_a_deg > 0)
 181                         beta_deg = 40;
 182                 else
 183                         beta_deg = -40;
 184         }
 185         else if (diff_a_deg_abs < 100.) {
 186                 radius = 100;
 187                 if (diff_a_deg > 0)
 188                         beta_deg = 40;
 189                 else
 190                         beta_deg = -40;
 191         }
 192         else {
 193                 radius = 120;
 194                 if (diff_a_deg > 0)
 195                         beta_deg = 60;
 196                 else
 197                         beta_deg = -60;
 198         }
 199
 200         /* XXX check return value !! */
 201         ret = trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
 202                                  line1_a_deg, 150., diff_a_deg, beta_deg,
 203                                  radius, xy_norm(l1.p1.x, l1.p1.y,
 204                                                  p.x, p.y));
 205         if (ret < 0)
 206                 DEBUG(E_USER_STRAT, "clitoid failed");
 207
 208         /* disabled */
 209         if (0) {
 210                 err = 0;
 211                 while (err == 0) {
 212                         err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF);
 213                         if (err == 0) {
 214                                 /* cobboard is stucked */
 215                                 trajectory_hardstop(&mainboard.traj);
 216                                 return err; /* XXX do something */
 217                         }
 218                         err = test_traj_end(0xFF);
 219                 }
 220         }
 221
 222         err = WAIT_COND_OR_TRAJ_END(get_cob_count() == 5, 0xFF);
 223         strat_get_speed(&d_speed, &a_speed);
 224
 225         /* XXX 600 -> cste */
 226         /* XXX does not work, do better */
 227 /*      if (err == 0 && d_speed < 600 && */
 228 /*          mainboard.traj.state == RUNNING_CLITOID_LINE) */
 229 /*              strat_set_speed(600, SPEED_ANGLE_FAST); */
 230
 231         err = wait_traj_end(0xFF);
 232
 233         return err;
 234 }
 235
 236 void num2line(struct line_2pts *l, uint8_t dir, uint8_t num)
 237 {
 238         float n = num;
 239
 240         switch (dir) {
 241
 242         case LINE_UP:
 243                 l->p1.x = n * 450 + 375;
 244                 l->p1.y = COLOR_Y(0);
 245                 l->p2.x = n * 450 + 375;
 246                 l->p2.y = COLOR_Y(2100);
 247                 break;
 248         case LINE_DOWN:
 249                 l->p1.x = n * 450 + 375;
 250                 l->p1.y = COLOR_Y(2100);
 251                 l->p2.x = n * 450 + 375;
 252                 l->p2.y = COLOR_Y(0);
 253                 break;
 254         case LINE_R_UP:
 255                 l->p1.x = 150;
 256                 l->p1.y = COLOR_Y(-n * 500 + 1472);
 257                 l->p2.x = 2850;
 258                 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
 259                 break;
 260         case LINE_L_DOWN:
 261                 l->p1.x = 2850;
 262                 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
 263                 l->p2.x = 150;
 264                 l->p2.y = COLOR_Y(-n * 500 + 1472);
 265                 break;
 266         case LINE_L_UP:
 267                 l->p1.x = 2850;
 268                 l->p1.y = COLOR_Y(-n * 500 + 1472);
 269                 l->p2.x = 150;
 270                 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
 271                 break;
 272         case LINE_R_DOWN:
 273                 l->p1.x = 150;
 274                 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
 275                 l->p2.x = 2850;
 276                 l->p2.y = COLOR_Y(-n * 500 + 1472);
 277                 break;
 278         default:
 279                 break;
 280         }
 281 }