Balancing success! With acceptable tune

Still playing with acceleration limits and constrains on outputs and buffers in the controllers but robot works quite good now.

main.cpp

@@ -18,6 +18,7 @@
 #include "src/control/lpf.h"
 #include "src/control/PID.h"
 #include "src/control/ImuFusion.h"
+#include "src/control/MadgwickAHRS.h"
 // Serialization
 #include "src/serialization/RCProtocol.h"
 
@@ -32,7 +33,12 @@ using namespace serialization;
 EventQueue queue;
 
 // Serial port (Servo Outputs)
-Serial serial(PA_2, PA_3, 57600);
+RawSerial serial(PA_2, PA_3, 250000);
+RCProtocol RC;
+
+// Pool to send Remote Control packages between threads
+MemoryPool<RCProtocol::Packet, 16> mpool;
+Queue<RCProtocol::Packet, 16> RCQueue;
 
 // MPU setup
 SPI spi(PA_7, PA_6, PA_5);   //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz
@@ -41,88 +47,136 @@ mpu6000_spi imu(spi,PA_4);   //define the mpu6000 object
 PwmOut ledBlue(D4);
 DigitalOut ledOrg(D5);
 
+// Stepper motorL(PC_9, PB_1, PC_8);
 Stepper motorL(PC_9, PC_7, PC_8);
 Stepper motorR(PB_15, PB_14, PC_6);
 
 Servo servo(PA_0);
 
 // Interrupt pin from Gyro to MCU
-InterruptIn gyroINT(PC_4);
+InterruptIn gyroISR(PC_4);
-
+// Madwick filter
+Madgwick madgwick;
+// Timer to calculate dT
 Timer timer;
 
-// TODO: Figure out some good values
+// A PI controller for throttle control
-controllerPI throttleControl(0.0025, 0.01, 5, 0); // 0.065, 0.05, 12, 40
+controllerPI throttleControl(35.0, 3.0, 20, 60); // 35.0, 3.0, 20, 60
 
-// TODO: Figure out some good values
+// A PID for angle control
-controllerPD angleControl(10.0, 146.0, 400);
+controllerPID angleControl(100.0f, 200.0f, 0.5f, 4000, 2000); // 100.0f, 200.0f, 0.5f, 4000, 2000
-// Alternatively try controllerPD2 which has another Dterm implementation
 
-// Draft of some control function
+// The control function to balance the robot
-// this runs in the context of eventThread and is triggered by the gyro ISR pin
+// Ran at high prio and triggered as an event by the gyro Interrupt pin
-void controlFunc()
+void runControl()
 {
     static ImuFusion imuFusion(&imu);
     static float controlOutput(0.0f);
 
+    static RCProtocol::Packet remote;
+    osEvent evt = RCQueue.get(0);
+    if (evt.status == osEventMessage) {
+        RCProtocol::Packet* pPacket = (RCProtocol::Packet*)evt.value.p;
+        remote = *pPacket;
+        mpool.free(pPacket);
+
+        throttleControl.setGainScaling(remote.Ki, remote.Kd);
+        //throttleControl.setGainScaling(0, 0);
+
+        //angleControl.setGainScaling(remote.Kp, remote.Ki, remote.Kd);
+        angleControl.setGainScaling(remote.Kp, 0, -900);
+
+        servo.setPosition((float)remote.Throttle/1000.0f);
+    }
+    
+
     // Calculate dT in sec
-    float dT = timer.read_us()/(float)1000000.0;
+    float dT = timer.read_us()/(float)10e6;
     timer.reset();
 
-    // Retrieve IMU angle (Only x-axis implemented for now)
+    // Retrieve IMU angle
-    float angleX = imuFusion.getAngle(dT);
+    madgwick.updateIMU(imu.read_rot(0),
-    float forceX = sin(angleX/180*PI);
+                       imu.read_rot(1),
+                       imu.read_rot(2),
+                       imu.read_acc(0),
+                       imu.read_acc(1), 
+                       imu.read_acc(2));
+    float angleX = madgwick.getPitch();
+
     // 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)
+    if (!remote.Enabled || (abs(angleX) > (float)50.0f && !disabled))
     {
         controlOutput = 0.0f; // rinse integral
         disabled = true;
         motorL.disable();
         motorR.disable();
     }
-    else if (abs(angleX) < (float)50.0f && disabled)
+    else if (remote.Enabled && abs(angleX) < (float)10.0f && disabled)
     {
         disabled = false;
         motorL.enable();
         motorR.enable();
     }
 
+    if (disabled) 
+    { 
+        throttleControl.flushIntegral(); 
+        angleControl.flushIntegral();
+    }
+
     /* --------------------------
     Calculate estimated groundspeed 
-    of the robot. 
+    of the robot in m/s
-    TODO: Inject correct input (compensate for robot rotation)
     -------------------------- */
-    // Calculate filtered groundspeed of the robot (From deg/s to m/s)
+    // Static variables
-    float estimatedSpeed((motorL.getSpeed() + motorR.getSpeed())/2);
+    static float angleXOld = 0;
-    static incrementalLPF speedFilter;
+    static float motorAngularVelocity = 0;
-    float filteredEstSpeed(speedFilter.filter(estimatedSpeed));
+    static float motorAngularVelocityOld = 0;
-    float speedScale((1.0f/360.0f)*WHEEL_SIZE*PI);
+    static incrementalLPF filteredGroundSpeed;
-    float groundSpeed(filteredEstSpeed*speedScale);
+
+    if (disabled) 
+    {
+        angleXOld = 0;
+        motorAngularVelocity = 0;
+        motorAngularVelocityOld = 0;
+        filteredGroundSpeed.clear();
+    }
+    
+    // angular velocity - This way we get use of the madgwick filter
+    float angularVelocity = (angleX - angleXOld) * dT;
+    angleXOld = angleX;
+
+    // Motor Speed - Convert from deg/s to m/s
+    motorAngularVelocityOld = motorAngularVelocity;
+    motorAngularVelocity = ((motorL.getSpeed() + motorR.getSpeed())/2);
+
+    float estimatedGroundSpeed = 
+        (motorAngularVelocityOld - angularVelocity)/360.0f*PI*WHEEL_SIZE;
+    estimatedGroundSpeed = filteredGroundSpeed.filter(estimatedGroundSpeed);
 
     /* --------------------------
     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 throttle = remote.Throttle*0.001f;
-    float angleSP = 0.0f; // Temporarily emulate an output
-    //float angleSP = throttleControl.run(dT, groundSpeed, throttle);
 
+    if (remote.Throttle < 50 && remote.Throttle > -50) {throttle = 0;}
+    
+    // TODO: Figure out why we have the wrong sign here. I also had to reverse motors
+    // For the angle control loop. Is the madgwick filter outputting the wrong sign?
+    float angleSP = -throttleControl.run(dT, estimatedGroundSpeed, throttle);
 
     /* --------------------------
     The last control loop. Angle (PD)
-    TODO:
     -------------------------- */
-    // Integrating the output to obtain acceleration
     if (!disabled)
     {
-        controlOutput += angleControl.run(dT, forceX, angleSP);
+        controlOutput = angleControl.run(dT, angleX, angleSP);
 
         controlOutput = constrain(controlOutput, 3000.0f);
     }
@@ -130,27 +184,22 @@ void controlFunc()
     /* --------------------------
      Lastly the steering is added
      straight on the output
-     TODO: Activate when implemented
     -------------------------- */
-    float steering(0.0f); // This will come from remote
+    float steering = remote.Steering*0.5;
+    if (remote.Steering < 50 && remote.Steering > -50) {steering = 0;}
     motorL.setSpeed(controlOutput - steering);
     motorR.setSpeed(controlOutput + steering);
 
     // Blink LED at 1hz
     static int i = 0;
-    i++;
+    if (++i > (disabled ? 25 : 100))
-    if (i > 100)
     {
         i = 0;
         ledOrg = !ledOrg;
     }
 }
 
