64 lines
1.3 KiB
C
64 lines
1.3 KiB
C
|
|
#include "control.h"
|
|
#include "lookahead.h"
|
|
|
|
#include <stdio.h>
|
|
|
|
#define LATENCY 2
|
|
|
|
void trial_loop(trial *t) {
|
|
do {
|
|
if (-1 == trial_wait_for_start(t)) return;
|
|
if (t->finished) break;
|
|
if (t->runcnt == 0) {
|
|
goradar(t);
|
|
vehicle_accel(t);
|
|
switch (t->vehicle.turn) {
|
|
case TURN_LEFT:
|
|
case TURN_STRAIGHT:
|
|
vehicle_hard_left(t); break;
|
|
case TURN_RIGHT:
|
|
vehicle_hard_right(t); break;
|
|
default: break;
|
|
}
|
|
while (t->telemetry.length < 5) {
|
|
if (-1 == trial_wait_for_input(t)) return;
|
|
}
|
|
fit_parameter( (telemetry*) g_queue_peek_nth(&t->telemetry,2),
|
|
(telemetry*) g_queue_peek_nth(&t->telemetry,3),
|
|
(telemetry*) g_queue_peek_nth(&t->telemetry,4),
|
|
&t->map);
|
|
fprintf(stderr,"Fitparameter: a: %f aw: %f k: %f\n",t->map.a,t->map.aw,t->map.k);
|
|
|
|
}
|
|
while (t->alive) {
|
|
radar_dgl(t, getcurts(t) + LATENCY);
|
|
// goradar(t);
|
|
vehicle_break(t);
|
|
vehicle_hard_right(t);
|
|
if (-1 == trial_check_input(t)) return;
|
|
// if (-1 == trial_wait_for_input(t)) return;
|
|
}
|
|
trial_reset_run(t);
|
|
} while (!t->finished);
|
|
}
|
|
|
|
int main(int argc, char **argv) {
|
|
trial *t;
|
|
|
|
if (argc <= 2) {
|
|
fprintf(stderr, "Syntax: %s hostname port\n", argv[0]);
|
|
return 1;
|
|
}
|
|
|
|
if (NULL == (t = trial_new(argv[1], argv[2]))) {
|
|
return 2;
|
|
}
|
|
|
|
trial_loop(t);
|
|
|
|
trial_free(t);
|
|
|
|
return 0;
|
|
}
|