Diff: main.cpp

Revision:

0:67f5b4041c15

Child:

1:bf71a7bd2d3e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 06 17:09:31 2015 +0000
@@ -0,0 +1,216 @@
+/*////////////////////////////////////////////////////////////////
+ECE 4180 Mini Project
+Balancing Robot
+Nico van Duijn
+Samer Mabrouk
+3/6/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
+*/////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////
+// Includes
+#include "mbed.h"                                               // mbed library
+#include "LSM9DS0.h"                                            // 9axis IMU
+#include "math.h"                                               // We need to be able to do trig
+
+//////////////////////////////////////////////////////////////////
+// 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
+
+//////////////////////////////////////////////////////////////////
+// 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 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
+Ticker start;                                                   // Initialize ticker to call control function
+#ifdef DEBUG
+Serial pc(USBTX, USBRX);                                        // Creates virtual Serial connection though USB
+#endif
+
+//////////////////////////////////////////////////////////////////
+// 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 accBias = 0;                                              // Accelerometer Bias
+float gyroBias = 0;                                             // Gyro Bias
+float accAngle = 0;                                             // Global to hold angle measured by Accelerometer
+float gyroAngle = 0;                                            // This variable holds the amount the angle has changed
+float speed = 0;                                                // Variable for motor speed
+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)
+
+//////////////////////////////////////////////////////////////////
+// Function Prototypes
+void drive(float);                                              // Function declaration for driving the motors
+void calibrate();                                               // Function to calibrate gyro and accelerometer
+void control();                                                 // Function implementing PID control
+#ifdef DEBUG
+void callback();                                                // Interrupt triggered function for serial communication
+#endif
+//////////////////////////////////////////////////////////////////
+// Main function
+int main()
+{
+    uint16_t status = imu.begin();                              // Use the begin() function to initialize the LSM9DS0 library.
+#ifdef DEBUG
+    pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);   // Make sure communication is working
+    pc.printf("Should be 0x49D4\n\n");                          // Check if we're talking to the right guy
+    pc.attach(&callback);                                       // Attach interrupt to serial communication
+#endif
+    calibrate();                                                // Calibrate gyro and accelerometer
+    start.attach(&control, 0.01);                               // Looptime 10ms ca. 100Hz
+    while(1) {                                                  // Stay in this loop forever
+#ifdef DEBUG
+        pc.printf("%f\n",speed);                                // Print to USB the speed
+#endif
+    }
+}
+/////////////////////////////////////////////////////////////////
+// Control function, implements PID
+void control()
+{
+    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
+        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
+}
+//////////////////////////////////////////////////////////////////
+// Drive function
+void drive(float speed)
+{
+    int direction=0;                                            // Variable to hold direction we want to drive
+    if (speed>0)direction=1;                                    // Positive speed indicates forward
+    if (speed<0)direction=2;                                    // Negative speed indicates backwards
+    if(speed==0)direction=0;                                    // Zero speed indicates stopping
+    switch( direction) {                                        // Depending on what direction was passed
+        case 0:                                                 // Stop case
+            motorSpeedLeft=0;                                   // Set the DigitalOuts to stop the motors
+            motorSpeedRight=0;
+            motorDirLeft=0;
+            NmotorDirLeft=0;
+            motorDirRight=0;
+            NmotorDirRight=0;
+            break;
+        case 1:                                                 // Forward case
+            motorSpeedLeft=speed;                               // Set the DigitalOuts to run the motors forward
+            motorSpeedRight=speed;
+            motorDirLeft=1;
+            NmotorDirLeft=0;
+            motorDirRight=1;
+            NmotorDirRight=0;
+            break;
+        case 2:                                                 // Backwards
+            motorSpeedLeft=-speed;                              // Set the DigitalOuts to run the motors backward
+            motorSpeedRight=-speed;
+            motorDirLeft=0;
+            NmotorDirLeft=1;
+            motorDirRight=0;
+            NmotorDirRight=1;
+            break;
+        default:                                                // Catch-all (Stop)
+            motorSpeedLeft=0;                                   // Set the DigitalOuts to stop the motors
+            motorSpeedRight=0;
+            motorDirLeft=0;
+            NmotorDirLeft=0;
+            motorDirRight=0;
+            NmotorDirRight=0;
+            break;
+    }
+}
+
+//////////////////////////////////////////////////////////////////
+// Calibrate function to find gyro drift and accelerometer bias
+void calibrate()
+{
+    for(int i=0; i<1000; 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
+#ifdef DEBUG
+    pc.printf("accBias: %f gyroBias: %f\n",accBias, gyroBias);  // Print biases to USB
+#endif
+}
+
+#ifdef DEBUG
+/////////////////////////////////////////////////////////////////
+// Callback function to change values/gains
+void callback()
+{
+    char val;                                                   // Needed for Serial communication (need to be global?)
+    val = pc.getc();                                            // Reat the value from Serial
+    pc.printf("%c\n", val);                                     // Print it back to the screen
+    if( val =='p') {                                            // If character was a 'p'
+        pc.printf("enter kp \n");                               // Adjust kp
+        val = pc.getc();                                        // Wait for kp value
+        if(val == 0x2b) {                                       // If character is a plus sign
+            kp=kp+10;                                           // Increase kp
+        } else if (val == 0x2d) {                               // If recieved character is the minus sign
+            kp=kp-10;                                           // Decrease kp
+        } else {
+            kp = val - 48;                                      // Cast char to float
+        }
+        pc.printf(" kp = %f \n",kp);                            // Print current kp value to screen
+    } else if( val == 'd') {                                    // Adjust kd
+        pc.printf("enter kd \n");                               // Wait for kd
+        val= pc.getc();                                         // Read value from serial
+        if(val == '+') {                                        // If given plus sign
+            kd++;                                               // Increase kd
+        } else if (val == '-') {                                // If given negative sign
+            kd--;                                               // Decrease kd
+        } else {                                                // If given some other ascii (a number?)
+            kd = val - 48;                                      // Set derivative gain
+        }
+        pc.printf(" kd = %f \n",kd);                            // Print kd back to screen
+    } else if( val == 'i') {                                    // If given i - integral gain
+        pc.printf("enter ki \n");                               // Prompt on screen to ask for ascii
+        val= pc.getc();                                         // Get the input
+        if(val == '+') {                                        // If given the plus sign
+            ki++;                                               // Increase ki
+        } else if (val == '-') {                                // If given the minus sign
+            ki--;                                               // Decrease ki
+        } else {                                                // If given some other ascii
+            ki = val - 48;                                      // Set ki to that number
+        }
+        pc.printf(" ki = %f \n",ki);
+    } else if( val == 'o') {
+        pc.printf("enter OVERALL_SCALAR \n");
+        val= pc.getc();
+        if(val == '+') {
+            OVERALL_SCALAR=OVERALL_SCALAR+1000;
+        } else if (val == '-') {
+            OVERALL_SCALAR=OVERALL_SCALAR-1000;
+        } else {
+            OVERALL_SCALAR=(val-48)*1000;;
+        }
+        pc.printf(" OVERALL_SCALAR = %f \n",OVERALL_SCALAR);
+    }
+}
+#endif
\ No newline at end of file