PES2_mbed_os_6

Dependencies:   Servo

Files at this revision

API Documentation at this revision

Comitter:
boro
Date:
Fri Mar 12 13:04:33 2021 +0000
Child:
1:0e396592f33d
Commit message:
controller added;

Changed in this revision

.gitignore Show annotated file Show diff for this revision Revisions of this file
Controller.cpp Show annotated file Show diff for this revision Revisions of this file
Controller.h Show annotated file Show diff for this revision Revisions of this file
EncoderCounter.cpp Show annotated file Show diff for this revision Revisions of this file
EncoderCounter.h Show annotated file Show diff for this revision Revisions of this file
LowpassFilter.cpp Show annotated file Show diff for this revision Revisions of this file
LowpassFilter.h Show annotated file Show diff for this revision Revisions of this file
Servo.lib Show annotated file Show diff for this revision Revisions of this file
ThreadFlag.cpp Show annotated file Show diff for this revision Revisions of this file
ThreadFlag.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-os.lib Show annotated file Show diff for this revision Revisions of this file
mbed_app.json Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/.gitignore	Fri Mar 12 13:04:33 2021 +0000
@@ -0,0 +1,4 @@
+.build
+.mbed
+projectfiles
+*.py*
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Fri Mar 12 13:04:33 2021 +0000
@@ -0,0 +1,129 @@
+#include "Controller.h"
+
+using namespace std;
+
+const float Controller::PERIOD = 0.001f;                    // period of 1 ms
+const float Controller::COUNTS_PER_TURN = 1200.0f;         // encoder resolution
+const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;  // given in [rad/s]
+const float Controller::KN = 40.0f;                         // speed constant in [rpm/V]
+const float Controller::KP = 0.2f;                          // speed control parameter
+const float Controller::MAX_VOLTAGE = 12.0f;                // battery voltage in [V]
+const float Controller::MIN_DUTY_CYCLE = 0.02f;             // minimum duty-cycle
+const float Controller::MAX_DUTY_CYCLE = 0.98f;             // maximum duty-cycle
+
+Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
+                        EncoderCounter& counterLeft, EncoderCounter& counterRight) :
+                        pwmLeft(pwmLeft), pwmRight(pwmRight),
+                        counterLeft(counterLeft), counterRight(counterRight) {
+
+    // Initialisieren der PWM Ausgaenge
+    
+    pwmLeft.period(0.00005f); // PWM Periode von 50 us
+    pwmLeft = 0.5f; // Duty-Cycle von 50%
+    pwmRight.period(0.00005f); // PWM Periode von 50 us
+    pwmRight = 0.5f; // Duty-Cycle von 50%
+
+    // Initialisieren von lokalen Variabeln
+
+    previousValueCounterLeft = counterLeft.read();
+    previousValueCounterRight = counterRight.read();
+
+    speedLeftFilter.setPeriod(PERIOD);
+    speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+
+    speedRightFilter.setPeriod(PERIOD);
+    speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+
+    desiredSpeedLeft = 0.0f;
+    desiredSpeedRight = 0.0f;
+
+    actualSpeedLeft = 0.0f;
+    actualSpeedRight = 0.0f;
+    
+    actualAngleLeft = 0.0f;
+    actualAngleRight = 0.0f;
+
+    // Starten des periodischen Tasks
+    thread.start(callback(this, &Controller::run));
+    ticker.attach(callback(this, &Controller::sendThreadFlag), PERIOD);
+}
+
+Controller::~Controller()
+{
+    ticker.detach(); // Stoppt den periodischen Task
+}
+
+
+void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
+{
+    this->desiredSpeedLeft = desiredSpeedLeft;
+}
+
+void Controller::setDesiredSpeedRight(float desiredSpeedRight)
+{
+    this->desiredSpeedRight = desiredSpeedRight;
+}
+
+float Controller::getSpeedLeft()
+{
+    return actualSpeedLeft;
+}
+
+float Controller::getSpeedRight()
+{
+    return actualSpeedRight;
+}
+
+/**
+ * This method is called by the ticker timer interrupt service routine.
+ * It sends a flag to the thread to make it run again.
+ */
+void Controller::sendThreadFlag() {
+    
+    thread.flags_set(threadFlag);
+}
+
+void Controller::run() {
+
+    while(true) {
+
+        // wait for the periodic signal
+
+        ThisThread::flags_wait_any(threadFlag);
+
+        // calculate actual speed of motors in [rpm]
+
+        short valueCounterLeft = counterLeft.read();
+        short valueCounterRight = counterRight.read();
+
+        short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
+        short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
+
+        previousValueCounterLeft = valueCounterLeft;
+        previousValueCounterRight = valueCounterRight;
+
+        actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
+        actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
+
+        actualAngleLeft = actualAngleLeft + actualSpeedLeft/60.0f*PERIOD;
+        actualAngleRight = actualAngleRight + actualSpeedRight/60.0f*PERIOD;
+
+        // calculate motor phase voltages
+
+        float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
+        float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
+
+        // calculate, limit and set duty cycles
+        
+        float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
+        if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
+        else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
+        pwmLeft.write(dutyCycleLeft);
+
+        float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
+        if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
+        else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
+        pwmRight.write(dutyCycleRight);
+
+    }
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.h	Fri Mar 12 13:04:33 2021 +0000
@@ -0,0 +1,58 @@
+#ifndef CONTROLLER_H_
+#define CONTROLLER_H_
+#include <cstdlib>
+#include <mbed.h>
+#include "EncoderCounter.h"
+#include "LowpassFilter.h"
+#include "ThreadFlag.h"
+
+class Controller
+{
+
+public:
+
+    Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
+               EncoderCounter& counterLeft, EncoderCounter& counterRight);
+
+    virtual ~Controller();
+    void setDesiredSpeedLeft(float desiredSpeedLeft);
+    void setDesiredSpeedRight(float desiredSpeedRight);
+    float getSpeedLeft();
+    float getSpeedRight();
+
+private:
+
+    static const float PERIOD;
+    static const float COUNTS_PER_TURN;
+    static const float LOWPASS_FILTER_FREQUENCY;
+    static const float KN;
+    static const float KP;   
+    static const float MAX_VOLTAGE;
+    static const float MIN_DUTY_CYCLE;
+    static const float MAX_DUTY_CYCLE;
+
+    PwmOut&            pwmLeft;
+    PwmOut&            pwmRight;
+    EncoderCounter&    counterLeft;
+    EncoderCounter&    counterRight;
+    short              previousValueCounterLeft;
+    short              previousValueCounterRight;
+    LowpassFilter      speedLeftFilter;
+    LowpassFilter      speedRightFilter;
+    float              desiredSpeedLeft;
+    float              desiredSpeedRight;
+    float              actualSpeedLeft;
+    float              actualSpeedRight;
+    float               actualAngleLeft;
+    float               actualAngleRight;
+    
+    ThreadFlag          threadFlag;
+    Thread              thread;
+    Ticker              ticker;
+    
+    void                sendThreadFlag();
+    void                run();
+
+};
+
+#endif /* CONTROLLER_H_ */
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.cpp	Fri Mar 12 13:04:33 2021 +0000
@@ -0,0 +1,171 @@
+/*
+ * EncoderCounter.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include "EncoderCounter.h"
+
+using namespace std;
+
+/**
+ * Creates and initializes the driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ * @param a the input pin for the channel A.
+ * @param b the input pin for the channel B.
+ */
+EncoderCounter::EncoderCounter(PinName a, PinName b) {
+    
+    // check pins
+    
+    if ((a == PA_0) && (b == PA_1)) {
+        
+        // pinmap OK for TIM2 CH1 and CH2
+        
+        TIM = TIM2;
+        
+        // configure general purpose I/O registers
+        
+        GPIOA->MODER &= ~GPIO_MODER_MODER0;     // reset port A0
+        GPIOA->MODER |= GPIO_MODER_MODER0_1;    // set alternate mode of port A0
+        GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR0;     // reset pull-up/pull-down on port A0
+        GPIOA->PUPDR |= GPIO_PUPDR_PUPDR0_1;    // set input as pull-down
+        GPIOA->AFR[0] &= ~(0xF << 4*0);         // reset alternate function of port A0
+        GPIOA->AFR[0] |= 1 << 4*0;              // set alternate funtion 1 of port A0
+        
+        GPIOA->MODER &= ~GPIO_MODER_MODER1;     // reset port A1
+        GPIOA->MODER |= GPIO_MODER_MODER1_1;    // set alternate mode of port A1
+        GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR1;     // reset pull-up/pull-down on port A1
+        GPIOA->PUPDR |= GPIO_PUPDR_PUPDR1_1;    // set input as pull-down
+        GPIOA->AFR[0] &= ~(0xF << 4*1);         // reset alternate function of port A1
+        GPIOA->AFR[0] |= 1 << 4*1;              // set alternate funtion 1 of port A1
+        
+        // configure reset and clock control registers
+        
+        RCC->APB1RSTR |= RCC_APB1RSTR_TIM2RST;  //reset TIM2 controller
+        RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM2RST;
+        
+        RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;     // TIM2 clock enable
+        
+    } else if ((a == PA_6) && (b == PC_7)) {
+        
+        // pinmap OK for TIM3 CH1 and CH2
+        
+        TIM = TIM3;
+        
+        // configure reset and clock control registers
+        
+        RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;    // manually enable port C (port A enabled by mbed library)
+        
+        // configure general purpose I/O registers
+        
+        GPIOA->MODER &= ~GPIO_MODER_MODER6;     // reset port A6
+        GPIOA->MODER |= GPIO_MODER_MODER6_1;    // set alternate mode of port A6
+        GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR6;     // reset pull-up/pull-down on port A6
+        GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1;    // set input as pull-down
+        GPIOA->AFR[0] &= ~(0xF << 4*6);         // reset alternate function of port A6
+        GPIOA->AFR[0] |= 2 << 4*6;              // set alternate funtion 2 of port A6
+        
+        GPIOC->MODER &= ~GPIO_MODER_MODER7;     // reset port C7
+        GPIOC->MODER |= GPIO_MODER_MODER7_1;    // set alternate mode of port C7
+        GPIOC->PUPDR &= ~GPIO_PUPDR_PUPDR7;     // reset pull-up/pull-down on port C7
+        GPIOC->PUPDR |= GPIO_PUPDR_PUPDR7_1;    // set input as pull-down
+        GPIOC->AFR[0] &= ~0xF0000000;           // reset alternate function of port C7
+        GPIOC->AFR[0] |= 2 << 4*7;              // set alternate funtion 2 of port C7
+        
+        // configure reset and clock control registers
+        
+        RCC->APB1RSTR |= RCC_APB1RSTR_TIM3RST;  //reset TIM3 controller
+        RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM3RST;
+        
+        RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;     // TIM3 clock enable
+        
+    } else if ((a == PB_6) && (b == PB_7)) {
+        
+        // pinmap OK for TIM4 CH1 and CH2
+        
+        TIM = TIM4;
+        
+        // configure reset and clock control registers
+        
+        RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;    // manually enable port B (port A enabled by mbed library)
+        
+        // configure general purpose I/O registers
+        
+        GPIOB->MODER &= ~GPIO_MODER_MODER6;     // reset port B6
+        GPIOB->MODER |= GPIO_MODER_MODER6_1;    // set alternate mode of port B6
+        GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR6;     // reset pull-up/pull-down on port B6
+        GPIOB->PUPDR |= GPIO_PUPDR_PUPDR6_1;    // set input as pull-down
+        GPIOB->AFR[0] &= ~(0xF << 4*6);         // reset alternate function of port B6
+        GPIOB->AFR[0] |= 2 << 4*6;              // set alternate funtion 2 of port B6
+        
+        GPIOB->MODER &= ~GPIO_MODER_MODER7;     // reset port B7
+        GPIOB->MODER |= GPIO_MODER_MODER7_1;    // set alternate mode of port B7
+        GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR7;     // reset pull-up/pull-down on port B7
+        GPIOB->PUPDR |= GPIO_PUPDR_PUPDR7_1;    // set input as pull-down
+        GPIOB->AFR[0] &= ~0xF0000000;           // reset alternate function of port B7
+        GPIOB->AFR[0] |= 2 << 4*7;              // set alternate funtion 2 of port B7
+        
+        // configure reset and clock control registers
+        
+        RCC->APB1RSTR |= RCC_APB1RSTR_TIM4RST;  //reset TIM4 controller
+        RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM4RST;
+        
+        RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;     // TIM4 clock enable
+        
+    } else {
+        
+        printf("pinmap not found for peripheral\n");
+    }
+    
+    // configure general purpose timer 3 or 4
+    
+    TIM->CR1 = 0x0000;          // counter disable
+    TIM->CR2 = 0x0000;          // reset master mode selection
+    TIM->SMCR = TIM_SMCR_SMS_1 | TIM_SMCR_SMS_0; // counting on both TI1 & TI2 edges
+    TIM->CCMR1 = TIM_CCMR1_CC2S_0 | TIM_CCMR1_CC1S_0;
+    TIM->CCMR2 = 0x0000;        // reset capture mode register 2
+    TIM->CCER = TIM_CCER_CC2E | TIM_CCER_CC1E;
+    TIM->CNT = 0x0000;          // reset counter value
+    TIM->ARR = 0xFFFF;          // auto reload register
+    TIM->CR1 = TIM_CR1_CEN;     // counter enable
+}
+
+EncoderCounter::~EncoderCounter() {}
+
+/**
+ * Resets the counter value to zero.
+ */
+void EncoderCounter::reset() {
+    
+    TIM->CNT = 0x0000;
+}
+
+/**
+ * Resets the counter value to a given offset value.
+ * @param offset the offset value to reset the counter to.
+ */
+void EncoderCounter::reset(int16_t offset) {
+    
+    TIM->CNT = -offset;
+}
+
+/**
+ * Reads the quadrature encoder counter value.
+ * @return the quadrature encoder counter as a signed 16-bit integer value.
+ */
+int16_t EncoderCounter::read() {
+    
+    return static_cast<int16_t>(-TIM->CNT);
+}
+
+/**
+ * The empty operator is a shorthand notation of the <code>read()</code> method.
+ */
+EncoderCounter::operator int16_t() {
+    
+    return read();
+}
+
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.h	Fri Mar 12 13:04:33 2021 +0000
@@ -0,0 +1,37 @@
+/*
+ * EncoderCounter.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef ENCODER_COUNTER_H_
+#define ENCODER_COUNTER_H_
+
+#include <cstdlib>
+#include <stdint.h>
+#include <mbed.h>
+
+/**
+ * This class implements a driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ */
+class EncoderCounter {
+    
+    public:
+        
+                    EncoderCounter(PinName a, PinName b);
+        virtual     ~EncoderCounter();
+        void        reset();
+        void        reset(int16_t offset);
+        int16_t     read();
+                    operator int16_t();
+        
+    private:
+        
+        TIM_TypeDef*    TIM;
+};
+
+#endif /* ENCODER_COUNTER_H_ */
+
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LowpassFilter.cpp	Fri Mar 12 13:04:33 2021 +0000
@@ -0,0 +1,113 @@
+/*
+ * LowpassFilter.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "LowpassFilter.h"
+
+using namespace std;
+
+/**
+ * Creates a LowpassFilter object with a default cutoff frequency of 1000 [rad/s].
+ */
+LowpassFilter::LowpassFilter() {
+    
+    period = 1.0f;
+    frequency = 1000.0f;
+    
+    a11 = (1.0f+frequency*period)*exp(-frequency*period);
+    a12 = period*exp(-frequency*period);
+    a21 = -frequency*frequency*period*exp(-frequency*period);
+    a22 = (1.0f-frequency*period)*exp(-frequency*period);
+    b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
+    b2 = period*exp(-frequency*period);
+    
+    x1 = 0.0f;
+    x2 = 0.0f;
+}
+
+/**
+ * Deletes the LowpassFilter object.
+ */
+LowpassFilter::~LowpassFilter() {}
+
+/**
+ * Resets the filtered value to zero.
+ */
+void LowpassFilter::reset() {
+    
+    x1 = 0.0f;
+    x2 = 0.0f;
+}
+
+/**
+ * Resets the filtered value to a given value.
+ * @param value the value to reset the filter to.
+ */
+void LowpassFilter::reset(float value) {
+    
+    x1 = value/frequency/frequency;
+    x2 = (x1-a11*x1-b1*value)/a12;
+}
+
+/**
+ * Sets the sampling period of the filter.
+ * This is typically the sampling period of the periodic task of a controller that uses this filter.
+ * @param the sampling period, given in [s].
+ */
+void LowpassFilter::setPeriod(float period) {
+    
+    this->period = period;
+    
+    a11 = (1.0f+frequency*period)*exp(-frequency*period);
+    a12 = period*exp(-frequency*period);
+    a21 = -frequency*frequency*period*exp(-frequency*period);
+    a22 = (1.0f-frequency*period)*exp(-frequency*period);
+    b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
+    b2 = period*exp(-frequency*period);
+}
+
+/**
+ * Sets the cutoff frequency of this filter.
+ * @param frequency the cutoff frequency of the filter in [rad/s].
+ */
+void LowpassFilter::setFrequency(float frequency) {
+    
+    this->frequency = frequency;
+    
+    a11 = (1.0f+frequency*period)*exp(-frequency*period);
+    a12 = period*exp(-frequency*period);
+    a21 = -frequency*frequency*period*exp(-frequency*period);
+    a22 = (1.0f-frequency*period)*exp(-frequency*period);
+    b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
+    b2 = period*exp(-frequency*period);
+}
+
+/**
+ * Gets the current cutoff frequency of this filter.
+ * @return the current cutoff frequency in [rad/s].
+ */
+float LowpassFilter::getFrequency() {
+    
+    return frequency;
+}
+
+/**
+ * Filters a value.
+ * @param value the original unfiltered value.
+ * @return the filtered value.
+ */
+float LowpassFilter::filter(float value) {
+
+    float x1old = x1;
+    float x2old = x2;
+    
+    x1 = a11*x1old+a12*x2old+b1*value;
+    x2 = a21*x1old+a22*x2old+b2*value;
+    
+    return frequency*frequency*x1;
+}
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LowpassFilter.h	Fri Mar 12 13:04:33 2021 +0000
@@ -0,0 +1,40 @@
+/*
+ * LowpassFilter.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef LOWPASS_FILTER_H_
+#define LOWPASS_FILTER_H_
+
+#include <cstdlib>
+
+/**
+ * This class implements a time-discrete 2nd order lowpass filter for a series of data values.
+ * This filter can typically be used within a periodic task that takes measurements that need
+ * to be filtered, like speed or position values.
+ */
+class LowpassFilter {
+    
+    public:
+    
+                LowpassFilter();
+        virtual ~LowpassFilter();
+        void    reset();
+        void    reset(float value);
+        void    setPeriod(float period);
+        void    setFrequency(float frequency);
+        float   getFrequency();
+        float   filter(float value);
+        
+    private:
+        
+        float   period;
+        float   frequency;
+        float   a11, a12, a21, a22, b1, b2;
+        float   x1, x2;
+};
+
+#endif /* LOWPASS_FILTER_H_ */
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Fri Mar 12 13:04:33 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/jdenkers/code/Servo/#352133517ccc
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ThreadFlag.cpp	Fri Mar 12 13:04:33 2021 +0000
@@ -0,0 +1,54 @@
+/*
+ * ThreadFlag.cpp
+ * Copyright (c) 2020, ZHAW
+ * All rights reserved.
+ */
+
+#include "ThreadFlag.h"
+
+using namespace std;
+
+unsigned int ThreadFlag::threadFlags = 0;
+
+/**
+ * Creates a signal object and assignes a unique flag.
+ */
+ThreadFlag::ThreadFlag() {
+    
+    mutex.lock();
+    
+    unsigned int n = 0;
+    while ((((1 << n) & threadFlags) > 0) && (n < 30)) n++;
+    threadFlag = (1 << n);
+    
+    mutex.unlock();
+}
+
+/**
+ * Deletes the signal object and releases the assigned flag.
+ */
+ThreadFlag::~ThreadFlag() {
+    
+    mutex.lock();
+    
+    threadFlags &= ~threadFlag;
+    
+    mutex.unlock();
+}
+
+/**
+ * Gets the assigned thread flag.
+ */
+unsigned int ThreadFlag::read() {
+    
+    return threadFlag;
+}
+
+/**
+ * The empty operator is a shorthand notation of the <code>read()</code> method.
+ */
+ThreadFlag::operator unsigned int() {
+    
+    return read();
+}
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ThreadFlag.h	Fri Mar 12 13:04:33 2021 +0000
@@ -0,0 +1,33 @@
+/*
+ * ThreadFlag.h
+ * Copyright (c) 2020, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef THREAD_FLAG_H_
+#define THREAD_FLAG_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+/**
+ * This class manages the handling of unique thread flags to trigger rtos threads.
+ */
+class ThreadFlag {
+    
+    public:
+        
+                                ThreadFlag();
+        virtual                 ~ThreadFlag();
+        virtual unsigned int    read();
+                                operator unsigned int();
+        
+    private:
+        
+        static unsigned int threadFlags;    // variable that holds all assigned thread flags
+        unsigned int        threadFlag;     // thread flag of this object
+        Mutex               mutex;          // mutex to lock critical sections
+};
+
+#endif /* THREAD_FLAG_H_ */
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 12 13:04:33 2021 +0000
@@ -0,0 +1,119 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2019 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "mbed.h"
+#include "platform/mbed_thread.h"
+#include "SDBlockDevice.h"
+#include "FATFileSystem.h"
+#include "EncoderCounter.h"
+#include "Servo.h"
+#include "Controller.h"
+
+
+// Blinking rate in milliseconds
+#define BLINKING_RATE_MS                                                    500
+
+
+int main()
+{
+    
+    DigitalIn user_button(USER_BUTTON);
+    
+    // initialise PWM
+    PwmOut pwm_motor1(PB_13);
+    PwmOut pwm_motor2(PA_9);
+    PwmOut pwm_motor3(PA_10);
+    
+    // crete Encoder read objects
+    EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B)
+    EncoderCounter counter2(PB_6, PB_7);
+    EncoderCounter counter3(PA_0, PA_1);
+    
+    // create controller
+    Controller controller(pwm_motor1, pwm_motor2, counter1, counter1);
+    
+    DigitalOut enable(PB_15);
+    
+    // create servo objects
+    Servo  S0(PB_2);
+    Servo  S1(PC_8);
+    Servo  S2(PC_6);
+    
+    SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2);
+    printf("BlockDevice created\r\n"); 
+    FATFileSystem fs("fs", &sd);
+    
+    // Initialise the digital pin LED1 as an output
+    DigitalOut myled(LED1);
+    
+    
+    
+    // initialise PWM
+    pwm_motor1.period(0.00005f);// 0.05ms 20KHz
+    pwm_motor1.write(0.5f);
+    pwm_motor2.period(0.00005f);// 0.05ms 20KHz
+    pwm_motor2.write(0.5f);
+    pwm_motor3.period(0.00005f);// 0.05ms 20KHz
+    pwm_motor3.write(0.5f);
+    
+    // initialise and test Servo
+    S0.Enable(1000,20000);
+    S1.Enable(1000,20000);
+    S2.Enable(1000,20000);
+    
+    printf("Test writing... ");
+    FILE* fp = fopen("/fs/data.csv", "w");
+    fprintf(fp, "test %.5f\r\n",1.23);
+    fclose(fp);
+    printf("done\r\n");
+    
+    printf("Test reading... ");
+    // read from SD card
+    fp = fopen("/fs/data.csv", "r");
+    if (fp != NULL) {
+        char c = fgetc(fp);
+        if (c == 't')
+            printf("done\r\n");
+        else
+            printf("incorrect char (%c)!\n", c);
+        fclose(fp);
+    } else {
+        printf("Reading failed!\n");
+    }
+    
+    // enable driver DC motors
+    enable = 1;
+
+    while (true) {
+        
+        if(!user_button) {
+            // LED off, set controller speed, pwm2, position servo
+            myled = 0;
+            controller.setDesiredSpeedLeft(50.0f);
+            controller.setDesiredSpeedRight(50.0f);
+            pwm_motor3.write(0.9f);
+
+            S0.SetPosition(1200);
+            S1.SetPosition(1200);
+            S2.SetPosition(1200);
+
+        } else {
+            // LED on, reset controller speed, pwm2, position servo
+            myled = 1;
+            controller.setDesiredSpeedLeft(0.0f);
+            controller.setDesiredSpeedRight(0.0f);
+            pwm_motor3.write(0.5f);
+
+            S0.SetPosition(1900);
+            S1.SetPosition(1900);
+            S2.SetPosition(1900);
+
+        }
+        
+        printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
+        
+        thread_sleep_for(BLINKING_RATE_MS);
+    }
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Fri Mar 12 13:04:33 2021 +0000
@@ -0,0 +1,1 @@
+https://github.com/ARMmbed/mbed-os/#cf4f12a123c05fcae83fc56d76442015cb8a39e9
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed_app.json	Fri Mar 12 13:04:33 2021 +0000
@@ -0,0 +1,7 @@
+{
+    "target_overrides": {
+        "NUCLEO_F446RE": {
+            "target.components_add": ["SD"]
+        }
+    }
+}