main.cpp

00001 /*
00002     4180Balancer
00003     Names: John Lee, Nyle Malik, Austin Lowe
00004     Date: 12/6/19
00005     Description:
00006         Balances a robot on two wheels using a PID controller for fine tuning the 
00007         motor speed and behavior. It uses an IMU to send input to the PID controller
00008         and calibrate the robot's desired angle.
00009 */
00010 
00011 #include "LSM9DS1.h"
00012 #include "Motor.h"
00013 #include "mbed.h"
00014 #include <time.h>
00015 #include <stdlib.h>
00016 
00017 #define RAD_TO_DEG 57.29578
00018 #define M_PI 3.14159265358979323846
00019 #define FILTER_CONSTANT 0.98
00020 #define DT 0.001
00021 
00022 DigitalOut myled(LED1);
00023 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
00024 Motor left(p21, p7, p8);
00025 Motor right(p22, p11, p12);
00026 Ticker PID;
00027 Serial pc(USBTX, USBRX);
00028 
00029 //Globals
00030 //Configure these
00031 float scalingFactor = 2000;
00032 float KP = 100;
00033 float KI = 100;
00034 float KD = 1000;
00035 
00036 float aBias = 0;
00037 float gBias = 0;
00038 float aAngle = 0;
00039 float gAngle = 0;
00040 float intAngle = 0;
00041 float derivAngle = 0;
00042 float propAngle = 0;
00043 float angle = 0;   
00044 float speed = 0;
00045 
00046 void calibrate()
00047 {
00048     for(int i=0; i<100; i++) {                            
00049         imu.readAccel();                                  
00050         imu.readGyro();                                   
00051         aBias=aBias+(-1)*atan2((double)(imu.ay),(double)(imu.az))*180/3.142-90; 
00052         gBias=gBias+imu.gx;                               
00053     }
00054     aBias=aBias/100;                                      
00055     gBias=gBias/100;                                      
00056 }
00057 
00058 void pid()
00059 {
00060      // Save the previous data
00061      derivAngle = propAngle;
00062      
00063      //Read data from the IMU
00064      imu.readAccel();
00065      imu.readGyro();
00066      
00067      //Controls and PID calculations
00068      aAngle = (-1) * atan2((double)(imu.ay), (double)(imu.az)) * RAD_TO_DEG - 90 - aBias;
00069      gAngle = (-1)*(imu.gx - gBias) * DT; 
00070      propAngle = FILTER_CONSTANT * (propAngle + gAngle) + (1 - FILTER_CONSTANT) * aAngle - angle;
00071      derivAngle = propAngle - derivAngle;
00072      intAngle += (propAngle * DT);     {
00073      speed = -((KP * propAngle) + (KI * intAngle) + (KD * derivAngle)) / scalingFactor;
00074      if (speed < -1)
00075      {
00076          speed = -1;
00077      }else if (speed > 1)
00078      {
00079          speed = 1;
00080      }
00081      
00082      // Set the speed of the motors
00083      left.speed(speed);
00084      right.speed(speed);  
00085 }
00086 
00087 int main() {
00088     
00089     imu.begin();
00090     if (!imu.begin()) 
00091     {
00092         pc.printf("Failed to communicate with LSM9DS1.\n");
00093         exit(0);
00094     }
00095     
00096     // Calibrate the IMU and set the biases
00097     calibrate();
00098     
00099     // Attach ticker to run every 10ms
00100     // This handles the pid calculations for controlling the motors
00101     PID.attach(&pid, 0.001);
00102     
00103     // Program loop
00104     while(1)
00105     {    
00106     }
00107 }