Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

Updater.cpp@26:2dc31a801cc8, 2018-12-21 (annotated)

Committer:
shimniok
Date:
Fri Dec 21 21:23:53 2018 +0000
Revision:
26:2dc31a801cc8
Parent:
25:b8176ebb96c6
Child:
32:eb673f6f5734
replaced shared hi prio event queue (which starts its own thread). Attempt to fix timer overflow

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 11:8ec858b7c6d1 1 #include "Updater.h"
shimniok 14:1dd83e626153 2 #include "pinouts.h"
shimniok 14:1dd83e626153 3 #include "IncrementalEncoder.h"
shimniok 11:8ec858b7c6d1 4
shimniok 25:b8176ebb96c6 5 Updater::Updater() {
shimniok 25:b8176ebb96c6 6 thisTime = 0;
shimniok 26:2dc31a801cc8 7 lastTime = 0;
shimniok 25:b8176ebb96c6 8 t = new Timer;
shimniok 25:b8176ebb96c6 9 t->start();
shimniok 25:b8176ebb96c6 10 _callback = 0;
shimniok 25:b8176ebb96c6 11 }
shimniok 25:b8176ebb96c6 12
shimniok 12:3cd91e150d9c 13 Updater *Updater::instance()
shimniok 12:3cd91e150d9c 14 {
shimniok 11:8ec858b7c6d1 15 static Updater instance;
shimniok 11:8ec858b7c6d1 16
shimniok 11:8ec858b7c6d1 17 return &instance;
shimniok 11:8ec858b7c6d1 18 }
shimniok 11:8ec858b7c6d1 19
shimniok 11:8ec858b7c6d1 20
shimniok 25:b8176ebb96c6 21 void Updater::attach(Callback<void()> cb) {
shimniok 25:b8176ebb96c6 22 _callback = cb;
shimniok 15:35c40765f7c3 23
shimniok 15:35c40765f7c3 24 return;
shimniok 11:8ec858b7c6d1 25 }
shimniok 11:8ec858b7c6d1 26
shimniok 11:8ec858b7c6d1 27
shimniok 11:8ec858b7c6d1 28 void Updater::update()
shimniok 11:8ec858b7c6d1 29 {
shimniok 14:1dd83e626153 30 static L3G4200D gyro(I2CSDA, I2CSCL); // TODO parameterize
shimniok 14:1dd83e626153 31 static IncrementalEncoder enc(ALEFT);
shimniok 11:8ec858b7c6d1 32
shimniok 11:8ec858b7c6d1 33 // Compute dt
shimniok 11:8ec858b7c6d1 34 thisTime = t->read_us();
shimniok 26:2dc31a801cc8 35 _dt = ((float) thisTime - (float) lastTime) / 1000.0;
shimniok 26:2dc31a801cc8 36 if (lastTime > thisTime) _dt = -_dt; // attempt to fix rollover
shimniok 11:8ec858b7c6d1 37 lastTime = thisTime;
shimniok 26:2dc31a801cc8 38
shimniok 11:8ec858b7c6d1 39 // Read encoders
shimniok 14:1dd83e626153 40 _ecount += enc.read();
shimniok 14:1dd83e626153 41
shimniok 11:8ec858b7c6d1 42 // Read gyro
shimniok 12:3cd91e150d9c 43 gyro.read(_gyro);
shimniok 11:8ec858b7c6d1 44
shimniok 11:8ec858b7c6d1 45 //gyro[_z_] = (g_sign[_z_] * (g[_z_] - g_offset[_z_])) / g_scale[_z_];
shimniok 11:8ec858b7c6d1 46
shimniok 11:8ec858b7c6d1 47 // Save current data into history fifo to use 1 second from now
shimniok 11:8ec858b7c6d1 48 //history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled
shimniok 11:8ec858b7c6d1 49 //history[now].gyro = sensors.gyro[_z_]; // current raw gyro data
shimniok 11:8ec858b7c6d1 50 //history[now].dt = dt; // current dt, to address jitter
shimniok 11:8ec858b7c6d1 51 //history[now].hdg = clamp360( history[prev].hdg + dt*(history[now].gyro) ); // compute current heading from current gyro
shimniok 11:8ec858b7c6d1 52 //float r = PI/180 * history[now].hdg;
shimniok 11:8ec858b7c6d1 53 //history[now].x = history[prev].x + history[now].dist * sin(r); // update current position
shimniok 11:8ec858b7c6d1 54 //history[now].y = history[prev].y + history[now].dist * cos(r);
shimniok 11:8ec858b7c6d1 55
shimniok 11:8ec858b7c6d1 56 // Convert this into some kind of status message/event thingy
shimniok 25:b8176ebb96c6 57
shimniok 25:b8176ebb96c6 58 if (_callback)
shimniok 25:b8176ebb96c6 59 _callback();
shimniok 11:8ec858b7c6d1 60
shimniok 11:8ec858b7c6d1 61 return;
shimniok 15:35c40765f7c3 62 }
shimniok 15:35c40765f7c3 63
shimniok 15:35c40765f7c3 64
shimniok 15:35c40765f7c3 65 void Updater::gyro(int g[3], float& dt)
shimniok 15:35c40765f7c3 66 {
shimniok 15:35c40765f7c3 67 for (int i=0; i < 3; i++) {
shimniok 15:35c40765f7c3 68 g[i] = _gyro[i];
shimniok 15:35c40765f7c3 69 }
shimniok 15:35c40765f7c3 70 dt = _dt;
shimniok 15:35c40765f7c3 71
shimniok 15:35c40765f7c3 72 return;
shimniok 15:35c40765f7c3 73 }
shimniok 15:35c40765f7c3 74
shimniok 15:35c40765f7c3 75
shimniok 15:35c40765f7c3 76 int Updater::encoder()
shimniok 15:35c40765f7c3 77 {
shimniok 15:35c40765f7c3 78 int result=_ecount;
shimniok 15:35c40765f7c3 79 _ecount = 0;
shimniok 15:35c40765f7c3 80
shimniok 15:35c40765f7c3 81 return result;
shimniok 15:35c40765f7c3 82 }
shimniok 15:35c40765f7c3 83