2018 revision to classic DataBus AVC code.
Dependencies: LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell
Diff: main.cpp
- Revision:
- 10:9fb3feb38746
- Parent:
- 9:fc3575d2cbbf
- Child:
- 11:8ec858b7c6d1
diff -r fc3575d2cbbf -r 9fb3feb38746 main.cpp --- a/main.cpp Fri Dec 07 15:58:41 2018 +0000 +++ b/main.cpp Mon Dec 10 20:12:41 2018 +0000 @@ -20,10 +20,16 @@ Config config; SimpleShell sh; -L3G4200D gyro(p9, p10); DigitalOut led1(LED1); +DigitalOut led2(LED2); +Thread thread; -Thread thread; +typedef struct { + int g[3]; // gyro + float dt; // delta time +} sensor_data_t; + +sensor_data_t data; /******** SHELL COMMANDS ********/ @@ -33,21 +39,39 @@ void read_gyro() { - int g[3]; - gyro.read(g); - printf("Gyro: %d, %d, %d\n", g[0], g[1], g[2]); + printf("Gyro: %d, %d, %d %f\n", data.g[0], data.g[1], data.g[2], data.dt); } +/********** PERIODIC FUNCTIONS ***********/ +L3G4200D gyro(p9, p10); +Timer timer; +int thisTime = 0; +int lastTime = -1; -/******** GYRO UPDATER ********/ -void gyro_update() +void periodic() { - int g[3]; - - while (1) { - gyro.read(g); - // send message / add to status fifo? - } + // Compute dt + thisTime = timer.read_us(); + data.dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0 + lastTime = thisTime; + + // Read encoders + // Read gyro + gyro.read(data.g); + //gyro[_z_] = (g_sign[_z_] * (g[_z_] - g_offset[_z_])) / g_scale[_z_]; + + // Save current data into history fifo to use 1 second from now + //history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled + //history[now].gyro = sensors.gyro[_z_]; // current raw gyro data + //history[now].dt = dt; // current dt, to address jitter + //history[now].hdg = clamp360( history[prev].hdg + dt*(history[now].gyro) ); // compute current heading from current gyro + //float r = PI/180 * history[now].hdg; + //history[now].x = history[prev].x + history[now].dist * sin(r); // update current position + //history[now].y = history[prev].y + history[now].dist * cos(r); + + led2 = !led2; + + return; } @@ -56,12 +80,14 @@ // main() runs in its own thread in the OS int main() { + EventQueue *queue = mbed_highprio_event_queue(); + + timer.start(); + printf("Bootup...\n"); fflush(stdout); printf("Loading config...\n"); - - // Add valid keywords config.add("intercept_distance", Config::DOUBLE); config.add("waypoint_threshold", Config::DOUBLE); config.add("minimum_turning_radius", Config::DOUBLE); @@ -87,12 +113,20 @@ if (config.load("/etc/2018cfg.txt")) { printf("error loading config\n"); } - + + // Startup shell sh.attach(test, "test"); sh.attach(read_gyro, "gyro"); - thread.start(callback(&sh, &SimpleShell::run)); + //ticker.attach(callback(periodic), 0.010); // call periodic every 10ms + + // schedule calls to periodic at 20 Hz + Event<void()> event(queue, periodic); + event.period(50); + event.post(); + queue->dispatch_forever(); + /* FILE *fp; char buf[128];