2018 revision to classic DataBus AVC code.
Dependencies: LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell
Updater.cpp@13:5566df1250f1, 2018-12-12 (annotated)
- Committer:
- shimniok
- Date:
- Wed Dec 12 17:38:02 2018 +0000
- Revision:
- 13:5566df1250f1
- Parent:
- 12:3cd91e150d9c
- Child:
- 14:1dd83e626153
compute and pass back dt from updater
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 11:8ec858b7c6d1 | 1 | #include "Updater.h" |
shimniok | 11:8ec858b7c6d1 | 2 | |
shimniok | 11:8ec858b7c6d1 | 3 | |
shimniok | 13:5566df1250f1 | 4 | void Updater::gyro(int g[3], float& dt) |
shimniok | 12:3cd91e150d9c | 5 | { |
shimniok | 12:3cd91e150d9c | 6 | for (int i=0; i < 3; i++) { |
shimniok | 12:3cd91e150d9c | 7 | g[i] = _gyro[i]; |
shimniok | 12:3cd91e150d9c | 8 | } |
shimniok | 12:3cd91e150d9c | 9 | |
shimniok | 13:5566df1250f1 | 10 | dt = _dt; |
shimniok | 13:5566df1250f1 | 11 | |
shimniok | 12:3cd91e150d9c | 12 | return; |
shimniok | 12:3cd91e150d9c | 13 | } |
shimniok | 12:3cd91e150d9c | 14 | |
shimniok | 12:3cd91e150d9c | 15 | |
shimniok | 12:3cd91e150d9c | 16 | Updater *Updater::instance() |
shimniok | 12:3cd91e150d9c | 17 | { |
shimniok | 11:8ec858b7c6d1 | 18 | static Updater instance; |
shimniok | 11:8ec858b7c6d1 | 19 | |
shimniok | 11:8ec858b7c6d1 | 20 | return &instance; |
shimniok | 11:8ec858b7c6d1 | 21 | } |
shimniok | 11:8ec858b7c6d1 | 22 | |
shimniok | 11:8ec858b7c6d1 | 23 | |
shimniok | 11:8ec858b7c6d1 | 24 | void Updater::start(int interval_ms) |
shimniok | 11:8ec858b7c6d1 | 25 | { |
shimniok | 11:8ec858b7c6d1 | 26 | Timer timer; |
shimniok | 11:8ec858b7c6d1 | 27 | |
shimniok | 11:8ec858b7c6d1 | 28 | thisTime = 0; |
shimniok | 11:8ec858b7c6d1 | 29 | lastTime = 0; |
shimniok | 11:8ec858b7c6d1 | 30 | t = &timer; |
shimniok | 11:8ec858b7c6d1 | 31 | t->start(); |
shimniok | 11:8ec858b7c6d1 | 32 | |
shimniok | 11:8ec858b7c6d1 | 33 | EventQueue *queue = mbed_highprio_event_queue(); |
shimniok | 11:8ec858b7c6d1 | 34 | Event<void()> event(queue, callback(this, &Updater::update)); |
shimniok | 11:8ec858b7c6d1 | 35 | event.period(interval_ms); |
shimniok | 11:8ec858b7c6d1 | 36 | event.post(); |
shimniok | 11:8ec858b7c6d1 | 37 | queue->dispatch_forever(); |
shimniok | 11:8ec858b7c6d1 | 38 | } |
shimniok | 11:8ec858b7c6d1 | 39 | |
shimniok | 11:8ec858b7c6d1 | 40 | |
shimniok | 11:8ec858b7c6d1 | 41 | void Updater::update() |
shimniok | 11:8ec858b7c6d1 | 42 | { |
shimniok | 11:8ec858b7c6d1 | 43 | static DigitalOut led2(LED2); |
shimniok | 11:8ec858b7c6d1 | 44 | static L3G4200D gyro(p9, p10); // TODO parameterize |
shimniok | 11:8ec858b7c6d1 | 45 | |
shimniok | 11:8ec858b7c6d1 | 46 | // Compute dt |
shimniok | 11:8ec858b7c6d1 | 47 | thisTime = t->read_us(); |
shimniok | 13:5566df1250f1 | 48 | _dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0 |
shimniok | 11:8ec858b7c6d1 | 49 | lastTime = thisTime; |
shimniok | 11:8ec858b7c6d1 | 50 | |
shimniok | 11:8ec858b7c6d1 | 51 | // Read encoders |
shimniok | 11:8ec858b7c6d1 | 52 | // Read gyro |
shimniok | 12:3cd91e150d9c | 53 | gyro.read(_gyro); |
shimniok | 11:8ec858b7c6d1 | 54 | |
shimniok | 11:8ec858b7c6d1 | 55 | //gyro[_z_] = (g_sign[_z_] * (g[_z_] - g_offset[_z_])) / g_scale[_z_]; |
shimniok | 11:8ec858b7c6d1 | 56 | |
shimniok | 11:8ec858b7c6d1 | 57 | // Save current data into history fifo to use 1 second from now |
shimniok | 11:8ec858b7c6d1 | 58 | //history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled |
shimniok | 11:8ec858b7c6d1 | 59 | //history[now].gyro = sensors.gyro[_z_]; // current raw gyro data |
shimniok | 11:8ec858b7c6d1 | 60 | //history[now].dt = dt; // current dt, to address jitter |
shimniok | 11:8ec858b7c6d1 | 61 | //history[now].hdg = clamp360( history[prev].hdg + dt*(history[now].gyro) ); // compute current heading from current gyro |
shimniok | 11:8ec858b7c6d1 | 62 | //float r = PI/180 * history[now].hdg; |
shimniok | 11:8ec858b7c6d1 | 63 | //history[now].x = history[prev].x + history[now].dist * sin(r); // update current position |
shimniok | 11:8ec858b7c6d1 | 64 | //history[now].y = history[prev].y + history[now].dist * cos(r); |
shimniok | 11:8ec858b7c6d1 | 65 | |
shimniok | 11:8ec858b7c6d1 | 66 | // Convert this into some kind of status message/event thingy |
shimniok | 11:8ec858b7c6d1 | 67 | led2 = !led2; |
shimniok | 11:8ec858b7c6d1 | 68 | |
shimniok | 11:8ec858b7c6d1 | 69 | return; |
shimniok | 11:8ec858b7c6d1 | 70 | } |