70 lines
1.5 KiB
C
70 lines
1.5 KiB
C
|
|
#include "control.h"
|
|
#include "lookahead.h"
|
|
|
|
#include <stdio.h>
|
|
|
|
#define LATENCY 2
|
|
|
|
void trial_loop(trial *t) {
|
|
FILE* sim;
|
|
char name[80];
|
|
do {
|
|
sprintf(name,"simulation%d.m",t->runcnt);
|
|
sim = fopen(name,"w");
|
|
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 < 6) {
|
|
if (-1 == trial_wait_for_input(t)) return;
|
|
}
|
|
fit_parameter( (telemetry*) g_queue_peek_nth(&t->telemetry,3),
|
|
(telemetry*) g_queue_peek_nth(&t->telemetry,4),
|
|
(telemetry*) g_queue_peek_nth(&t->telemetry,5),
|
|
&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);
|
|
vehicle_matlab(sim,t->sim.tm.ts,&t->sim.tm.vehicle);
|
|
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_matlab(t);
|
|
trial_reset_run(t);
|
|
fclose(sim);
|
|
} 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;
|
|
}
|