-void serialContext()
+void serialISR()
-{
-    static RCProtocol RC;
-
-    while (true)
 {
     // Receive
     while (serial.readable())
@@ -159,16 +208,17 @@ void serialContext()
 
         if (newPackage)
         {
-                RCProtocol::Packet packet = RC.read();
+            RCProtocol::Packet* pPacket = mpool.alloc();
+            *pPacket = RC.read();
+            RCQueue.put(pPacket);
         }
     }
     // Sleep some?
+    //Thread::wait(1);
     
     // Transmit 
     // serial.putc()...
 }
-}
-
 
 // This context just pulses the blue LED
 void pulseLedContext()
@@ -192,13 +242,9 @@ void pulseLedContext()
 // main() runs in its own thread
 int main() {
 
-    // Serial in / out
-    Thread readUartThread;
-    readUartThread.start(callback(&serialContext));
-
     // MPU startup at 100hz
     if(imu.init(10,BITS_DLPF_CFG_188HZ)){
-        printf("\nCouldn't initialize MPU6000 via SPI!");
+        //printf("\nCouldn't initialize MPU6000 via SPI!");
     }  
 
     wait(0.1);
@@ -213,39 +259,54 @@ int main() {
     imu.calib_acc(0);
     calibrationResult = imu.resetOffset_acc();
 
+    // Setup Madwick filter
+    madgwick.begin(100);
+
+    /*-------------- Visible start sequence ------------*/
+    // Start sweeping the arm
+    //servo.sweep(0.0, 1, 2);
+
+    // Enable motor controllers (Will power the motors with no movement)
+    // motorL.enable();
+    // motorR.enable();
+    motorL.disable();
+    motorR.disable();
+    motorL.setDirection(-1);
+    motorR.setDirection(1);
+
+    // Servo Nod to tell us that we are done
+    //servo.nod();
+
+    //servo.setPosition(-0.2);
+
+    /*********************** Start all threads last **********************/
+    
+    // Start the pulsing blue led
+    ledBlue.period_ms(10); // Any period. This will be overwritten by motors
+    Thread ledPulseThread;
+    //ledPulseThread.start(callback(&pulseLedContext));
+    Thread::wait(100);
+
+    // Serial in / out
+    serial.set_dma_usage_rx(DMA_USAGE_ALWAYS);
+    serial.attach(&serialISR);
+    Thread::wait(100);
+
+    // ISR won't be called if the serial is not emptied first.
+    serialISR();
+
     // Enable/Activate the Gyro interrupt
     imu.enableInterrupt();
 
     // Start the timer used by the control loop
     timer.start(); // Used to calc dT
 
-    /*-------------- 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);
+    Thread eventThread(osPriorityHigh);
     eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
 
     // Attach gyro interrupt to add a control event
-    gyroINT.rise(queue.event(&controlFunc));
+    gyroISR.rise(queue.event(&runControl));
-
-    // 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();
-
-    servo.setPosition(-0.2);
 
     wait(osWaitForever);
 }

src/drivers/stepper.cpp

@@ -12,7 +12,7 @@ Stepper::Stepper(PinName stepPin,
 , m_dir(dirPin)
 , m_en(enPin)
 , m_accelerationLimitOn(true)
-, m_accelerationLimit(50.0f)
+, m_accelerationLimit(80.0f)
 , m_stepsPerRevolution(200)
 , m_microStepResolution(8)
 , m_currentPeriod(0)