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