root / branches / encoders / code / behaviors / spline / server / server.c @ 1344
History | View | Annotate | Download (6.67 KB)
1 |
#include <termios.h> |
---|---|
2 |
#include <stdio.h> |
3 |
#include <stdlib.h> |
4 |
#include <sys/types.h> |
5 |
#include <sys/stat.h> |
6 |
#include <fcntl.h> |
7 |
#include "internals.h" |
8 |
#include <math.h> |
9 |
#include "speed_map.h" |
10 |
|
11 |
#include <time.h> |
12 |
|
13 |
#define PG 8 |
14 |
#define DG .1 |
15 |
#define IG .1 |
16 |
|
17 |
int VEL_S = 1; |
18 |
|
19 |
|
20 |
#define TICKS_PER_CM 48.03 |
21 |
#define TIME_INTERVAL 1520 |
22 |
|
23 |
#define LEN 30000 |
24 |
|
25 |
|
26 |
state robot; |
27 |
double vel_l_i;
|
28 |
double vel_r_i;
|
29 |
double vel_l_old;
|
30 |
double vel_r_old;
|
31 |
|
32 |
|
33 |
void init(double x, double y, double theta) |
34 |
{ |
35 |
robot.x = x; |
36 |
robot.y = y; |
37 |
robot.theta = theta; |
38 |
|
39 |
robot.vl = 0;
|
40 |
robot.vr = 0;
|
41 |
robot.vl_ref = 0;
|
42 |
robot.vr_ref = 0;
|
43 |
} |
44 |
|
45 |
void setAttrib(int file) |
46 |
{ |
47 |
struct termios attributes;
|
48 |
tcgetattr(file, &attributes); |
49 |
|
50 |
cfsetispeed(&attributes, B115200); |
51 |
cfsetospeed(&attributes, B115200); |
52 |
attributes.c_cflag &= ~PARENB; |
53 |
attributes.c_cflag &= ~CSTOPB; |
54 |
attributes.c_cflag &= ~CSIZE; |
55 |
attributes.c_cflag |= CS8; |
56 |
|
57 |
attributes.c_cflag &= ~ICRNL; |
58 |
attributes.c_cflag &= ~OCRNL; |
59 |
attributes.c_cflag |= (CLOCAL | CREAD); |
60 |
attributes.c_lflag &= ~ICANON; |
61 |
|
62 |
|
63 |
if (tcsetattr(file, TCSANOW, &attributes) < 0){ |
64 |
perror("tcsetattr failed");
|
65 |
exit(-1);
|
66 |
} |
67 |
} |
68 |
|
69 |
//#define FORWARD
|
70 |
|
71 |
#ifdef FORWARD
|
72 |
#undef LEFT
|
73 |
#undef RIGHT
|
74 |
#define LEFT 0 |
75 |
#define RIGHT 1 |
76 |
#endif
|
77 |
|
78 |
#define CIRCLE
|
79 |
//#define SINE
|
80 |
//#define COSINE
|
81 |
//#define LINE45
|
82 |
//#define LINE
|
83 |
|
84 |
#define RADIUS 70 |
85 |
#define AMP 70 |
86 |
|
87 |
int main(int argc, char** args) |
88 |
{ |
89 |
#ifdef CIRCLE
|
90 |
init(20, 30, 0); |
91 |
#endif
|
92 |
#ifdef SINE
|
93 |
init(0, 0, M_PI/4); |
94 |
#endif
|
95 |
#ifdef COSINE
|
96 |
init(0, AMP, 0); |
97 |
#endif
|
98 |
#ifdef LINE45
|
99 |
init(0,0,0); |
100 |
#endif
|
101 |
#ifdef LINE
|
102 |
init(0,0,0); |
103 |
#endif
|
104 |
double x_ref[LEN];
|
105 |
double y_ref[LEN];
|
106 |
int i;
|
107 |
for (i = 0; i < LEN; i++) { |
108 |
#ifdef CIRCLE
|
109 |
x_ref[i] = RADIUS * cos((double)(i) / LEN * 2.0 * M_PI); |
110 |
y_ref[i] = RADIUS * sin((double)(i) / LEN * 2.0 * M_PI); |
111 |
#endif
|
112 |
#ifdef SINE
|
113 |
x_ref[i] = ((double)(i+1) / LEN * 2.0 * M_PI) * 100; |
114 |
y_ref[i] = AMP *sin (x_ref[i] / 50);
|
115 |
#endif
|
116 |
#ifdef COSINE
|
117 |
x_ref[i] = ((double)(i+1) / LEN * 2.0 * M_PI) * 100; |
118 |
y_ref[i] = AMP *cos (x_ref[i] / 50);
|
119 |
#endif
|
120 |
#ifdef LINE45
|
121 |
x_ref[i] = (double)i / LEN * 1000; |
122 |
y_ref[i] = (double)i / LEN * 1000; |
123 |
#endif
|
124 |
#ifdef LINE
|
125 |
x_ref[i] = (double)i / LEN * 1000; |
126 |
y_ref[i] = 0;
|
127 |
#endif
|
128 |
|
129 |
//printf("%g \t%g\n", x_ref[i], y_ref[i]);
|
130 |
//getchar();
|
131 |
} |
132 |
|
133 |
//printf("aaaaaaa\n");
|
134 |
|
135 |
curv_t ref; |
136 |
ref.x = x_ref; |
137 |
ref.y = y_ref; |
138 |
|
139 |
int serialFileIn = open("/dev/ttyUSB0", O_RDWR); |
140 |
int serialFileOut = open("/dev/ttyUSB1", O_RDWR); |
141 |
if(serialFileIn < 1 || serialFileOut < 1) |
142 |
{ |
143 |
printf("Error opening serial\n");
|
144 |
return -1; |
145 |
} |
146 |
|
147 |
setAttrib(serialFileOut); |
148 |
setAttrib(serialFileIn); |
149 |
|
150 |
//unsigned char encoders[4] = {0,0,0,0};
|
151 |
char ref_vels[2] = {0,0}; |
152 |
|
153 |
char senderNum = SENDER;
|
154 |
char receiverNum = RECEIVER;
|
155 |
|
156 |
write(serialFileIn, &receiverNum, 1);
|
157 |
sleep(1);
|
158 |
write(serialFileOut, &senderNum, 1);
|
159 |
|
160 |
//Sending velocity as LS LD RS RD
|
161 |
int tempCount = 0; |
162 |
|
163 |
int dx;
|
164 |
double Ul, Ur;
|
165 |
|
166 |
while(1) { |
167 |
|
168 |
pos_control_curve(&ref, LEN); |
169 |
|
170 |
// ref_vels[LEFT] = 20;
|
171 |
// ref_vels[RIGHT] = 20;
|
172 |
|
173 |
|
174 |
ref_vels[LEFT] = -(char)robot_velocity(1, robot.vl_ref); |
175 |
ref_vels[RIGHT] = -(char)robot_velocity(1, robot.vr_ref); |
176 |
#ifdef FORWARD
|
177 |
ref_vels[LEFT] = -ref_vels[LEFT]; |
178 |
ref_vels[RIGHT] = -ref_vels[RIGHT]; |
179 |
#endif
|
180 |
|
181 |
//printf("Ref Vs: \t\t%d\t%d\n", ref_vels[LEFT], ref_vels[RIGHT]);
|
182 |
// printf("Left Speed: %d \t Right Speed: %d\n", encoders[LS], encoders[RS]);
|
183 |
|
184 |
//tempCount++;
|
185 |
//printf("RECEIVED %d\n", tempCount);
|
186 |
int temp = 0; |
187 |
int count = 0; |
188 |
do
|
189 |
{ |
190 |
temp = write(serialFileOut, ref_vels + count, 1);
|
191 |
if(temp < 0) |
192 |
perror("Write Error");
|
193 |
count += temp; |
194 |
//printf("sent: %d\n", count);
|
195 |
usleep(10000);
|
196 |
// sleep(1);
|
197 |
} while(count < 2); |
198 |
|
199 |
count = 0;
|
200 |
//printf("READING DATA\n");
|
201 |
do
|
202 |
{ |
203 |
count += read(serialFileIn, ref_vels, 2);
|
204 |
}while(count < 2); |
205 |
|
206 |
|
207 |
#ifndef FORWARD
|
208 |
robot.vl = real_velocity(1, -ref_vels[LEFT]);
|
209 |
robot.vr = real_velocity(1, -ref_vels[RIGHT]);
|
210 |
#else
|
211 |
robot.vl = real_velocity(1, ref_vels[LEFT]);
|
212 |
robot.vr = real_velocity(1, ref_vels[RIGHT]);
|
213 |
#endif
|
214 |
|
215 |
//printf("ACTUAL V \t\t%g\t%g\n", robot.vl, robot.vr);
|
216 |
|
217 |
updatePosition(); |
218 |
//printf("Raw Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta);
|
219 |
printf("%g\t%g\n", robot.x, robot.y);
|
220 |
} |
221 |
} |
222 |
|
223 |
//Todo: This makes no sense, 4am
|
224 |
void updatePosition()
|
225 |
{ |
226 |
double wdt = (robot.vr - robot.vl)*DT/L;
|
227 |
double R;
|
228 |
|
229 |
if(robot.vr - robot.vl != 0) |
230 |
R = (L/2)*(robot.vr + robot.vl)/(robot.vr - robot.vl);
|
231 |
|
232 |
if(wdt != 0) |
233 |
{ |
234 |
double ICC[2] = {robot.x - R * sin(robot.theta), robot.y + R*cos(robot.theta)}; |
235 |
robot.x = (cos(wdt)*(robot.x-ICC[0]) - sin(wdt)*(robot.y-ICC[1]) + ICC[0]); |
236 |
robot.y = (sin(wdt)*(robot.x-ICC[0]) + cos(wdt)*(robot.y-ICC[1]) + ICC[1]); |
237 |
robot.theta = robot.theta + wdt; |
238 |
} |
239 |
else
|
240 |
{ |
241 |
robot.x += cos(robot.theta)*robot.vr*DT; |
242 |
robot.y += sin(robot.theta)*robot.vr*DT; |
243 |
} |
244 |
if (robot.theta > 2*M_PI) |
245 |
robot.theta -= 2*M_PI;
|
246 |
if (robot.theta < -2*M_PI) |
247 |
robot.theta += 2*M_PI;
|
248 |
} |
249 |
|
250 |
|
251 |
static int find_goal(curv_t *ref, int len); |
252 |
|
253 |
void pos_control_curve(curv_t *ref, int len) { |
254 |
int goal_idx = find_goal(ref, len);
|
255 |
//printf("\tGOAL: x: %g, y: %g\n", ref->x[goal_idx], ref->y[goal_idx]);
|
256 |
//did a transformation here.
|
257 |
double goal_y = cos(robot.theta)*(ref->y[goal_idx]-robot.y)
|
258 |
- sin(robot.theta)*(ref->x[goal_idx]-robot.x); |
259 |
double goal_rad = 2*goal_y / LOOK_AHEAD; |
260 |
|
261 |
|
262 |
robot.vl_ref = real_velocity(1, VELOCITY)*(1-0.5*goal_rad); |
263 |
robot.vr_ref = real_velocity(1, VELOCITY)*(1+0.5*goal_rad); |
264 |
/*
|
265 |
if(robot.vl_ref > 127)
|
266 |
robot.vl_ref = 127;
|
267 |
else if(robot.vl_ref < -128)
|
268 |
robot.vl_ref = -128;
|
269 |
if(robot.vr_ref > 127)
|
270 |
robot.vr_ref = 127;
|
271 |
else if(robot.vr_ref < -128)
|
272 |
robot.vr_ref = -128;
|
273 |
*/
|
274 |
|
275 |
} |
276 |
|
277 |
static int find_goal(curv_t *ref, int len) { |
278 |
int i, goal_idx;
|
279 |
double min_dist = 100000000; |
280 |
double temp, err;
|
281 |
int min_idx;
|
282 |
int ctr = 0; |
283 |
for (i = 0; i < len; i++) { |
284 |
temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y); |
285 |
if (temp < min_dist) {
|
286 |
min_dist = temp; |
287 |
min_idx = i; |
288 |
} |
289 |
} |
290 |
/*
|
291 |
if (min_dist > LOOK_AHEAD) {
|
292 |
printf("toooooo far away \n");
|
293 |
return min_idx;
|
294 |
}
|
295 |
*/
|
296 |
//printf("%d min %f\n", min_idx, min_dist);
|
297 |
//getchar();
|
298 |
goal_idx = -1;
|
299 |
err = LOOK_AHEAD_ERR_THRES; |
300 |
while(ctr < 3) { |
301 |
for (i = min_idx; i < len; i++) {
|
302 |
#ifdef CIRCLE
|
303 |
if (i == len-1) |
304 |
i = 0;
|
305 |
#endif
|
306 |
temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y); |
307 |
if (abs(temp - LOOK_AHEAD) < err) {
|
308 |
goal_idx = i; |
309 |
return goal_idx;
|
310 |
} |
311 |
} |
312 |
err += 5;
|
313 |
ctr++; |
314 |
} |
315 |
while(1) { |
316 |
printf("eol\n");
|
317 |
exit(0);
|
318 |
} |
319 |
return goal_idx;
|
320 |
} |
321 |
|