Revision 0:7a97ebb833eb, committed 2019-05-05

Comitter:

rzalog

Date:

Sun May 05 00:02:11 2019 +0000

Commit message:

test commit;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/beep.lib	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/dreschpe/code/beep/#d8e14429a95f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoders.cpp	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,46 @@
+#include "encoders.h"
+#include "pins.h"
+#include "globals.h"
+#include "mbed.h"
+#include "QEI.h"
+
+/***
+ * Assignment 2
+ * 
+ * Import the QEI in order to have functioning encoders.
+ ***/
+QEI wheel_R(ENC_RB, ENC_RF, NC, 624, QEI::X4_ENCODING);
+QEI wheel_L(ENC_LF, ENC_LB, NC, 624, QEI::X4_ENCODING);
+
+///////
+// Encoder class implementation
+///////
+
+Encoder::Encoder() {
+    reset();
+}
+
+void Encoder::reset() volatile {
+    m_countR = 0;
+    m_countL = 0;
+    wheel_R.reset();
+    wheel_L.reset();
+}
+
+void Encoder::update() volatile {
+    m_countR = wheel_R.getPulses();
+    m_countL = wheel_L.getPulses();
+}
+
+int Encoder::right() volatile {
+    return wheel_R.getPulses();
+}
+
+int Encoder::left() volatile {
+    return wheel_L.getPulses();
+}
+
+void Encoder::printValues() volatile {
+//    Serial pc(TX, RX);
+    pc.printf("R: %d\tL: %d\n", right(), left());
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoders.h	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,23 @@
+#pragma once
+
+/***
+ * Assignment 2
+ * 
+ * Experimentally determine this value.
+ ***/
+const float COUNTS_FOR_CELL = 0; // TODO
+
+class Encoder {
+public:
+    Encoder();
+    void reset() volatile;
+    void update() volatile;
+    int right() volatile;
+    int left() volatile;
+    
+    void printValues() volatile;
+private:    
+    volatile int m_countR;
+    volatile int m_countL;
+};
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/globals.cpp	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,20 @@
+#include "globals.h"
+#include "mbed.h"
+#include "led.h"
+#include "pins.h"
+
+Led led(LED_BACK);
+
+Serial pc(TX, RX);
+
+Systick systick;
+volatile unsigned int millis = 0;
+
+Motors motors;
+MainController mainController;
+
+volatile Encoder encoders;
+//volatile Gyro gyro;
+volatile IR ir;
+
+//Beep buzz(PB_14);
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/globals.h	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,26 @@
+#pragma once
+
+#include "mbed.h"
+#include "led.h"
+#include "motors.h"
+#include "encoders.h"
+#include "ir.h"
+#include "systick.h"
+#include "main_controller.h"
+#include "beep.h"
+
+extern Led led;
+
+extern Serial pc;
+
+extern Systick systick;
+extern volatile unsigned int millis;
+
+extern Motors motors;
+extern MainController mainController;
+
+extern volatile Encoder encoders;
+//extern volatile Gyro gyro;
+extern volatile IR ir;
+
+//extern Beep buzz;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ir.cpp	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,95 @@
+#include "ir.h"
+#include "pins.h"
+#include "globals.h"
+
+AnalogIn rec_RF(DET_Rfront);
+AnalogIn rec_LF(DET_Lfront);
+//AnalogIn rec_RS(DET_Rside);
+//AnalogIn rec_LS(DET_Lside);
+
+DigitalOut ir_RF(IR_Rfront);
+DigitalOut ir_LF(IR_Lfront);
+//DigitalOut ir_RS(IR_Rside);
+//DigitalOut ir_LS(IR_Lside);
+
+
+///////
+// IR class implementation
+///////
+
+IR::IR() {
+    m_rf = 0;
+    m_lf = 0;
+    m_rs = 0;
+    m_ls = 0;
+}
+
+/***
+ * Assignment 4 START
+ ***/
+
+/***
+ * Properly turn the IR's on, warm up receiver for WARM_UP_US,
+ * read the value, cool down receivers for COOL_DOWN_US,
+ * then turn off.
+ ***/
+float IR::flash_ir(DigitalOut ir, AnalogIn rec) volatile {
+    ir = 1;
+    wait_us(WARM_UP_US);
+    float ret = read_ir(rec);
+    ir = 0;
+    wait_us(COOL_DOWN_US);
+    
+    return ret;
+}
+
+float IR::RS_error() volatile {
+    // TODO
+
+    if (RS() > MIDR) {
+        //return RS_base() - RS();
+        return 0;
+    }
+    else return 0;
+}
+
+float IR::LS_error() volatile {
+    // TODO
+
+    if (LS() > MIDL) {
+        //return LS() - LS_base();
+        return 0;
+    }
+    else return 0;
+}
+
+float IR::read_ir(AnalogIn rec) volatile {
+    // Read an IR, averaging over 10 reads.
+    int n = 10;
+    float total = 0;
+       
+    for (int i = 0; i < n; i++) {
+        total += rec.read();   
+    }
+    
+    return total / n;
+}
+
+void IR::update() volatile {
+    // Update all your IR readings.
+
+    // Don't do any init because for some reason that doesn't work...
+    m_rf = flash_ir(ir_RF, rec_RF); //- init_rf;
+    m_lf = flash_ir(ir_LF, rec_LF); //- init_lf;
+    
+    //m_rs  = flash_ir(ir_RS, rec_RS); //- init_rs;
+    //m_ls = flash_ir(ir_LS, rec_LS); //- init_ls;
+}
+
+
+void IR::printValues() volatile {
+    // Utility: Use for diagnostics
+
+    pc.printf("RF: %.3f\tLF: %.3f\tRS: %.3f\tLS: %.3f\n",
+        m_rf, m_lf, m_rs, m_ls);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ir.h	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,37 @@
+#pragma once
+
+#include "mbed.h"
+
+const float MIDR = 0.0f; // TODO
+const float MIDL = 0.0f; // TODO
+
+// IR Stuff
+const int WARM_UP_US = 60;
+const int COOL_DOWN_US = 60;
+
+class IR {
+public:
+    IR();
+    void update() volatile;
+
+    float RF() volatile { return m_rf; };
+    float LF() volatile { return m_lf; };
+    float RS() volatile { return m_rs; };
+    float LS() volatile { return m_ls; };
+        
+    float RS_error() volatile;
+    float LS_error() volatile;
+    
+    void printValues() volatile;
+private:
+    float flash_ir(DigitalOut ir, AnalogIn rec) volatile;
+    float read_ir(AnalogIn rec) volatile;
+
+    float m_rf;
+    float m_lf;
+    float m_rs;
+    float m_ls;
+    
+    float init_rf;
+    float init_lf;
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/led.cpp	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,18 @@
+#include "led.h"
+#include "pins.h"
+
+Led::Led(PinName pin) : m_led(pin) {
+    
+}
+
+void Led::toggle() {
+    m_led = !m_led;   
+}
+
+void Led::on() {
+    m_led = 1;   
+}
+
+void Led::off() {
+    m_led = 0;   
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/led.h	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "mbed.h"
+
+class Led {
+public:
+    Led(PinName pin);
+    void toggle();
+    void on();
+    void off();
+private:
+    DigitalOut m_led;
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,18 @@
+#include "run_modes.h"
+#include "globals.h"
+#include "pins.h"
+
+#include "main_controller.h"
+#include "pid_controller.h"
+
+int main() {
+    pc.printf("\n---\nProgram has started.\n---\n");
+
+    led.on();
+    //Beep buzz(BUZZER);
+    //buzz.beep(1000, 0.5);
+    wait(1);
+    led.off();
+
+    test_buzzer();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_controller.cpp	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,97 @@
+#include <string>
+
+#include "globals.h"
+#include "main_controller.h"
+#include "pid_controller.h"
+
+MainController::MainController() {
+    m_pid = new PIDController;
+    this->stop();
+}
+
+MainController::~MainController() {
+    delete m_pid;
+}
+
+void MainController::start() {
+    m_pid->reset();
+    shouldUpdate = true;
+}
+
+void MainController::stop() {
+    shouldUpdate = false;
+    m_pid->reset();
+    motors.stop();
+}
+
+void MainController::update() {
+    if (shouldUpdate) {
+        m_pid->update();
+    }
+}
+
+void MainController::driveStraight() {
+    pc.printf("Driving straight.\n");
+    m_pid->reset();
+    
+    int start = millis;
+    
+    m_pid->setXGoal(100);
+    m_pid->setWGoal(0);
+
+    while (encoders.right() < 5000) {
+        pc.printf(m_pid->getData());
+        wait(0.5);
+    }
+    
+    int end = millis;
+    
+    systick.stop();
+    motors.stop();
+    
+    pc.printf("Done moving straight: %d ms\n", end - start);
+    encoders.printValues();
+}
+
+void MainController::turn(int deg) {
+    pc.printf("Turning %d degrees\n", deg);
+    this->start();
+    
+    int counts = deg * deg_to_counts;
+    
+    m_pid->setXGoal(0);
+    m_pid->setWGoal(counts);
+    
+    while (!m_pid->isDone()) {
+        pc.printf(m_pid->getData());
+    }
+
+    pc.printf(m_pid->getData());
+    this->stop();
+    pc.printf("Done turning %d degrees\n", deg);
+}
+
+void MainController::moveCells(float n) {
+    pc.printf("Moving %0.2f cells\n", n);
+    this->start();
+    
+    int counts = n * cells_to_counts;
+ 
+    pc.printf("Set goal x counts to %d\n", counts);
+    
+    int start = millis;
+    
+    m_pid->setXGoal(counts);
+    m_pid->setWGoal(0);
+ 
+    while (!m_pid->isDone()) {
+        pc.printf(m_pid->getData());
+    }
+ 
+    int end = millis;
+ 
+    pc.printf(m_pid->getData());
+
+    this->stop();
+    pc.printf("Finished moving %0.2f cells: %d ms\n", n, end - start);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_controller.h	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,26 @@
+#pragma once
+
+class PIDController;
+
+const float deg_to_counts = 4.65f;
+const float cells_to_counts = 216.0f;
+
+class MainController {
+public:
+    MainController();
+    ~MainController();
+    
+    void update();
+    
+    void driveStraight();
+    void turn(int deg);
+    void moveCells(float n);
+
+    void start();
+    void stop();
+    
+    PIDController* m_pid;
+
+private:
+    bool shouldUpdate;
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motors.cpp	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,66 @@
+#include "motors.h"
+#include "mbed.h"
+#include "pins.h"
+
+PwmOut m_RF(MOTOR_RF);
+PwmOut m_RB(MOTOR_RB);
+PwmOut m_LF(MOTOR_LF);
+PwmOut m_LB(MOTOR_LB);
+
+/***
+ * Assignment 2
+ * 
+ * Add logic to set the PWM based on postiive/negative number.
+ ***/
+
+void Motors::setMotorPwm(int motor, float pwm) {
+    if (abs(pwm) > MAX_SPEED) {
+        if (pwm > 0) pwm = MAX_SPEED;
+        else if (pwm < 0) pwm = -MAX_SPEED;
+    }
+    else if (abs(pwm) < MIN_SPEED) {
+        if (pwm > 0) pwm = MIN_SPEED;
+        else if (pwm < 0) pwm = -MIN_SPEED;
+    }
+    
+    // Use the "PwmOut" objects defined above
+    // Hint: Stop your backwards/forward motor before going forward/backwards.
+    if (pwm > 0) {
+        if (motor == RIGHT_MOTOR) {
+            m_RB = 0.f;
+            m_RF = pwm;
+        } else {
+            m_LB = 0.f;
+            m_LF = pwm;
+        }
+    } else {
+        if (motor == RIGHT_MOTOR) {
+            m_RF = 0.f;
+            m_RB = abs(pwm);
+        } else {
+            m_LF = 0.f;
+            m_LB = abs(pwm);
+        }
+    }
+}
+
+Motors::Motors() {
+    // DO NOT initialize PWM!!! It breaks the mouse.
+}
+
+void Motors::stop() {
+    setRightPwm(0);
+    setLeftPwm(0);   
+}
+
+void Motors::setRightPwm(float pwm) {
+    m_rpwm = pwm;
+    setMotorPwm(RIGHT_MOTOR, pwm);
+}
+
+void Motors::setLeftPwm(float pwm) {
+    m_lpwm = pwm;
+    setMotorPwm(LEFT_MOTOR, pwm);
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motors.h	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,26 @@
+#pragma once
+
+// Feel free to change these parameters as needed
+const float MIN_SPEED = 0.08f;
+const float MAX_SPEED = 0.8f;
+
+const int RIGHT_MOTOR = 0;
+const int LEFT_MOTOR = 1;
+
+class Motors {
+public:
+    Motors();
+    void startBaseSpeed();
+    void stop();
+
+    /***
+     * Range from 0.0-1.0.
+     ***/
+    void setLeftPwm(float pwm);
+    void setRightPwm(float pwm);
+private:
+    void setMotorPwm(int motor, float pwm);
+    
+    float m_rpwm;
+    float m_lpwm;
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid_controller.cpp	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,112 @@
+#include "pid_controller.h"
+#include "globals.h"
+
+PIDController::PIDController() {
+    this->reset();
+}
+
+void PIDController::reset() volatile {    
+    m_goalW = 0;
+    m_goalX = 0;
+
+    m_pwmW = 0;
+    m_pwmX = 0;
+
+    m_errorW = 0;
+    m_errorX = 0;
+    m_errorW_old = 0;
+    m_errorX_old = 0;
+
+    m_countsW = 0;
+    m_countsX = 0;
+    
+    m_doneCounter = 0;
+    
+    encoders.reset();
+}
+
+void PIDController::update() volatile {
+     getSensorFeedback();
+     x_controller();
+     w_controller();
+     updateMotorPwm();
+}
+
+void PIDController::setXGoal(int counts) {
+     m_goalX = counts;
+}
+
+void PIDController::setWGoal(int counts) {
+     m_goalW = counts;
+}
+
+bool PIDController::isDone() volatile {
+    return m_doneCounter > done_limit;
+}
+
+char* PIDController::getData() {
+    sprintf(buf, "goalx: %d\tgoalw: %d\tpwmr: %.3f\tpwml: %.3f\tpwmx: %.3f\tpwmw: %.3f\terrorx: %d\terrorw: %d\n",
+        m_goalX,
+        m_goalW,
+        m_pwmX + m_pwmW,
+        m_pwmX - m_pwmW,
+        m_pwmX,
+        m_pwmW,
+        m_errorX,
+        m_errorW);
+    return buf;
+}
+
+/**
+ * Private functions to do the internal work for PID.
+ **/
+void PIDController::getSensorFeedback() volatile {
+     int right, left;
+     right = encoders.right();
+     left = encoders.left();
+     
+     m_countsW = right - left;
+     m_countsX = (right + left) / 2;
+}
+
+void PIDController::x_controller() volatile {
+     m_errorX = m_goalX - m_countsX;
+     m_pwmX = KpX * m_errorX + KdX * (m_errorX - m_errorX_old);
+     m_errorX_old = m_errorX;
+}
+
+void PIDController::w_controller() volatile {
+     m_errorW = m_goalW - m_countsW;
+     m_pwmW = KpW * m_errorW + KdW * (m_errorW - m_errorW_old);
+     m_errorW_old = m_errorW;
+}
+
+float limit(float val, float min, float max) {
+    if (abs(val) > max) {
+        if (val > 0) val = max;
+        else if (val < 0) val = -max;
+    }
+    else if (abs(val) < min) {
+        if (val > 0) val = min;
+        else if (val < 0) val = -min;
+    }
+    
+    return val;
+}
+
+void PIDController::updateMotorPwm() volatile {
+     m_pwmX = limit(m_pwmX, 0, max_speed);
+     m_pwmW = limit(m_pwmW, 0, max_speed);
+     
+     float right = m_pwmX + m_pwmW;
+     float left = m_pwmX - m_pwmW;
+     
+     if (abs(right) < min_speed && abs(left) < min_speed) {
+         m_doneCounter += 1;
+     }
+     else { 
+        motors.setRightPwm(m_pwmX + m_pwmW);
+        motors.setLeftPwm(m_pwmX - m_pwmW);
+     }
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid_controller.h	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,54 @@
+#pragma once
+
+#include "globals.h"
+
+const float KpX = 0.01;
+const float KdX = 0.1;
+
+const float KpW = 0.003;
+const float KdW = 0;
+
+// for turning KpW=0.01, KdW=0.1
+
+const float min_speed = 0.08;
+const float max_speed = 0.16;
+
+const int done_limit = 150;
+
+class PIDController {
+public:
+    PIDController();
+    
+    void reset() volatile;
+    void update() volatile;
+
+    void setXGoal(int counts);
+    void setWGoal(int counts);
+
+    bool isDone() volatile;
+    char* getData();
+private:
+    void getSensorFeedback() volatile;
+    void x_controller() volatile;
+    void w_controller() volatile;
+    void updateMotorPwm() volatile;
+    
+    int m_goalW;
+    int m_goalX;
+    
+    float m_pwmW;
+    float m_pwmX;
+    
+    int m_errorW;
+    int m_errorX;
+    int m_errorW_old;
+    int m_errorX_old;
+    
+    int m_countsW;
+    int m_countsX;
+    
+    char buf[200];
+    
+    int m_doneCounter;
+};
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pins.h	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,39 @@
+#pragma once
+
+// LEDs
+#define LED_BACK    PA_15
+
+// Serial I/O
+#define TX      PA_9
+#define RX      PA_10
+
+// Motors (R/L = right/left, F/B = forward/backwards)
+#define MOTOR_RF    PB_6
+#define MOTOR_RB    PB_7
+#define MOTOR_LF    PB_8
+#define MOTOR_LB    PB_9
+
+// Encoders (left/right, A/B channels)
+#define ENC_LF      PB_3
+#define ENC_LB      PC_10
+#define ENC_RF      PB_5
+#define ENC_RB      PB_4
+
+// IR emitters
+#define IR_Rfront   PA_1
+#define IR_Lfront   PB_0
+#define IR_Rside    PA_7
+#define IR_Lside    PC_4
+
+// IR detectors
+#define DET_Rfront  PA_0
+#define DET_Lfront  PB_1
+#define DET_Rside   PA_6
+#define DET_Lside   PC_5
+
+// Gyro
+//#define GYRO    0
+//#define VREF    0
+
+// Buzzer
+#define BUZZER PB_14

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/run_modes.cpp	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,93 @@
+#include "globals.h"
+#include "pins.h"
+#include "beep.h"
+
+/***
+ * Driving stuff
+ ***/
+void test_drive_straight() {    
+    systick.start();
+    //motors.setRightPwm(0.1f);
+    //motors.setLeftPwm(0.1f);
+    while(1) {
+        mainController.moveCells(100);
+    }
+}
+
+/***
+ * Assignment 2
+ ***/
+
+void basic_motor_movement() {
+    motors.setRightPwm(0.15f);
+    motors.setLeftPwm(0.15f);
+
+    wait(3);
+    
+    motors.stop();
+    
+    wait(1);
+
+    motors.stop();
+    pc.printf("right: %d, left: %d\n", encoders.right(), encoders.left());
+
+    
+    motors.setRightPwm(-0.2f);
+    motors.setLeftPwm(-0.2f);
+    
+    wait(3);
+    motors.stop();
+
+    // TODO: Turn
+}
+
+void read_encoders() {
+    while (true) {
+        encoders.printValues();
+        wait(0.5);
+    }
+}
+
+void test_systick() {
+    systick.start();
+
+    while (true) {
+        wait(1);
+        pc.printf("Current time (ms): %d\n", millis);
+    }
+}
+
+void test_buzzer() {
+    Beep buzzer(PB_14);
+    
+    for (int i=0; i < 5; i++) {
+        buzzer.beep(1000, 1);
+        wait(2);
+    }
+}
+
+inline float flash(DigitalOut emit, AnalogIn recv) {
+    float val;
+    emit = 1;
+    wait_us(60);
+    val = recv.read();
+    emit = 0;
+    wait_us(60);
+    
+    return val;
+}
+
+void test_ir() {
+    
+    pc.printf("Testing IRs.\n");
+    
+    systick.start();
+    
+    while(1) {        
+        //ir.update();
+        pc.printf("millis: %d\t", millis);
+        ir.printValues();
+        
+        wait(0.3);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/run_modes.h	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,7 @@
+#pragma once
+
+void test_drive_straight();
+void basic_motor_movement();
+void read_encoders();
+void test_buzzer();
+void test_ir();
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/systick.cpp	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,35 @@
+#include "systick.h"
+#include "mbed.h"
+#include "globals.h"
+
+void systickFunction() {
+    // Update global counter
+    millis++;
+    mainController.update();
+    
+    // Update other sensors
+    ir.update();
+}
+
+void Systick::start() {
+     m_systicker.attach(&systickFunction, SYS_TICK_TIME);
+}
+
+void Systick::stop() {
+     m_systicker.detach();
+}
+
+Systick::Systick() {
+    // A little bit of magic; if you want to know why, ask Robert
+    NVIC_SetPriority(TIM5_IRQn, 255);
+}
+
+void Systick::wait(float sec) {
+    // Utility function: allows you to wait while using systick.
+    int init = millis;
+    
+    float num_ticks = sec / SYS_TICK_TIME;
+
+    while (millis - init < num_ticks)
+        ;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/systick.h	Sun May 05 00:02:11 2019 +0000
@@ -0,0 +1,20 @@
+#pragma once
+
+#include "mbed.h"
+
+// 1ms systick time
+const float SYS_TICK_TIME = .001;
+
+void systickFunction();
+
+class Systick {
+public:
+    Systick();
+     
+    void start();
+    void stop();
+    
+    void wait(float sec);    
+private:
+    Ticker m_systicker;
+};