Diff: main.cpp

Revision:

3:8898bbbe00d6

Parent:

0:e8167f37725c

--- a/main.cpp	Wed Feb 03 18:24:29 2016 +0000
+++ b/main.cpp	Fri Dec 06 13:47:56 2019 +0000
@@ -1,29 +1,107 @@
+/*
+    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() {
-    //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
-    LSM9DS1 lol(p9, p10, 0xD6, 0x3C);
-    lol.begin();
-    if (!lol.begin()) {
+    
+    imu.begin();
+    if (!imu.begin()) 
+    {
         pc.printf("Failed to communicate with LSM9DS1.\n");
+        exit(0);
     }
-    lol.calibrate();
-    while(1) {
-        lol.readTemp();
-        lol.readMag();
-        lol.readGyro();
-        
-        //pc.printf("%d %d %d %d %d %d %d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz), lol.ax, lol.ay, lol.az, lol.mx, lol.my, lol.mz);
-        //pc.printf("%d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz));
-        pc.printf("gyro: %d %d %d\n\r", lol.gx, lol.gy, lol.gz);
-        pc.printf("accel: %d %d %d\n\r", lol.ax, lol.ay, lol.az);
-        pc.printf("mag: %d %d %d\n\n\r", lol.mx, lol.my, lol.mz);
-        myled = 1;
-        wait(2);
-        myled = 0;
-        wait(2);
+    
+    // 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)
+    {    
     }
 }