File content as of revision 3:8898bbbe00d6:

/*
    4180Balancer
    Names: John Lee, Nyle Malik, Austin Lowe
    Date: 12/6/19
    Description:
        Balances a robot on two wheels using a PID controller for fine tuning the 
        motor speed and behavior. It uses an IMU to send input to the PID controller
        and calibrate the robot's desired angle.
*/

#include "LSM9DS1.h"
#include "Motor.h"
#include "mbed.h"
#include <time.h>
#include <stdlib.h>

#define RAD_TO_DEG 57.29578
#define M_PI 3.14159265358979323846
#define FILTER_CONSTANT 0.98
#define DT 0.001

DigitalOut myled(LED1);
LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
Motor left(p21, p7, p8);
Motor right(p22, p11, p12);
Ticker PID;
Serial pc(USBTX, USBRX);

//Globals
//Configure these
float scalingFactor = 2000;
float KP = 100;
float KI = 100;
float KD = 1000;

float aBias = 0;
float gBias = 0;
float aAngle = 0;
float gAngle = 0;
float intAngle = 0;
float derivAngle = 0;
float propAngle = 0;
float angle = 0;   
float speed = 0;

void calibrate()
{
    for(int i=0; i<100; i++) {                            
        imu.readAccel();                                  
        imu.readGyro();                                   
        aBias=aBias+(-1)*atan2((double)(imu.ay),(double)(imu.az))*180/3.142-90; 
        gBias=gBias+imu.gx;                               
    }
    aBias=aBias/100;                                      
    gBias=gBias/100;                                      
}

void pid()
{
     // Save the previous data
     derivAngle = propAngle;
     
     //Read data from the IMU
     imu.readAccel();
     imu.readGyro();
     
     //Controls and PID calculations
     aAngle = (-1) * atan2((double)(imu.ay), (double)(imu.az)) * RAD_TO_DEG - 90 - aBias;
     gAngle = (-1)*(imu.gx - gBias) * DT; 
     propAngle = FILTER_CONSTANT * (propAngle + gAngle) + (1 - FILTER_CONSTANT) * aAngle - angle;
     derivAngle = propAngle - derivAngle;
     intAngle += (propAngle * DT);     {
     speed = -((KP * propAngle) + (KI * intAngle) + (KD * derivAngle)) / scalingFactor;
     if (speed < -1)
     {
         speed = -1;
     }else if (speed > 1)
     {
         speed = 1;
     }
     
     // Set the speed of the motors
     left.speed(speed);
     right.speed(speed);  
}

int main() {
    
    imu.begin();
    if (!imu.begin()) 
    {
        pc.printf("Failed to communicate with LSM9DS1.\n");
        exit(0);
    }
    
    // Calibrate the IMU and set the biases
    calibrate();
    
    // Attach ticker to run every 10ms
    // This handles the pid calculations for controlling the motors
    PID.attach(&pid, 0.001);
    
    // Program loop
    while(1)
    {    
    }
}