Revision 1342
sort of works, does the 45 line correctly.
server.c | ||
---|---|---|
7 | 7 |
#include "encoders.h" |
8 | 8 |
#include "internals.h" |
9 | 9 |
#include <math.h> |
10 |
#include "speed_map.h" |
|
10 | 11 |
|
12 |
#include <time.h> |
|
11 | 13 |
|
12 |
#define PG 150
|
|
13 |
#define DG 100
|
|
14 |
#define IG 10
|
|
14 |
#define PG 8
|
|
15 |
#define DG .1
|
|
16 |
#define IG .1
|
|
15 | 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 |
|
|
16 | 27 |
state robot; |
17 | 28 |
double vel_l_i; |
18 | 29 |
double vel_r_i; |
... | ... | |
27 | 38 |
robot.theta = theta; |
28 | 39 |
|
29 | 40 |
robot.vl = 0; |
30 |
robot.vr = 0; |
|
41 |
robot.vr = 0;
|
|
31 | 42 |
robot.vl_ref = 0; |
32 | 43 |
robot.vr_ref = 0; |
33 | 44 |
} |
... | ... | |
42 | 53 |
attributes.c_cflag &= ~PARENB; |
43 | 54 |
attributes.c_cflag &= ~CSTOPB; |
44 | 55 |
attributes.c_cflag &= ~CSIZE; |
45 |
attributes.c_cflag |= CS8; |
|
56 |
attributes.c_cflag |= CS8;
|
|
46 | 57 |
|
47 | 58 |
attributes.c_cflag &= ~ICRNL; |
48 | 59 |
attributes.c_cflag &= ~OCRNL; |
... | ... | |
56 | 67 |
} |
57 | 68 |
} |
58 | 69 |
|
59 |
int main() |
|
70 |
|
|
71 |
int main(int argc, char** args) |
|
60 | 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 |
|
|
61 | 91 |
int serialFileIn = open("/dev/ttyUSB0", O_RDWR); |
62 | 92 |
int serialFileOut = open("/dev/ttyUSB1", O_RDWR); |
63 | 93 |
if(serialFileIn < 1 || serialFileOut < 1) |
... | ... | |
68 | 98 |
|
69 | 99 |
setAttrib(serialFileOut); |
70 | 100 |
setAttrib(serialFileIn); |
71 |
|
|
72 |
unsigned char encoders[4] = {1,1,1,1}; |
|
73 |
|
|
101 |
|
|
102 |
//unsigned char encoders[4] = {0,0,0,0}; |
|
103 |
char ref_vels[2] = {0,0}; |
|
104 |
|
|
74 | 105 |
char senderNum = SENDER; |
75 | 106 |
char receiverNum = RECEIVER; |
76 | 107 |
|
... | ... | |
79 | 110 |
write(serialFileOut, &senderNum, 1); |
80 | 111 |
|
81 | 112 |
//Sending velocity as LS LD RS RD |
82 |
int tempCount = 190;
|
|
113 |
int tempCount = 0; |
|
83 | 114 |
|
84 |
int dx, left_v;
|
|
115 |
int dx; |
|
85 | 116 |
double Ul, Ur; |
86 | 117 |
|
87 |
while(1) |
|
88 |
{ |
|
118 |
while(1) { |
|
89 | 119 |
|
90 |
printf("SENDING DATA\n"); |
|
91 |
|
|
92 |
velocityControl(&Ul, &Ur); |
|
93 |
if(Ul > 0) |
|
94 |
encoders[LD] = FORWARD; |
|
95 |
else |
|
96 |
encoders[LD] = BACKWARD; |
|
97 |
encoders[LS] = (unsigned char)abs(Ul); |
|
120 |
pos_control_curve(&ref, LEN); |
|
121 |
|
|
122 |
// ref_vels[LEFT] = 20; |
|
123 |
// ref_vels[RIGHT] = 20; |
|
98 | 124 |
|
99 |
if(Ur > 0)
|
|
100 |
encoders[RD] = FORWARD;
|
|
101 |
else |
|
102 |
encoders[RD] = BACKWARD;
|
|
103 |
encoders[RS] = (unsigned char)abs(Ur);
|
|
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]);
|
|
104 | 130 |
|
105 |
|
|
131 |
//tempCount++; |
|
132 |
//printf("RECEIVED %d\n", tempCount); |
|
106 | 133 |
int temp = 0; |
107 | 134 |
int count = 0; |
108 | 135 |
do |
109 | 136 |
{ |
110 |
temp = write(serialFileOut, encoders + count, 1);
|
|
137 |
temp = write(serialFileOut, ref_vels + count, 1);
|
|
111 | 138 |
if(temp < 0) |
112 | 139 |
perror("Write Error"); |
113 | 140 |
count += temp; |
114 |
printf("sent: %d\n", count); |
|
115 |
usleep(200); |
|
116 |
}while(count < 4); |
|
141 |
//printf("sent: %d\n", count); |
|
142 |
usleep(10000); |
|
143 |
// sleep(1); |
|
144 |
} while(count < 2); |
|
145 |
|
|
117 | 146 |
count = 0; |
118 |
printf("READING DATA\n"); |
|
147 |
//printf("READING DATA\n");
|
|
119 | 148 |
do |
120 | 149 |
{ |
121 |
count += read(serialFileIn, encoders, 4);
|
|
122 |
}while(count < 4);
|
|
150 |
count += read(serialFileIn, ref_vels, 2);
|
|
151 |
}while(count < 2);
|
|
123 | 152 |
|
124 |
robot.vl = (encoders[0] << 8) | encoders[1]; |
|
125 |
robot.vr = (encoders[2] << 8) | encoders[3]; |
|
126 | 153 |
|
127 |
//printf("%d %d\n", robot.vl, robot.vr); |
|
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); |
|
128 | 158 |
|
129 | 159 |
updatePosition(); |
130 |
printf("Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta);
|
|
131 |
|
|
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); |
|
132 | 162 |
} |
163 |
|
|
164 |
return 0; |
|
133 | 165 |
|
134 | 166 |
} |
135 | 167 |
|
136 | 168 |
void scaleInput(double* input) |
137 | 169 |
{ |
170 |
printf("INPUT: %g\n", *input); |
|
138 | 171 |
if(*input > 0) |
139 | 172 |
{ |
140 |
(*input) += 170; |
|
173 |
//(*input) += 170;
|
|
141 | 174 |
if(*input > 210) |
142 | 175 |
*input = 210; |
143 | 176 |
} |
144 |
else |
|
177 |
else if((*input) < 0)
|
|
145 | 178 |
{ |
146 |
(*input) -= 170; |
|
179 |
//(*input) -= 170;
|
|
147 | 180 |
if(*input < -210) |
148 | 181 |
*input = -210; |
149 | 182 |
} |
150 | 183 |
} |
151 | 184 |
|
152 |
void velocityControl(double* Ul, double* Ur) |
|
153 |
{ |
|
154 |
double vel_l_e = robot.vl_ref - robot.vl; |
|
155 |
double vel_r_e = robot.vr_ref - robot.vr; |
|
156 |
|
|
157 |
vel_l_i = vel_l_i + vel_l_e; |
|
158 |
vel_r_i = vel_r_i + vel_r_e; |
|
159 |
|
|
160 |
(*Ul) = PG * vel_l_e + DG * (vel_l_e - vel_l_old) + IG * vel_l_i; |
|
161 |
(*Ur) = PG * vel_r_e + DG * (vel_r_e - vel_r_old) + IG * vel_r_i; |
|
162 |
|
|
163 |
scaleInput(Ul); |
|
164 |
scaleInput(Ur); |
|
165 |
|
|
166 |
vel_l_old = vel_l_e; |
|
167 |
vel_l_old = vel_r_e; |
|
168 |
|
|
169 |
} |
|
170 |
|
|
185 |
//Todo: This makes no sense, 4am |
|
171 | 186 |
void updatePosition() |
172 | 187 |
{ |
173 | 188 |
double wdt = (robot.vr - robot.vl)*DT/L; |
174 | 189 |
double R; |
175 | 190 |
|
176 |
if(robot.vr - robot.vl > 0)
|
|
191 |
if(robot.vr - robot.vl != 0)
|
|
177 | 192 |
R = (L/2)*(robot.vr + robot.vl)/(robot.vr - robot.vl); |
178 | 193 |
|
179 | 194 |
if(wdt != 0) |
180 | 195 |
{ |
181 | 196 |
double ICC[2] = {robot.x - R * sin(robot.theta), robot.y + R*cos(robot.theta)}; |
182 |
robot.x = cos(wdt)*(robot.x-ICC[0]) - sin(wdt)*(robot.y-ICC[1]) + ICC[0];
|
|
183 |
robot.y = sin(wdt)*(robot.x-ICC[0]) + cos(wdt)*(robot.y-ICC[1]) + ICC[1];
|
|
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]);
|
|
184 | 199 |
robot.theta = robot.theta + wdt; |
185 | 200 |
} |
186 | 201 |
else |
187 | 202 |
{ |
188 |
robot.x += cos(robot.theta)*robot.vr; |
|
189 |
robot.y += sin(robot.theta)*robot.vr; |
|
203 |
robot.x += cos(robot.theta)*robot.vr*DT;
|
|
204 |
robot.y += sin(robot.theta)*robot.vr*DT;
|
|
190 | 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; |
|
191 | 210 |
} |
192 | 211 |
|
193 | 212 |
|
194 | 213 |
static int find_goal(curv_t *ref, int len); |
195 |
|
|
196 |
void pos_control_curv(curv_t *ref, int len) { |
|
214 |
|
|
215 |
void pos_control_curve(curv_t *ref, int len) {
|
|
197 | 216 |
int goal_idx = find_goal(ref, len); |
198 | 217 |
//did a transformation here. |
199 | 218 |
double goal_y = cos(robot.theta)*(ref->y[goal_idx]-robot.y) |
200 | 219 |
- sin(robot.theta)*(ref->x[goal_idx]-robot.x); |
201 | 220 |
double goal_rad = 2*goal_y / LOOK_AHEAD; |
202 | 221 |
|
203 |
robot.vl_ref = VELOCITY*(1-0.5*goal_rad); |
|
204 |
robot.vr_ref = VELOCITY*(1+0.5*goal_rad); |
|
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 |
|
|
205 | 237 |
} |
206 | 238 |
|
207 | 239 |
static int find_goal(curv_t *ref, int len) { |
... | ... | |
216 | 248 |
min_idx = i; |
217 | 249 |
} |
218 | 250 |
} |
251 |
//printf("%d min %f\n", min_idx, min_dist); |
|
252 |
//getchar(); |
|
219 | 253 |
goal_idx = -1; |
220 | 254 |
for (i = min_idx; i < len; i++) { |
221 | 255 |
temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y); |
222 | 256 |
if (abs(temp - LOOK_AHEAD) < LOOK_AHEAD_ERR_THRES) { |
223 | 257 |
goal_idx = i; |
224 |
break;
|
|
225 |
}
|
|
258 |
return goal_idx;
|
|
259 |
}
|
|
226 | 260 |
} |
261 |
while(1) { |
|
262 |
printf("eol\n"); |
|
263 |
exit(0); |
|
264 |
} |
|
227 | 265 |
return goal_idx; |
228 |
} |
|
266 |
} |
|
267 |
|
Also available in: Unified diff