Revision 1343
WORKING
branches/encoders/code/behaviors/spline/server/internals.h | ||
---|---|---|
6 | 6 |
#define RS 2 |
7 | 7 |
#define RD 3 |
8 | 8 |
|
9 |
#define LEFT 0
|
|
10 |
#define RIGHT 1
|
|
9 |
#define LEFT 1
|
|
10 |
#define RIGHT 0
|
|
11 | 11 |
|
12 | 12 |
#define FORWARD 1 |
13 | 13 |
#define BACKWARD 0 |
... | ... | |
17 | 17 |
#define INFINITE -1 |
18 | 18 |
#define L 13.5 |
19 | 19 |
|
20 |
#define DT .102 |
|
20 |
//#define DT .102 |
|
21 |
#define DT .088 |
|
21 | 22 |
|
22 | 23 |
#define DIST(x1, y1, x2, y2) (sqrt(((x1)-(x2))*((x1)-(x2)) + ((y1)-(y2))*((y1)-(y2)))) |
23 |
#define LOOK_AHEAD 30
|
|
24 |
#define LOOK_AHEAD 40
|
|
24 | 25 |
#define LOOK_AHEAD_ERR_THRES 5 |
25 |
#define VELOCITY 20
|
|
26 |
#define VELOCITY 10
|
|
26 | 27 |
|
27 | 28 |
typedef struct state |
28 | 29 |
{ |
branches/encoders/code/behaviors/spline/server/server.c | ||
---|---|---|
4 | 4 |
#include <sys/types.h> |
5 | 5 |
#include <sys/stat.h> |
6 | 6 |
#include <fcntl.h> |
7 |
#include "encoders.h" |
|
8 | 7 |
#include "internals.h" |
9 | 8 |
#include <math.h> |
10 | 9 |
#include "speed_map.h" |
... | ... | |
67 | 66 |
} |
68 | 67 |
} |
69 | 68 |
|
70 |
|
|
69 |
#define RADIUS 100 |
|
71 | 70 |
int main(int argc, char** args) |
72 | 71 |
{ |
73 |
init(50, 0, M_PI/2); |
|
72 |
//init(RADIUS, 0, M_PI/2); |
|
73 |
init(0, 50, 0); |
|
74 | 74 |
//init(0,0,0); |
75 | 75 |
double x_ref[LEN]; |
76 | 76 |
double y_ref[LEN]; |
77 | 77 |
int i; |
78 | 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); |
|
79 |
// x_ref[i] = RADIUS * cos((double)(i+1) / LEN * 5.0 / 2.0 * M_PI); |
|
80 |
// y_ref[i] = RADIUS * sin((double)(i+1) / LEN * 5.0 / 2.0 * M_PI); |
|
81 |
|
|
82 |
x_ref[i] = ((double)(i+1) / LEN * 2.0 * M_PI) * 100; |
|
83 |
y_ref[i] = 50 *cos (x_ref[i] / 16.6667);//(double)(i+1) / LEN * 3.0 / 2.0 * M_PI); |
|
81 | 84 |
// 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]);
|
|
85 |
// y_ref[i] = (double)i / LEN * 100; |
|
86 |
//printf("%g \t%g\n", x_ref[i], y_ref[i]);
|
|
84 | 87 |
//getchar(); |
85 | 88 |
} |
89 |
|
|
90 |
//printf("aaaaaaa\n"); |
|
86 | 91 |
|
87 | 92 |
curv_t ref; |
88 | 93 |
ref.x = x_ref; |
... | ... | |
119 | 124 |
|
120 | 125 |
pos_control_curve(&ref, LEN); |
121 | 126 |
|
122 |
// ref_vels[LEFT] = 20;
|
|
123 |
// ref_vels[RIGHT] = 20;
|
|
127 |
// ref_vels[LEFT] = 20;
|
|
128 |
// ref_vels[RIGHT] = 20;
|
|
124 | 129 |
|
125 | 130 |
ref_vels[LEFT] = -(char)robot_velocity(1, robot.vl_ref); |
126 | 131 |
ref_vels[RIGHT] = -(char)robot_velocity(1, robot.vr_ref); |
127 | 132 |
|
128 |
// printf("Ref Vs: \t\t%d\t%d\n", ref_vels[LEFT], ref_vels[RIGHT]);
|
|
133 |
//printf("Ref Vs: \t\t%d\t%d\n", ref_vels[LEFT], ref_vels[RIGHT]);
|
|
129 | 134 |
// printf("Left Speed: %d \t Right Speed: %d\n", encoders[LS], encoders[RS]); |
130 | 135 |
|
131 | 136 |
//tempCount++; |
... | ... | |
154 | 159 |
robot.vl = real_velocity(1, -ref_vels[LEFT]); |
155 | 160 |
robot.vr = real_velocity(1, -ref_vels[RIGHT]); |
156 | 161 |
|
157 |
// printf("ACTUAL V \t\t%g\t%g\n", robot.vl, robot.vr);
|
|
162 |
// printf("ACTUAL V \t\t%g\t%g\n", robot.vl, robot.vr);
|
|
158 | 163 |
|
159 | 164 |
updatePosition(); |
160 | 165 |
//printf("Raw Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta); |
161 | 166 |
printf("%g\t%g\n", robot.x, robot.y); |
162 | 167 |
} |
163 |
|
|
164 |
return 0; |
|
165 |
|
|
166 | 168 |
} |
167 | 169 |
|
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 | 170 |
//Todo: This makes no sense, 4am |
186 | 171 |
void updatePosition() |
187 | 172 |
{ |
... | ... | |
239 | 224 |
static int find_goal(curv_t *ref, int len) { |
240 | 225 |
int i, goal_idx; |
241 | 226 |
double min_dist = 100000000; |
242 |
double temp; |
|
227 |
double temp, err;
|
|
243 | 228 |
int min_idx; |
229 |
int ctr = 0; |
|
244 | 230 |
for (i = 0; i < len; i++) { |
245 | 231 |
temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y); |
246 | 232 |
if (temp < min_dist) { |
... | ... | |
251 | 237 |
//printf("%d min %f\n", min_idx, min_dist); |
252 | 238 |
//getchar(); |
253 | 239 |
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 |
} |
|
240 |
err = LOOK_AHEAD_ERR_THRES; |
|
241 |
while(goal_idx == -1 && ctr < 3) { |
|
242 |
for (i = min_idx; i < len; i++) { |
|
243 |
temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y); |
|
244 |
if (abs(temp - LOOK_AHEAD) < err) { |
|
245 |
goal_idx = i; |
|
246 |
return goal_idx; |
|
247 |
} |
|
248 |
} |
|
249 |
err += 5; |
|
250 |
ctr++; |
|
251 |
} |
|
261 | 252 |
while(1) { |
262 |
printf("eol\n"); |
|
253 |
//printf("eol\n");
|
|
263 | 254 |
exit(0); |
264 | 255 |
} |
265 | 256 |
return goal_idx; |
Also available in: Unified diff