From 74e0d979861b67020a39ffb190e1dacde0d2a74c Mon Sep 17 00:00:00 2001
From: philsson <philsson@gmail.com>
Date: Wed, 29 Aug 2018 22:52:38 +0200
Subject: [PATCH] Main file test for motors, servos and other drivers

---
 main.cpp | 148 +++++++++++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 133 insertions(+), 15 deletions(-)

diff --git a/main.cpp b/main.cpp
index 11d601e..512a906 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,31 +1,149 @@
+#include "mbed.h"
+
 #include <cmath>
 
-#include "mbed.h"
+#include "mbed_events.h"
+
 #include "src/targets/revo_f4/pins.h"
+#include "src/drivers/MPU6000.h"
+#include "src/drivers/stepper.h"
+#include "src/drivers/servo.h"
+#include "src/control/lpf.h"
+
+#ifndef PI
+    #define PI 3.14159265358979f
+#endif
+
+using namespace drivers;
+using namespace control;
+
+EventQueue queue;
+
+Ticker queueReader;
+
+
+// MPU setup
+SPI spi(PA_7, PA_6, PA_5);   //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz
+mpu6000_spi imu(spi,PA_4);   //define the mpu6000 object
 
 PwmOut led1(D4); //PB_5
-PwmOut led2(D5); //PB_4
+DigitalOut ledOrg(D5);
+
+Stepper motor1(PC_9, PC_7, PC_8);
+Stepper motor2(PB_15, PB_14, PC_6);
+
+Servo servo(PA_0);
+
+// Interrupt pin from Gyro to MCU
+InterruptIn gyroINT(PC_4);
+
+// Draft of some control function
+void controlFunc(int a)
+{
+    static incrementalLPF filter;
+    double accY = 1;
+    accY = imu.read_acc(1);
+    double filtered = filter.filter(accY);
+    motor1.setSpeed(360.0*2*filtered);   
+}
+
+// This context just pulses the blue LED
+void pulseLedContext()
+{
+    while (true)
+    {
+        float in, out;
+  
+        for (in = 0; in < 2*PI; in = in + PI/50)
+        {
+            out = sin(in) + 0.5;
+            // We cannot set pulsewidth or period due to
+            // sharing timer with motor outputs. "write"
+            // instead sets the dutycycle in range 0-1
+            led1.write(out);
+            Thread::wait(10);
+        }
+    }
+}
+
+void readQueue()
+{
+    //queue.dispatch(0);
+}
+
+// Interrupt driven function
+// At the moment its running at 100hz
+void ISRGYRO() 
+{
+    static int i = 0;
+    i++;
+    if (i > 100)
+    {
+        i = 0;
+        ledOrg = !ledOrg;
+    }
+    // Report that there is new data to read
+    // TODO: This hack has to be done in a better way
+    queue.call(controlFunc, 0);
+}
 
 int i = 0;
 
+// main() runs in its own thread
 int main() {
 
-    led1.period_us(100);
-    led2.period_us(100);
+    // Start sweeping the arm
+    servo.sweep(0.0, 1, 2);
 
-    led1.pulsewidth_us(1);
-    led2.pulsewidth_us(100);
+    Thread ledPulseThread;
+    ledPulseThread.start(callback(&pulseLedContext));
 
-    while (true) {
-        i++;
-        if (i > 1000)
-        {
-            i = 1;
-        }
+    // Register a event queue reader at set interval
+    queueReader.attach(&readQueue, 0.001);
 
-        led1.pulsewidth_us(i);
-        led2.pulsewidth_us(1000-i);
+    // Enable motor controllers (Will power the motors with no movement)
+    motor1.enable();
+    motor2.enable();
 
-        wait(0.002);
+    // Set motor 2 to a fixed speed 
+    // (To test that timers don't interfeer with each other)
+    motor2.setSpeed(90.0);
+
+    // Function to attach to gyro interrupt
+    gyroINT.rise(&ISRGYRO);
+
+    // MPU startup
+    if(imu.init(10,BITS_DLPF_CFG_188HZ)){
+        printf("\nCouldn't initialize MPU6000 via SPI!");
+    }  
+
+    // Enable/Activate the Gyro interrupt
+    imu.enableInterrupt();
+
+    // Read some test values
+    int wmi = imu.whoami();
+    int scale = imu.set_acc_scale(BITS_FS_16G);
+
+    // Stop the sweeping arm and do some other stuff with it
+    servo.stop();
+
+    Thread::wait(1000);
+
+    servo.nod();
+
+    Thread::wait(1000);
+
+    servo.sweep(1.0, 1.5, 3);
+
+    servo.stop();
+
+    servo.setPosition(-1);
+
+    while (true) 
+    {
+        // HACK!!!
+        // Dispatch any content of the event queue.
+        queue.dispatch(0);
+        Thread::wait(1);
     }
 }
\ No newline at end of file