File content as of revision 4:21dfcd81b397:

/*////////////////////////////////////////////////////////////////
ECE 4180 Final Project
Self Balancing Robot

Drew Paschal
4/20/2019

Project features

*/////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////
// Includes
#include "mbed.h"                                               // mbed library
#include "LSM9DS1.h"                                            // 9axis IMU
#include "math.h"                                               // used for atan2()
#include "motordriver.h"
//////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////
// I/O and object instatiation
LSM9DS1 imu(p9, p10, 0xD6, 0x3C);                               // Creates on object for IMU
Ticker start;                                                   // Initialize ticker to call control function

Serial pc(USBTX, USBRX);                                        // Serial connection though USB

Motor  left(p22, p23, p24, 1); // pwm, fwd, rev, brake
Motor right(p21, p25, p26, 1); //
//////////////////////////////////////////////////////////////////
// Globals
float kp = 5;                                                   // Proportional gain
float kd = .25;                                                 // Derivative gain
float ki = 0;                                                   // Integrative gain
float OVERALL_SCALAR = 120;                                     // Overall normalization constant
float accTheta = 0;                                             // Global to hold angle measured by Accelerometer
float gyroTheta = 0;                                            // Global to hold dps measured by Gryoscope
float speed = 0;                                                // Variable for motor speed (PWM signal)
float iTheta = 0;                                               // Integral value of angle-error (sum of gyro-angles)
float dTheta = 0;                                               // Derivative value, angular velocity 
float pTheta = 0;                                               // Proportional value, current angle 
float desiredTheta=0;                                           // Reference angle 

float thetaError = 0;                                           // Error input for PID loop
float tempError = 0;                                            // Error temp variable

//////////////////////////////////////////////////////////////////
// Function(s)                                                  
void controlPID();                                              // Function implementing PID control
//////////////////////////////////////////////////////////////////
// Main function
int main()
{
    uint16_t status = imu.begin();                              
    imu.calibrate(1);
    start.attach(&controlPID, 0.01);                            // loop frequency 100Hz
    while(1) {                                                 

       pc.printf("%f\n\r",speed);   

    }
}

/////////////////////////////////////////////////////////////////
// Control function, implements PID
void controlPID()
{
    dTheta=pTheta;// use previous angle value
    imu.readGyro();                                             // Read the Gyro
    imu.readAccel();                                            // Read the Accelerometer
    accTheta=(-1)*atan2(imu.calcAccel(imu.ax),                  //
    imu.calcAccel(imu.az))*180/3.14159;                         // Like this, 0 is upright, forward is negative, backward positive
    gyroTheta=-(imu.calcGyro(imu.gy))*0.01;                     // Calculates small deltaTheta angle to be used in the complementary filter
    pTheta=0.98*(pTheta+gyroTheta)+0.02*accTheta;               // Complementary filter yields best value for current angle
    dTheta=pTheta-dTheta;                                       // Angular velocity, the difference is used instead of using a deltaTheta and  
    thetaError  = desiredTheta - pTheta;                        // Error to be used in PID control 
    iTheta += thetaError*.01;                                   // Sum of present angle error scaled by small time step
    speed  = (kp * thetaError + 
                kd * dTheta + 
                ki * iTheta) / OVERALL_SCALAR; 
    left.speed(speed);                                          // Write speed to the motors
    right.speed(speed);
}