Revision 1344
Final version of feedback controls project
| server.c | ||
|---|---|---|
| 66 | 66 |
} |
| 67 | 67 |
} |
| 68 | 68 |
|
| 69 |
#define RADIUS 100 |
|
| 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 |
|
|
| 70 | 87 |
int main(int argc, char** args) |
| 71 | 88 |
{
|
| 72 |
//init(RADIUS, 0, M_PI/2); |
|
| 73 |
init(0, 50, 0); |
|
| 74 |
//init(0,0,0); |
|
| 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 |
|
| 75 | 104 |
double x_ref[LEN]; |
| 76 | 105 |
double y_ref[LEN]; |
| 77 | 106 |
int i; |
| 78 | 107 |
for (i = 0; i < LEN; i++) {
|
| 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 |
|
|
| 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 |
|
| 82 | 113 |
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); |
|
| 84 |
// x_ref[i] = (double)i / LEN * 100; |
|
| 85 |
// y_ref[i] = (double)i / LEN * 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 |
|
|
| 86 | 129 |
//printf("%g \t%g\n", x_ref[i], y_ref[i]);
|
| 87 | 130 |
//getchar(); |
| 88 | 131 |
} |
| ... | ... | |
| 127 | 170 |
// ref_vels[LEFT] = 20; |
| 128 | 171 |
// ref_vels[RIGHT] = 20; |
| 129 | 172 |
|
| 173 |
|
|
| 130 | 174 |
ref_vels[LEFT] = -(char)robot_velocity(1, robot.vl_ref); |
| 131 | 175 |
ref_vels[RIGHT] = -(char)robot_velocity(1, robot.vr_ref); |
| 132 |
|
|
| 176 |
#ifdef FORWARD |
|
| 177 |
ref_vels[LEFT] = -ref_vels[LEFT]; |
|
| 178 |
ref_vels[RIGHT] = -ref_vels[RIGHT]; |
|
| 179 |
#endif |
|
| 180 |
|
|
| 133 | 181 |
//printf("Ref Vs: \t\t%d\t%d\n", ref_vels[LEFT], ref_vels[RIGHT]);
|
| 134 | 182 |
// printf("Left Speed: %d \t Right Speed: %d\n", encoders[LS], encoders[RS]);
|
| 135 | 183 |
|
| ... | ... | |
| 156 | 204 |
}while(count < 2); |
| 157 | 205 |
|
| 158 | 206 |
|
| 207 |
#ifndef FORWARD |
|
| 159 | 208 |
robot.vl = real_velocity(1, -ref_vels[LEFT]); |
| 160 | 209 |
robot.vr = real_velocity(1, -ref_vels[RIGHT]); |
| 161 |
|
|
| 162 |
// printf("ACTUAL V \t\t%g\t%g\n", robot.vl, robot.vr);
|
|
| 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);
|
|
| 163 | 216 |
|
| 164 | 217 |
updatePosition(); |
| 165 | 218 |
//printf("Raw Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta);
|
| ... | ... | |
| 199 | 252 |
|
| 200 | 253 |
void pos_control_curve(curv_t *ref, int len) {
|
| 201 | 254 |
int goal_idx = find_goal(ref, len); |
| 255 |
//printf("\tGOAL: x: %g, y: %g\n", ref->x[goal_idx], ref->y[goal_idx]);
|
|
| 202 | 256 |
//did a transformation here. |
| 203 | 257 |
double goal_y = cos(robot.theta)*(ref->y[goal_idx]-robot.y) |
| 204 | 258 |
- sin(robot.theta)*(ref->x[goal_idx]-robot.x); |
| 205 | 259 |
double goal_rad = 2*goal_y / LOOK_AHEAD; |
| 206 | 260 |
|
| 207 |
//printf("GOAL: y: %g, o: %g\n", goal_y, goal_rad);
|
|
| 208 | 261 |
|
| 209 | 262 |
robot.vl_ref = real_velocity(1, VELOCITY)*(1-0.5*goal_rad); |
| 210 | 263 |
robot.vr_ref = real_velocity(1, VELOCITY)*(1+0.5*goal_rad); |
| ... | ... | |
| 234 | 287 |
min_idx = i; |
| 235 | 288 |
} |
| 236 | 289 |
} |
| 290 |
/* |
|
| 291 |
if (min_dist > LOOK_AHEAD) {
|
|
| 292 |
printf("toooooo far away \n");
|
|
| 293 |
return min_idx; |
|
| 294 |
} |
|
| 295 |
*/ |
|
| 237 | 296 |
//printf("%d min %f\n", min_idx, min_dist);
|
| 238 | 297 |
//getchar(); |
| 239 | 298 |
goal_idx = -1; |
| 240 | 299 |
err = LOOK_AHEAD_ERR_THRES; |
| 241 |
while(goal_idx == -1 && ctr < 3) {
|
|
| 300 |
while(ctr < 3) {
|
|
| 242 | 301 |
for (i = min_idx; i < len; i++) {
|
| 302 |
#ifdef CIRCLE |
|
| 303 |
if (i == len-1) |
|
| 304 |
i = 0; |
|
| 305 |
#endif |
|
| 243 | 306 |
temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y); |
| 244 | 307 |
if (abs(temp - LOOK_AHEAD) < err) {
|
| 245 | 308 |
goal_idx = i; |
| ... | ... | |
| 250 | 313 |
ctr++; |
| 251 | 314 |
} |
| 252 | 315 |
while(1) {
|
| 253 |
//printf("eol\n");
|
|
| 316 |
printf("eol\n");
|
|
| 254 | 317 |
exit(0); |
| 255 | 318 |
} |
| 256 | 319 |
return goal_idx; |
Also available in: Unified diff