Diff: main.cpp

Revision:

3:dd0c62b586ea

Parent:

2:ad080363a22c

Child:

4:21dfcd81b397

--- a/main.cpp	Mon Apr 06 16:25:18 2015 +0000
+++ b/main.cpp	Mon Apr 20 16:33:09 2015 +0000
@@ -1,16 +1,22 @@
 /*////////////////////////////////////////////////////////////////
-ECE 4180 Mini Project
+ECE 4180 Final Project
 Balancing Robot
+
 Nico van Duijn
 Samer Mabrouk
-3/6/2015
+Anthony Agnone
+Jay 
+4/20/2015
 
 This project consists of a robot balancing on two wheels. We use
 the 9-axis IMU LSM9DS0 to give us Accelerometer and Gyroscope data
 about the current angle and angular velocity of the robot. This
 data is then filtered using complementary filters and PID control
 to drive the two motors. The motors are controlled through digital
-outputs in their direction and their seepd by PWM using an H-bridge
+outputs in their direction and their seepd by PWM using an H-bridge.
+The robot receives steering commands via the XBee module which is
+interfaced with from a C# GUI that runs on a desktop computer.
+
 */////////////////////////////////////////////////////////////////
 
 //////////////////////////////////////////////////////////////////
@@ -23,14 +29,14 @@
 // Constants
 #define LSM9DS0_XM_ADDR  0x1D                                   // Would be 0x1E if SDO_XM is LOW
 #define LSM9DS0_G_ADDR   0x6B                                   // Would be 0x6A if SDO_G is LOW
-#define DEBUG                                                   // Comment this out for final version
+//#define DEBUG                                                   // Comment this out for final version
 
 //////////////////////////////////////////////////////////////////
 // I/O and object instatiation
 PwmOut motorSpeedLeft(p21);                                     // PWM port to control speed of left motor
 PwmOut motorSpeedRight(p22);                                    // PWM port to control speed of right motor
-DigitalOut motorDirLeft(p23);                                   // Digital pin for direction of left motor
-DigitalOut NmotorDirLeft(p24);                                  // Usually inverse of motorDirLeft
+DigitalOut motorDirLeft(p24);                                   // Digital pin for direction of left motor
+DigitalOut NmotorDirLeft(p23);                                  // Usually inverse of motorDirLeft
 DigitalOut motorDirRight(p26);                                  // Digital pin for direction of right motor
 DigitalOut NmotorDirRight(p25);                                 // Usually inverse of motorDirRight
 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);          // Creates on object for IMU
@@ -41,10 +47,10 @@
 
 //////////////////////////////////////////////////////////////////
 // Globals
-float kp = 800;                                                 // Proportional gain
-float kd = 90;                                                  // Derivative gain
-float ki = 4;                                                   // Integrative gain
-float OVERALL_SCALAR = 7000;                                    // Overall scalar that speed is divided by
+float kp = 98;   //40                                              // 145 Proportional gain kU 400-500
+float kd = 200;   //2 was quite good                              // Derivative gain
+float ki = 935;    //30                                              // Integrative gain
+float OVERALL_SCALAR = 170;                                    // Overall scalar that speed is divided by
 float accBias = 0;                                              // Accelerometer Bias
 float gyroBias = 0;                                             // Gyro Bias
 float accAngle = 0;                                             // Global to hold angle measured by Accelerometer
@@ -53,6 +59,8 @@
 float iAngle = 0;                                               // Integral value of angle-error (sum of gyro-angles)NOT EQUAL TO gyroAngle
 float dAngle = 0;                                               // Derivative value for angle, angular velocity, how fast angle is changing
 float pAngle = 0;                                               // Proportional value for angle, current angle (best measurement)
+float desiredAngle=0;                                           // Setpoint. Set unequal zero to drive
+// float turnspeed=0;
 
 //////////////////////////////////////////////////////////////////
 // Function Prototypes
@@ -86,19 +94,25 @@
 // Control function, implements PID
 void control()
 {
+    dAngle=pAngle;// remember old p-value
     imu.readGyro();                                             // Read the gyro
     imu.readAccel();                                            // Read the Accelerometer
     accAngle=(-1)*atan2(imu.ay,imu.az)*180/3.142-90-accBias;    // Like this, 0 is upright, forward is negative, backward positive
     gyroAngle=-(imu.gx-gyroBias)*0.01;                          // This is deltaangle, how much angle has changed
-    pAngle=0.98*(pAngle+gyroAngle)+0.02*accAngle;               // Complementary filter yields best value for current angle
-    dAngle = -(imu.gx-gyroBias);                                // This is angular velocity
-    iAngle=(0.99*iAngle+0.01*gyroAngle);                        // Sorta- running average-integral thing
-    if(abs(pAngle)>=0.6&&abs(pAngle)<=25) {                     // If it is tilted enough, but not too much
+    pAngle=0.98*(pAngle+gyroAngle)+0.02*accAngle-desiredAngle;               // Complementary filter yields best value for current angle
+    //iAngle=(0.9*iAngle+0.1*gyroAngle);     DOESNT ACTUALLY INTEGRATE ERROR      // Sorta- running average-integral thing
+    dAngle=pAngle-dAngle;                                       //Ang. Veloc. less noisy than dAngle = -(imu.gx-gyroBias);
+    iAngle+=(pAngle*.01);// integrate the angle (multiply by timestep to get dt!)
+    //if((ki*iAngle/OVERALL_SCALAR)>=3)iAngle=3*OVERALL_SCALAR/ki;// if ki dominates three-fold
+    //if((ki*iAngle/OVERALL_SCALAR)<=-3)iAngle=-3*OVERALL_SCALAR/ki;//50 is an arbitrary cap - kind of worked
+    // try angle dev. .55 and find value for imu.gx
+    if(abs(pAngle-desiredAngle)>=0.6&&abs(pAngle-desiredAngle)<=15) {                    // If it is tilted enough, but not too much
         speed=-(ki*iAngle+kd*dAngle+kp*pAngle)/OVERALL_SCALAR;  // drive to correct
         if(speed<-1) speed=-1;                                  // Cap if undershoot
         else if(speed>1) speed=1;                               // Cap if overshoot
     } else speed=0;                                             // If we've fallen over or are steady on top
     drive(speed);                                               // Write speed to the motors
+    
 }
 
 //////////////////////////////////////////////////////////////////
@@ -146,17 +160,17 @@
 }
 
 //////////////////////////////////////////////////////////////////
-// Calibrate function to find gyro drift and accelerometer bias
+// Calibrate function to find gyro drift and accelerometer bias accbias: -0.3 gyrobias: +0.15
 void calibrate()
 {
-    for(int i=0; i<1000; i++) {                                 // Read a thousand values
+    for(int i=0; i<100; i++) {                                 // Read a thousand values
         imu.readAccel();                                        // Read the Accelerometer
         imu.readGyro();                                         // Read the gyro
         accBias=accBias+(-1)*atan2(imu.ay,imu.az)*180/3.142-90; // Like this, 0 is upright, forward is negative, backward positive
         gyroBias=gyroBias+imu.gx;                               // Add up all the gyro Biases
     }
-    accBias=accBias/1000;                                       // Convert sum to average
-    gyroBias=gyroBias/1000;                                     // Convert sum to average
+    accBias=accBias/100;                                       // Convert sum to average
+    gyroBias=gyroBias/100;                                     // Convert sum to average
 #ifdef DEBUG
     pc.printf("accBias: %f gyroBias: %f\n",accBias, gyroBias);  // Print biases to USB
 #endif