modules/devices/robot/trajectory_manager/test/main.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.1.2.3 2007-12-31 16:25:01 zer0 Exp $
  19  *
  20  */
  21
  22 #include <stdio.h>
  23 #include <string.h>
  24 #include <math.h>
  25 #include <aversive.h>
  26 #include <aversive/pgmspace.h>
  27 #include <aversive/error.h>
  28 #include <aversive/wait.h>
  29
  30 #include <scheduler.h>
  31 #include <quadramp.h>
  32 #include <control_system_manager.h>
  33 #include <position_manager.h>
  34 #include <trajectory_manager.h>
  35
  36
  37
  38 /* XXX it is 90 don't forget it :) */
  39 #define MATCH_TIME 90
  40
  41 /* decrease track to decrease angle */
  42 #define MOT_TRACK_CM 23.906
  43 #define EXT_TRACK_CM 8.995 // 8.5
  44 #define VIRTUAL_TRACK_CM MOT_TRACK_CM
  45
  46 #define DIST_IMP_CM 6160.0
  47
  48 #define LEFT_ENCODER_MOT  ((void *)0)
  49 #define RIGHT_ENCODER_MOT ((void *)1)
  50 #define LEFT_ENCODER_EXT   ((void *)2)
  51 #define RIGHT_ENCODER_EXT  ((void *)3)
  52
  53
  54 #define LEFT_PWM             ((void *)PWM3C)
  55 #define RIGHT_PWM            ((void *)PWM1A)
  56 #define ROLL_PWM            ((void *)PWM2)
  57
  58 #define DO_CS     1  /* control system */
  59 #define DO_RS     2  /* robot system (angle/distance) */
  60 #define DO_POS    4  /* process position */
  61 #define DO_BD     8  /* process blocking detection */
  62 #define DO_TIMER  16 /* check time and stop after 90 secs */
  63
  64 /** ERROR NUMS */
  65 #define E_USER_NOTHING    193
  66
  67 #define BALL_IS_READY      1
  68 #define BALL_IS_WHITE      2
  69 #define BALL_COUNT     (4+8)
  70 #define BALL_SUCKED       16
  71 #define BALL_BUSY         32
  72
  73 #define BALL_COUNT_BIT     2
  74
  75 #define BALL_LAYDOWN 1
  76 #define BALL_EJECT   2
  77 #define BALL_RESUCK  3
  78 #define BALL_OFF     4
  79 #define BALL_ON      5
  80 #define BALL_LAYDOWN_FORCE 6
  81 #define BALL_EJECT_FORCE   7
  82
  83 struct robot {
  84         s08 cs_events;
  85
  86         struct robot_system rs;
  87         struct robot_position pos;
  88
  89         struct cs cs_a;
  90         struct quadramp_filter qr_a;
  91
  92         struct cs cs_d;
  93         struct quadramp_filter qr_d;
  94
  95         struct trajectory traj;
  96 } robot;
  97
  98 /* WARNING : this test is only a compilation test */
  99 /* it does not check that the module is working */
 100 int main(void)
 101 {
 102         /* ROBOT_SYSTEM */
 103         rs_init(&robot.rs);
 104         rs_set_ratio(&robot.rs, MOT_TRACK_CM / EXT_TRACK_CM);
 105         rs_set_flags(&robot.rs, 0); /* rs use mot */
 106
 107         /* POSITION MANAGER */
 108         position_init(&robot.pos);
 109         position_set_physical_params(&robot.pos, VIRTUAL_TRACK_CM, DIST_IMP_CM);
 110         position_set_related_robot_system(&robot.pos, &robot.rs);
 111         position_use_ext(&robot.pos);
 112
 113         /* CS DISTANCE */
 114         quadramp_init(&robot.qr_d);
 115         quadramp_set_1st_order_vars(&robot.qr_d, 1000, 1000); /* set speed */
 116         quadramp_set_2nd_order_vars(&robot.qr_d, 10, 10); /* set accel */
 117
 118         cs_init(&robot.cs_d);
 119         cs_set_consign_filter(&robot.cs_d, quadramp_do_filter, &robot.qr_d);
 120         cs_set_consign(&robot.cs_d, 0);
 121
 122         /* CS ANGLE */
 123         quadramp_init(&robot.qr_a);
 124         quadramp_set_1st_order_vars(&robot.qr_a, 500, 500); /* set speed */
 125         quadramp_set_2nd_order_vars(&robot.qr_a, 5, 5); /* set accel */
 126
 127         cs_init(&robot.cs_a);
 128         cs_set_consign_filter(&robot.cs_a, quadramp_do_filter, &robot.qr_a);
 129         cs_set_consign(&robot.cs_a, 0);
 130
 131         /* TRAJECTORY MANAGER */
 132         trajectory_init(&robot.traj);
 133         trajectory_set_cs(&robot.traj, &robot.cs_d, &robot.cs_a);
 134         trajectory_set_robot_params(&robot.traj, &robot.rs, &robot.pos);
 135         trajectory_set_windows(&robot.traj, 12.0, 15.0, 50.0);
 136
 137 #ifdef HOST_VERSION
 138         while(1) {
 139                 scheduler_interrupt();
 140         }
 141 #endif
 142
 143         return 0;
 144 }
 145
 146