Deployment of control loops

Find the bug! Needs fixing. Not tuned! And something crashes the board.
When vibrating a lot (probably high gains or high d-term / d-term noise) or when the robot leans a lot to one side everything freezes. Both loops blinking leds stop completely and steppers stay at last command.

I've tried electronic fixes and lastly put the controller on its own power supply.
Can be triggered even without any power electronics connected (rocking the robot by hand). So it seems to be software related!

.vscode/settings.json

@@ -4,6 +4,18 @@
     "C_Cpp.intelliSenseEngine": "Tag Parser",
     "files.associations": {
         "cmath": "cpp",
-        "queue": "cpp"
+        "queue": "cpp",
+        "__bit_reference": "cpp",
+        "__functional_base": "cpp",
+        "atomic": "cpp",
+        "deque": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "limits": "cpp",
+        "memory": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "vector": "cpp",
+        "string": "cpp"
     }
 }

main.cpp

@@ -1,21 +1,30 @@
 #include "mbed.h"
 
+// STD
 #include <cmath>
 
+// Mbed
 #include "mbed_events.h"
 
+// target definitions
 #include "src/targets/revo_f4/pins.h"
+// Mmath
+#include "src/math/Utilities.h"
+// Drivers
 #include "src/drivers/MPU6000.h"
 #include "src/drivers/stepper.h"
 #include "src/drivers/servo.h"
+// Control
 #include "src/control/lpf.h"
+#include "src/control/PID.h"
+#include "src/control/ImuFusion.h"
 
-#ifndef PI
+#define WHEEL_SIZE 0.09f
-    #define PI 3.14159265358979f
-#endif
 
+using namespace targets::revo_f4;
 using namespace drivers;
 using namespace control;
+using namespace math;
 
 EventQueue queue;
 
@@ -24,21 +33,100 @@ EventQueue queue;
 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 ledBlue(D4);
 DigitalOut ledOrg(D5);
 
-Stepper motor1(PC_9, PC_7, PC_8);
+Stepper motorL(PC_9, PC_7, PC_8);
-Stepper motor2(PB_15, PB_14, PC_6);
+Stepper motorR(PB_15, PB_14, PC_6);
 
 Servo servo(PA_0);
 
 // Interrupt pin from Gyro to MCU
 InterruptIn gyroINT(PC_4);
 
+Timer timer;
+
+// TODO: Figure out some good values
+controllerPI throttleControl(0.0025, 0.01, 5, 0); // 0.065, 0.05, 12, 40
+
+// TODO: Figure out some good values
+controllerPD angleControl(25.0, 35.25, 400);
+// Alternatively try controllerPD2 which has another Dterm implementation
+
 // Draft of some control function
 // this runs in the context of eventThread and is triggered by the gyro ISR pin
 void controlFunc()
 {
+    static ImuFusion imuFusion(&imu);
+    static float controlOutput(0.0f);
+
+    // Calculate dT in sec
+    float dT = timer.read_us()/(float)1000000.0;
+    timer.reset();
+
+    // Retrieve IMU angle (Only x-axis implemented for now)
+    float angleX = imuFusion.getAngle(dT);
+    // Reset anything left in the IMU FIFO queue
+    imu.fifo_reset();
+
+    // If the robot is above this angle we turn off motors
+    static bool disabled = false;
+    if (abs(angleX) > (float)50.0f && !disabled)
+    {
+        controlOutput = 0.0f; // rinse integral
+        disabled = true;
+    }
+    else if (abs(angleX) < (float)50.0f && disabled)
+    {
+        disabled = false;
+    }
+
+    /* --------------------------
+    Calculate estimated groundspeed 
+    of the robot. 
+    TODO: Inject correct input (compensate for robot rotation)
+    -------------------------- */
+    // Calculate filtered groundspeed of the robot (From deg/s to m/s)
+    float estimatedSpeed((motorL.getSpeed() + motorR.getSpeed())/2);
+    static incrementalLPF speedFilter;
+    float filteredEstSpeed(speedFilter.filter(estimatedSpeed));
+    float speedScale((1.0f/360.0f)*WHEEL_SIZE*PI);
+    float groundSpeed(filteredEstSpeed*speedScale);
+
+    /* --------------------------
+    Calculate the setpoint for 
+    the main control (control the angle)
+    through this throttle control loop (PI)
+    TODO: TEMP turned off the throttle 
+    control to tune the PD angle control
+    -------------------------- */
+    float throttle(0.0f); // TODO: This will be the input from remote
+    float angleSP = 0.0f; // Temporarily emulate an output
+    //float angleSP = throttleControl.run(dT, groundSpeed, throttle);
+
+    
+    /* --------------------------
+    The last control loop. Angle (PD)
+    TODO:
+    -------------------------- */
+    // Integrating the output to obtain acceleration
+    if (!disabled)
+    {
+        controlOutput += angleControl.run(dT, angleX, angleSP);
+
+        controlOutput = constrain(controlOutput, 800.0f);
+    }
+    
+    /* --------------------------
+     Lastly the steering is added
+     straight on the output
+     TODO: Activate when implemented
+    -------------------------- */
+    float steering(0.0f); // This will come from remote
+    motorL.setSpeed(controlOutput - steering);
+    motorR.setSpeed(controlOutput + steering);
+
+    // Blink LED at 1hz
     static int i = 0;
     i++;
     if (i > 100)
@@ -46,12 +134,6 @@ void controlFunc()
         i = 0;
         ledOrg = !ledOrg;
     }
