code 2010
[aversive.git] / projects / microb2010 / tests / hostsim / 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 <pid.h>
39 #include <quadramp.h>
40 #include <control_system_manager.h>
41 #include <trajectory_manager.h>
42 #include <blocking_detection_manager.h>
43 #include <robot_system.h>
44 #include <position_manager.h>
45
46 #include <parse.h>
47 #include <rdline.h>
48
49 #include "strat.h"
50 #include "strat_utils.h"
51 #include "main.h"
52
53 static int32_t l_pwm, r_pwm;
54 static int32_t l_enc, r_enc;
55
56 static int fdr, fdw;
57
58 /* */
59 #define FILTER  97
60 #define FILTER2 (100-FILTER)
61 #define SHIFT   4
62
63 void robotsim_dump(void)
64 {
65         char buf[BUFSIZ];
66         int len;
67
68         len = snprintf(buf, sizeof(buf), "%d %d %d\n",
69                       position_get_x_s16(&mainboard.pos),
70                       position_get_y_s16(&mainboard.pos),
71                       position_get_a_deg_s16(&mainboard.pos));
72         hostsim_lock();
73         write(fdw, buf, len);
74         hostsim_unlock();
75 }
76
77 /* must be called periodically */
78 void robotsim_update(void)
79 {
80         static int32_t l_pwm_shift[SHIFT];
81         static int32_t r_pwm_shift[SHIFT];
82         static int32_t l_speed, r_speed;
83         static unsigned i = 0;
84         static unsigned cpt = 0;
85         int32_t local_l_pwm, local_r_pwm;
86         double x, y, a, a2, d;
87         char cmd = 0;
88
89         /* corners of the robot */
90         double xfl, yfl; /* front left */
91         double xrl, yrl; /* rear left */
92         double xrr, yrr; /* rear right */
93         double xfr, yfr; /* front right */
94
95         /* time shift the command */
96         l_pwm_shift[i] = l_pwm;
97         r_pwm_shift[i] = r_pwm;
98         i ++;
99         i %= SHIFT;
100         local_l_pwm = l_pwm_shift[i];
101         local_r_pwm = r_pwm_shift[i];
102
103         /* read command */
104         if (((cpt ++) & 0x7) == 0) {
105                 if (read(fdr, &cmd, 1) != 1)
106                         cmd = 0;
107         }
108
109         x = position_get_x_double(&mainboard.pos);
110         y = position_get_y_double(&mainboard.pos);
111         a = position_get_a_rad_double(&mainboard.pos);
112
113         l_speed = ((l_speed * FILTER) / 100) +
114                 ((local_l_pwm * 1000 * FILTER2)/1000);
115         r_speed = ((r_speed * FILTER) / 100) +
116                 ((local_r_pwm * 1000 * FILTER2)/1000);
117
118         /* basic collision detection */
119         a2 = atan2(ROBOT_WIDTH/2, ROBOT_LENGTH/2);
120         d = norm(ROBOT_WIDTH/2, ROBOT_LENGTH/2);
121
122         xfl = x + cos(a+a2) * d;
123         yfl = y + sin(a+a2) * d;
124         if (!is_in_area(xfl, yfl, 0) && l_speed > 0)
125                 l_speed = 0;
126
127         xrl = x + cos(a+M_PI-a2) * d;
128         yrl = y + sin(a+M_PI-a2) * d;
129         if (!is_in_area(xrl, yrl, 0) && l_speed < 0)
130                 l_speed = 0;
131
132         xrr = x + cos(a+M_PI+a2) * d;
133         yrr = y + sin(a+M_PI+a2) * d;
134         if (!is_in_area(xrr, yrr, 0) && r_speed < 0)
135                 r_speed = 0;
136
137         xfr = x + cos(a-a2) * d;
138         yfr = y + sin(a-a2) * d;
139         if (!is_in_area(xfr, yfr, 0) && r_speed > 0)
140                 r_speed = 0;
141
142         /* perturbation */
143         if (cmd == 'l')
144                 l_enc += 5000; /* push 1 cm */
145         if (cmd == 'r')
146                 r_enc += 5000; /* push 1 cm */
147
148         /* XXX should lock */
149         l_enc += (l_speed / 1000);
150         r_enc += (r_speed / 1000);
151 }
152
153 void robotsim_pwm(void *arg, int32_t val)
154 {
155         //      printf("%p, %d\n", arg, val);
156         if (arg == LEFT_PWM)
157                 l_pwm = val;
158         else if (arg == RIGHT_PWM)
159                 r_pwm = val;
160 }
161
162 int32_t robotsim_encoder_get(void *arg)
163 {
164         if (arg == LEFT_ENCODER)
165                 return l_enc;
166         else if (arg == RIGHT_ENCODER)
167                 return r_enc;
168         return 0;
169 }
170
171 int robotsim_init(void)
172 {
173         mkfifo("/tmp/.robot_sim2dis", 0600);
174         mkfifo("/tmp/.robot_dis2sim", 0600);
175         fdw = open("/tmp/.robot_sim2dis", O_WRONLY, 0);
176         if (fdw < 0)
177                 return -1;
178         fdr = open("/tmp/.robot_dis2sim", O_RDONLY | O_NONBLOCK, 0);
179         if (fdr < 0) {
180                 close(fdw);
181                 return -1;
182         }
183         return 0;
184 }