This commit is contained in:
Stefan Bühler 2008-07-14 00:33:51 +02:00
parent 9ceefe2ec2
commit b92a9a3034
2 changed files with 8 additions and 6 deletions

View File

@ -93,7 +93,7 @@ void sim_update(trial* t){
fixangle(t->sim.tm.vehicle.dir - t->vehicle.dir),
t->sim.steps);
// fprintf(stderr, "w = %f, dir = %f\n", t->sim.tm.vehicle.w, t->sim.tm.vehicle.dir);
fprintf(stderr, "speed = %f\n", t->sim.tm.vehicle.speed);
// fprintf(stderr, "speed = %f\n", t->sim.tm.vehicle.speed);
/*update simulation vehicle from telemetry: everything and calculate w*/
// tm->vehicle.w = t->sim.tm.vehicle.w;
@ -107,7 +107,7 @@ void sim_update(trial* t){
}
// fprintf(stderr, "w = %f, dir = %f\n", t->sim.tm.vehicle.w, t->sim.tm.vehicle.dir);
fprintf(stderr, "speed = %f\n", t->sim.tm.vehicle.speed);
// fprintf(stderr, "speed = %f\n", t->sim.tm.vehicle.speed);
}
int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat){

View File

@ -129,20 +129,22 @@ void goradar(trial *t) {
}
}
ang = fixangle(ang - v.dir + ve_ho_angle);
// fprintf(stderr, "Angle: %f, objects: %u\n", ang, objects);
ang = fixangle(ang - (v.dir + 30*v.w) + ve_ho_angle);
// fprintf(stderr, "Angle: %f (%f), objects: %u\n", ang, 20*v.w, objects);
if (ang < 1) {
if (ang < -15) {
if (ang < -10) {
vehicle_hard_right(t);
} else {
vehicle_right(t);
}
} else if (ang > 1) {
if (ang > 15) {
if (ang > 10) {
vehicle_hard_left(t);
} else {
vehicle_left(t);
}
} else {
vehicle_straight(t);
}
ang = fabs(ang);
if (ang > 90) {