2018 revision to classic DataBus AVC code.
Dependencies: LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell
Updater.cpp
- Committer:
- shimniok
- Date:
- 2018-12-21
- Revision:
- 25:b8176ebb96c6
- Parent:
- 15:35c40765f7c3
- Child:
- 26:2dc31a801cc8
File content as of revision 25:b8176ebb96c6:
#include "Updater.h" #include "pinouts.h" #include "IncrementalEncoder.h" Updater::Updater() { thisTime = 0; lastTime = 0; t = new Timer; t->start(); _callback = 0; } Updater *Updater::instance() { static Updater instance; return &instance; } void Updater::attach(Callback<void()> cb) { _callback = cb; return; } void Updater::update() { static L3G4200D gyro(I2CSDA, I2CSCL); // TODO parameterize static IncrementalEncoder enc(ALEFT); // Compute dt thisTime = t->read_us(); _dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0 lastTime = thisTime; // Read encoders _ecount += enc.read(); // Read gyro gyro.read(_gyro); //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 if (_callback) _callback(); return; } void Updater::gyro(int g[3], float& dt) { for (int i=0; i < 3; i++) { g[i] = _gyro[i]; } dt = _dt; return; } int Updater::encoder() { int result=_ecount; _ecount = 0; return result; }