save
[aversive.git] / 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