Revision 0:260ca1f1cba7, committed 2018-05-30

Comitter:

dralisz82

Date:

Wed May 30 15:10:20 2018 +0000

Commit message:

Controller firmware for a mobile robot, having a K64F MCU on board.; ; See README.md for details;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/demo.cpp	Wed May 30 15:10:20 2018 +0000
@@ -0,0 +1,58 @@
+#include "demo.h"
+
+Demo::Demo() {}
+Demo::Demo(Drive* drive, Lights* lights) {
+    this->drive = drive;
+    this->lights = lights;
+
+    demoThread = 0;
+    // initialize flags
+    f_run = false;
+    printf("Demo created\n");
+}
+
+Demo::~Demo() {
+    delete demoThread;
+}
+
+void Demo::demoThread_main(void const *argument) {
+    Demo* self = (Demo*)argument;
+    printf("Demo thread started\n");
+    
+    Thread::wait(10000);
+    self->lights->headLightOn();
+    Thread::wait(1000);
+        self->drive->forward();
+        Thread::wait(500);
+    while(true) {
+    while(self->f_run) {
+        self->drive->forward();
+        self->drive->steerLeft();
+        Thread::wait(1000);
+        self->drive->stop();
+        Thread::wait(500);
+        self->drive->steerRight();
+        self->drive->backward();
+        Thread::wait(1500);
+        self->drive->stop();
+        Thread::wait(500);
+    }
+        printf("Demo thread paused\n");
+        Thread::wait(1000);
+    }
+}
+
+void Demo::playPause() {
+    f_run = !f_run;
+    if(demoThread == 0) {
+        printf("Launch Demo thread\n");
+        demoThread = new Thread(demoThread_main, this);    
+    }
+}
+
+void Demo::reset() {
+    delete demoThread;
+    f_run = false;
+    printf("Demo thread terminated\n");
+    lights->headLightOff();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/demo.h	Wed May 30 15:10:20 2018 +0000
@@ -0,0 +1,27 @@
+#ifndef demo_H
+#define demo_H
+
+#include "mbed.h"
+#include "rtos.h"
+#include "drive.h"
+#include "lights.h"
+
+class Demo {
+public:
+    Demo();
+    Demo(Drive* _drive, Lights* _lights);
+    ~Demo();
+    
+    void playPause();
+    void reset();
+
+private:
+    bool f_run;
+    
+    Drive* drive;
+    Lights* lights;
+    Thread *demoThread;
+    static void demoThread_main(void const *argument);
+};
+    
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/drive.cpp	Wed May 30 15:10:20 2018 +0000
@@ -0,0 +1,71 @@
+#include "drive.h"
+
+Drive::Drive() {}
+Drive::Drive(PinName pF, PinName pB, PinName pL, PinName pR, Lights* lights) {
+    // create Pwm and DigitalOut objects
+    po_forward = new PwmOut(pF);
+    po_backward = new PwmOut(pB);
+    do_steerLeft = new DigitalOut(pL);
+    do_steerRight = new DigitalOut(pR);
+    
+    this->lights = lights;
+    autoIndex = (lights)?true:false;
+    
+    po_forward->period(0.0001); // 10kHz
+    po_backward->period(0.0001); // 10kHz
+
+    printf("Drive created\n");
+}
+
+Drive::~Drive() {
+    delete po_forward;
+    delete po_backward;
+    delete do_steerLeft;
+    delete do_steerRight;
+}
+
+void Drive::forward() {
+    if(lights)
+        lights->reversingLightOff();
+    po_backward->write(0.0f);
+    po_forward->write(0.85f);
+}
+
+void Drive::backward() {
+    if(lights)
+        lights->reversingLightOn();
+    po_forward->write(0.0f);
+    po_backward->write(0.99f);
+}
+
+void Drive::stop() {
+    if(lights)
+        lights->reversingLightOff();
+    po_forward->write(0.0f);
+    po_backward->write(0.0f);
+}
+
+void Drive::steerLeft() {
+    if(lights && autoIndex)
+        lights->indexLeft();
+    do_steerRight->write(0);
+    do_steerLeft->write(1);
+}
+
+void Drive::steerRight() {
+    if(lights && autoIndex)
+        lights->indexRight();
+    do_steerLeft->write(0);
+    do_steerRight->write(1);
+}
+
+void Drive::steerStraight() {
+    if(lights && autoIndex)
+        lights->indexOff();
+    do_steerLeft->write(0);
+    do_steerRight->write(0);
+}
+
+void Drive::setAutoIndex(bool autoIndex) {
+    this->autoIndex = autoIndex;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/drive.h	Wed May 30 15:10:20 2018 +0000
@@ -0,0 +1,31 @@
+#ifndef drive_H
+#define drive_H
+
+#include "mbed.h"
+#include "lights.h"
+
+class Drive {
+public:
+    Drive();
+    Drive(PinName pF, PinName pB, PinName pL, PinName pR, Lights* lights=NULL);
+    ~Drive();
+    
+    void forward();
+    void backward();
+    void stop();
+    void steerLeft();
+    void steerRight();
+    void steerStraight();
+    
+    void setAutoIndex(bool autoIndex);
+
+private:
+    PwmOut* po_forward;
+    PwmOut* po_backward;
+    DigitalOut* do_steerLeft;
+    DigitalOut* do_steerRight;
+    Lights* lights;
+    bool autoIndex;
+};
+    
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lights.cpp	Wed May 30 15:10:20 2018 +0000
@@ -0,0 +1,113 @@
+#include "lights.h"
+
+Lights::Lights() {}
+
+Lights::Lights(PinName pIL, PinName pIR, PinName pHL, PinName pHB, PinName pRL, PinName pBL) {
+    // create DigitalOut objects
+    do_indexLeft = new DigitalOut(pIL);
+    do_indexRight = new DigitalOut(pIR);
+    do_headLight = new DigitalOut(pHL);
+    do_highBeam = new DigitalOut(pHB);
+    do_reversingLight = new DigitalOut(pRL);
+    do_brakeLight = new DigitalOut(pBL);
+    
+    // initialize flags
+    f_indexLeft = false;
+    f_indexRight = false;
+    
+    indexThread = new Thread(indexThread_main, this);
+    printf("Lights created\n");
+}
+
+Lights::~Lights() {
+    delete indexThread;
+    delete do_indexLeft;
+    delete do_indexRight;
+    delete do_headLight;
+    delete do_highBeam;
+    delete do_reversingLight;
+    delete do_brakeLight;
+}
+
+/**
+ * blinking indexes
+ */
+void Lights::indexThread_main(void const *argument) {
+    Lights* self = (Lights*)argument;
+    
+    while (true) {
+        if(self->f_indexLeft)
+            self->do_indexLeft->write(!self->do_indexLeft->read());
+        else
+            self->do_indexLeft->write(0);
+        if(self->f_indexRight)
+            self->do_indexRight->write(!self->do_indexRight->read());
+        else
+            self->do_indexRight->write(0);
+        Thread::wait(500);
+    }
+}
+
+void Lights::indexLeft() {
+    f_indexLeft = true;
+    f_indexRight = false;
+}
+
+void Lights::indexRight() {
+    f_indexLeft = false;
+    f_indexRight = true;
+}
+
+void Lights::indexOff() {
+    f_indexLeft = false;
+    f_indexRight = false;
+}
+
+void Lights::hazardLightsOn() {
+    f_indexLeft = true;
+    f_indexRight = true;
+}
+
+void Lights::hazardLightsOff() {
+    indexOff();
+}
+
+void Lights::headLightOn() {
+    do_headLight->write(1);
+}
+
+void Lights::headLightOff() {
+    do_headLight->write(0);
+}
+
+void Lights::headLightToggle() {
+    do_headLight->write(!do_headLight->read());
+}
+
+void Lights::highBeamOn() {
+    do_highBeam->write(1);
+}
+
+void Lights::highBeamOff() {
+    do_highBeam->write(0);
+}
+
+void Lights::highBeamToggle() {
+    do_highBeam->write(!do_highBeam->read());
+}
+
+void Lights::reversingLightOn() {
+    do_reversingLight->write(1);
+}
+
+void Lights::reversingLightOff() {
+    do_reversingLight->write(0);
+}
+
+void Lights::brakeLightOn() {
+    do_brakeLight->write(1);
+}
+
+void Lights::brakeLightOff() {
+    do_brakeLight->write(0);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lights.h	Wed May 30 15:10:20 2018 +0000
@@ -0,0 +1,49 @@
+#ifndef lights_H
+#define lights_H
+
+#include "mbed.h"
+#include "rtos.h"
+
+class Lights {
+public:
+    Lights();
+    Lights(PinName pIL, PinName pIR, PinName pHL, PinName pHB, PinName pRL, PinName pBL);
+    ~Lights();
+
+    void indexLeft();
+    void indexRight();
+    void indexOff();
+
+    void hazardLightsOn();
+    void hazardLightsOff();
+
+    void headLightOn();
+    void headLightOff();
+    void headLightToggle();
+
+    void highBeamOn();
+    void highBeamOff();
+    void highBeamToggle();
+
+    void reversingLightOn();
+    void reversingLightOff();
+
+    void brakeLightOn();
+    void brakeLightOff();
+
+private:
+    bool f_indexLeft;
+    bool f_indexRight;
+
+    DigitalOut* do_indexLeft;
+    DigitalOut* do_indexRight;
+    DigitalOut* do_headLight;
+    DigitalOut* do_highBeam;
+    DigitalOut* do_reversingLight;
+    DigitalOut* do_brakeLight;
+    
+    Thread *indexThread;
+    static void indexThread_main(void const *argument);
+};
+    
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sensors.cpp	Wed May 30 15:10:20 2018 +0000
@@ -0,0 +1,164 @@
+#include "sensors.h"
+
+extern Sensors* sensors;
+
+Sensor::Sensor() {
+}
+
+Sensor::~Sensor() {
+}
+
+void Sensor::setId(char *sId) {
+    strcpy(this->sId, sId);
+}
+
+void Sensor::setName(char *name) {
+    strcpy(this->name, name);
+}
+
+void Sensor::setMetric(char *metric) {
+    strcpy(this->metric, metric);
+}
+
+void Sensor::setFunction(float (*function)(void)) {
+    this->function = function;
+}
+
+char* Sensor::getId() {
+    return sId;
+}
+
+char* Sensor::getName() {
+    return name;
+}
+
+char* Sensor::getMetric() {
+    return metric;
+}
+
+float Sensor::readValue() {
+    return (*function)();
+}
+
+Sensors::Sensors() {
+    // create peripheral objects
+    analogInputs[0] = new AnalogIn(PTB2);
+    analogInputs[1] = new AnalogIn(PTB3);
+    analogInputs[2] = new AnalogIn(PTB10);
+    analogInputs[3] = new AnalogIn(PTB11);
+    analogInputs[4] = new AnalogIn(PTC11);
+    analogInputs[5] = new AnalogIn(PTC10);
+    
+    sensNum = 0;
+    createS1();
+    createS2();
+    createS3();
+    createVB();
+    createVM();
+    createVL();
+}
+
+Sensors::~Sensors() {
+    for(int i=0;i<=5;i++)
+        delete analogInputs[i];
+}
+
+Sensor* Sensors::getSensor(char *sId) {
+    if(!strcmp(sId, "vBattS1"))
+        return &sensArr[SENS_VBATTS1];
+    else if(!strcmp(sId, "vBattS2"))
+        return &sensArr[SENS_VBATTS2];
+    else if(!strcmp(sId, "vBattS3"))
+        return &sensArr[SENS_VBATTS3];
+    else if(!strcmp(sId, "vBatt"))
+        return &sensArr[SENS_VBATT];
+    else if(!strcmp(sId, "vMotor"))
+        return &sensArr[SENS_VMOTOR];
+    else if(!strcmp(sId, "vLogic"))
+        return &sensArr[SENS_VLOGIC];
+    return NULL;
+}
+
+AnalogIn* Sensors::getAnalogIn(int aiId) {
+    if(aiId >= 0 && aiId <= 5)
+        return analogInputs[aiId];
+    return NULL;
+}
+
+float readS1() {
+    float factor = 3.3;// TODO
+    return sensors->getAnalogIn(0)->read()*factor;
+}
+
+void Sensors::createS1() {
+    Sensor *s = &sensArr[sensNum++];
+    s->setId("vBattS1");
+    s->setName("battery cell #1 voltage");
+    s->setMetric("V");
+    s->setFunction(readS1);
+}
+
+float readS2() {
+    float factor = 3.3;// TODO
+    return (sensors->getAnalogIn(1)->read() - sensors->getAnalogIn(0)->read())*factor;
+}
+
+void Sensors::createS2() {
+    Sensor *s = &sensArr[sensNum++];
+    s->setId("vBattS2");
+    s->setName("battery cell #2 voltage");
+    s->setMetric("V");
+    s->setFunction(readS2);
+}
+
+float readS3() {
+    float factor = 3.3;// TODO
+    return (sensors->getAnalogIn(2)->read() - sensors->getAnalogIn(1)->read())*factor;
+}
+
+void Sensors::createS3() {
+    Sensor *s = &sensArr[sensNum++];
+    s->setId("vBattS3");
+    s->setName("battery cell #3 voltage");
+    s->setMetric("V");
+    s->setFunction(readS3);
+}
+
+float readVB() {
+    float factor = 3.3;// TODO
+    return sensors->getAnalogIn(2)->read()*factor;
+}
+
+void Sensors::createVB() {
+    Sensor *s = &sensArr[sensNum++];
+    s->setId("vBatt");
+    s->setName("battery voltage");
+    s->setMetric("V");
+    s->setFunction(readVB);
+}
+
+float readVM() {
+    float factor = 3.3;// TODO
+    return sensors->getAnalogIn(3)->read()*factor;
+}
+
+void Sensors::createVM() {
+    Sensor *s = &sensArr[sensNum++];
+    s->setId("vMotor");
+    s->setName("motor voltage");
+    s->setMetric("V");
+    s->setFunction(readVM);
+}
+
+float readVL() {
+    float factor = 3.3;// TODO
+    return sensors->getAnalogIn(4)->read()*factor;
+}
+
+void Sensors::createVL() {
+    Sensor *s = &sensArr[sensNum++];
+    s->setId("vLogic");
+    s->setName("logic voltage");
+    s->setMetric("V");
+    s->setFunction(readVL);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sensors.h	Wed May 30 15:10:20 2018 +0000
@@ -0,0 +1,59 @@
+#ifndef sensors_H
+#define sensors_H
+
+#include "mbed.h"
+#include <FunctionPointer.h>
+#define MAX_NUM_SENS 10
+#define SENS_VBATTS1 0
+#define SENS_VBATTS2 1
+#define SENS_VBATTS3 2
+#define SENS_VBATT 3
+#define SENS_VMOTOR 4
+#define SENS_VLOGIC 5
+
+
+class Sensors;
+
+class Sensor {
+public:
+    Sensor();
+    Sensor(char *sId, char *name, char *metric);
+    ~Sensor();
+    void setId(char *sId);
+    void setName(char *name);
+    void setMetric(char *metric);
+    void setFunction(float (*function)(void) = 0);
+    char *getId();
+    char *getName();
+    char * getMetric();
+    float readValue();
+    
+private:
+    char sId[10];
+    char name[30];
+    char metric[4]; // max 3 characters + terminating 0
+    float (*function)(void);
+};
+
+class Sensors {
+public:
+    Sensors();
+    ~Sensors();
+    Sensor* getSensor(char *sId);
+    AnalogIn* getAnalogIn(int aiId);
+
+private:
+    void createS1();
+    void createS2();
+    void createS3();
+    void createVB();
+    void createVM();
+    void createVL();
+    
+    Sensor sensArr[MAX_NUM_SENS];
+    int sensNum;
+    
+    AnalogIn* analogInputs[6];
+};
+    
+#endif