-
-    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
@@ -67,7 +149,7 @@ void pulseLedContext()
             // 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);
+            ledBlue.write(out);
             Thread::wait(10);
         }
     }
@@ -78,49 +160,55 @@ int i = 0;
 // main() runs in its own thread
 int main() {
 
-    // Start sweeping the arm
+    // MPU startup at 100hz
-    servo.sweep(0.0, 1, 2);
-
-    Thread ledPulseThread;
-    ledPulseThread.start(callback(&pulseLedContext));
-
-    // 
-    Thread eventThread;
-    eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
-
-    // Function to attach to gyro interrupt
-    gyroINT.rise(queue.event(&controlFunc));
-
-
-    // Enable motor controllers (Will power the motors with no movement)
-    motor1.enable();
-    motor2.enable();
-
-    // Set motor 2 to a fixed speed 
-    // (To test that timers don't interfeer with each other)
-    motor2.setSpeed(90.0);
-
-
-    // MPU startup
     if(imu.init(10,BITS_DLPF_CFG_188HZ)){
         printf("\nCouldn't initialize MPU6000 via SPI!");
     }  
 
+    wait(0.1);
+    // Set IMU scale
+    int accScale = imu.set_acc_scale(BITS_FS_16G);
+    wait(0.1);
+    int gyroScale = imu.set_gyro_scale(BITS_FS_2000DPS);
+    wait(0.1);
+
+    // Calibrate and Trim acc & gyro
+    bool calibrationResult = imu.resetOffset_gyro();
+    imu.calib_acc(0);
+    calibrationResult = imu.resetOffset_acc();
+
     // Enable/Activate the Gyro interrupt
     imu.enableInterrupt();
 
-    // Read some test values
+    // Start the timer used by the control loop
-    int wmi = imu.whoami();
+    timer.start(); // Used to calc dT
-    int scale = imu.set_acc_scale(BITS_FS_16G);
+
+    /*-------------- Visible start sequence ------------*/
+    // Start sweeping the arm
+    servo.sweep(0.0, 1, 2);
+
+    // Start the pulsing blue led
+    Thread ledPulseThread;
+    ledPulseThread.start(callback(&pulseLedContext));
+
+    // Create realtime eventhandler for control loop
+    Thread eventThread(osPriorityRealtime);
+    eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
+
+    // Attach gyro interrupt to add a control event
+    gyroINT.rise(queue.event(&controlFunc));
+
+    // Enable motor controllers (Will power the motors with no movement)
+    motorL.enable();
+    motorR.enable();
+    motorL.setDirection(1);
+    motorR.setDirection(-1);
 
     Thread::wait(1000);
 
+    // Servo Nod to tell us that we are done
     servo.nod();
 
-    Thread::wait(1000);
-
-    servo.sweep(1.0, 1.5, 3);
-
     servo.setPosition(-0.2);
 
     wait(osWaitForever);

src/control/ImuFusion.cpp

@@ -8,6 +8,7 @@ ImuFusion::ImuFusion(mpu6000_spi* pImu)
 {
 }
 
+// TODO: Implement for all axis
 float ImuFusion::getAngle(float dT)
 {
     int axis = 0;