2018 revision to classic DataBus AVC code.
Dependencies: LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell
Diff: Updater.cpp
- Revision:
- 11:8ec858b7c6d1
- Child:
- 12:3cd91e150d9c
diff -r 9fb3feb38746 -r 8ec858b7c6d1 Updater.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Updater.cpp Wed Dec 12 17:20:16 2018 +0000 @@ -0,0 +1,59 @@ +#include "Updater.h" + + +Updater *Updater::instance() { + static Updater instance; + + return &instance; +} + + +void Updater::start(int interval_ms) +{ + Timer timer; + + thisTime = 0; + lastTime = 0; + t = &timer; + t->start(); + + EventQueue *queue = mbed_highprio_event_queue(); + Event<void()> event(queue, callback(this, &Updater::update)); + event.period(interval_ms); + event.post(); + queue->dispatch_forever(); +} + + +void Updater::update() +{ + static DigitalOut led2(LED2); + static L3G4200D gyro(p9, p10); // TODO parameterize + + // Compute dt + thisTime = t->read_us(); + //data.dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0 + lastTime = thisTime; + + // Read encoders + // Read gyro + int g_a[3]; + gyro.read(g_a); + g = g_a[2]; + + //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); + + // Convert this into some kind of status message/event thingy + led2 = !led2; + + return; +} \ No newline at end of file