Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

Updater.cpp@14:1dd83e626153, 2018-12-12 (annotated)

Committer:
shimniok
Date:
Wed Dec 12 18:21:57 2018 +0000
Revision:
14:1dd83e626153
Parent:
13:5566df1250f1
Child:
15:35c40765f7c3
Added encoder read to Updater, added documentation, added shell cmd to read encoder

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 13:5566df1250f1 5 void Updater::gyro(int g[3], float& dt)
shimniok 12:3cd91e150d9c 6 {
shimniok 12:3cd91e150d9c 7 for (int i=0; i < 3; i++) {
shimniok 12:3cd91e150d9c 8 g[i] = _gyro[i];
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 14:1dd83e626153 15 int Updater::encoder()
shimniok 14:1dd83e626153 16 {
shimniok 14:1dd83e626153 17 int result=_ecount;
shimniok 14:1dd83e626153 18 _ecount = 0;
shimniok 14:1dd83e626153 19
shimniok 14:1dd83e626153 20 return result;
shimniok 14:1dd83e626153 21 }
shimniok 12:3cd91e150d9c 22
shimniok 12:3cd91e150d9c 23 Updater *Updater::instance()
shimniok 12:3cd91e150d9c 24 {
shimniok 11:8ec858b7c6d1 25 static Updater instance;
shimniok 11:8ec858b7c6d1 26
shimniok 11:8ec858b7c6d1 27 return &instance;
shimniok 11:8ec858b7c6d1 28 }
shimniok 11:8ec858b7c6d1 29
shimniok 11:8ec858b7c6d1 30
shimniok 11:8ec858b7c6d1 31 void Updater::start(int interval_ms)
shimniok 11:8ec858b7c6d1 32 {
shimniok 11:8ec858b7c6d1 33 Timer timer;
shimniok 11:8ec858b7c6d1 34
shimniok 11:8ec858b7c6d1 35 thisTime = 0;
shimniok 11:8ec858b7c6d1 36 lastTime = 0;
shimniok 11:8ec858b7c6d1 37 t = &timer;
shimniok 11:8ec858b7c6d1 38 t->start();
shimniok 11:8ec858b7c6d1 39
shimniok 11:8ec858b7c6d1 40 EventQueue *queue = mbed_highprio_event_queue();
shimniok 11:8ec858b7c6d1 41 Event<void()> event(queue, callback(this, &Updater::update));
shimniok 11:8ec858b7c6d1 42 event.period(interval_ms);
shimniok 11:8ec858b7c6d1 43 event.post();
shimniok 11:8ec858b7c6d1 44 queue->dispatch_forever();
shimniok 11:8ec858b7c6d1 45 }
shimniok 11:8ec858b7c6d1 46
shimniok 11:8ec858b7c6d1 47
shimniok 11:8ec858b7c6d1 48 void Updater::update()
shimniok 11:8ec858b7c6d1 49 {
shimniok 11:8ec858b7c6d1 50 static DigitalOut led2(LED2);
shimniok 14:1dd83e626153 51 static L3G4200D gyro(I2CSDA, I2CSCL); // TODO parameterize
shimniok 14:1dd83e626153 52 static IncrementalEncoder enc(ALEFT);
shimniok 11:8ec858b7c6d1 53
shimniok 11:8ec858b7c6d1 54 // Compute dt
shimniok 11:8ec858b7c6d1 55 thisTime = t->read_us();
shimniok 13:5566df1250f1 56 _dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0
shimniok 11:8ec858b7c6d1 57 lastTime = thisTime;
shimniok 11:8ec858b7c6d1 58
shimniok 11:8ec858b7c6d1 59 // Read encoders
shimniok 14:1dd83e626153 60 _ecount += enc.read();
shimniok 14:1dd83e626153 61
shimniok 11:8ec858b7c6d1 62 // Read gyro
shimniok 12:3cd91e150d9c 63 gyro.read(_gyro);
shimniok 11:8ec858b7c6d1 64
shimniok 11:8ec858b7c6d1 65 //gyro[_z_] = (g_sign[_z_] * (g[_z_] - g_offset[_z_])) / g_scale[_z_];
shimniok 11:8ec858b7c6d1 66
shimniok 11:8ec858b7c6d1 67 // Save current data into history fifo to use 1 second from now
shimniok 11:8ec858b7c6d1 68 //history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled
shimniok 11:8ec858b7c6d1 69 //history[now].gyro = sensors.gyro[_z_]; // current raw gyro data
shimniok 11:8ec858b7c6d1 70 //history[now].dt = dt; // current dt, to address jitter
shimniok 11:8ec858b7c6d1 71 //history[now].hdg = clamp360( history[prev].hdg + dt*(history[now].gyro) ); // compute current heading from current gyro
shimniok 11:8ec858b7c6d1 72 //float r = PI/180 * history[now].hdg;
shimniok 11:8ec858b7c6d1 73 //history[now].x = history[prev].x + history[now].dist * sin(r); // update current position
shimniok 11:8ec858b7c6d1 74 //history[now].y = history[prev].y + history[now].dist * cos(r);
shimniok 11:8ec858b7c6d1 75
shimniok 11:8ec858b7c6d1 76 // Convert this into some kind of status message/event thingy
shimniok 11:8ec858b7c6d1 77 led2 = !led2;
shimniok 11:8ec858b7c6d1 78
shimniok 11:8ec858b7c6d1 79 return;
shimniok 11:8ec858b7c6d1 80 }