Robot that balances on two wheels

Dependencies:   LSM9DS0 mbed

Files at this revision

API Documentation at this revision

Comitter:
nicovanduijn
Date:
Wed Apr 22 16:24:21 2015 +0000
Child:
1:a079ae75a86c
Commit message:
need to implement zigbee

Changed in this revision

LSM9DS0.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM9DS0.lib	Wed Apr 22 16:24:21 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/aswild/code/LSM9DS0/#5556e6fb99f5
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Apr 22 16:24:21 2015 +0000
@@ -0,0 +1,178 @@
+/*////////////////////////////////////////////////////////////////
+ECE 4180 Final Project
+Balancing Robot
+
+Nico van Duijn
+Samer Mabrouk
+Anthony Agnone
+Jay Danner
+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.
+The robot receives steering commands via the XBee module which is
+interfaced with from a C# GUI that runs on a desktop computer.
+
+*/////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////
+// 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(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
+Ticker start;                                                   // Initialize ticker to call control function
+Serial xbee(p13, p14);                                          // Creates virtual serial for xbee
+                                    
+
+//////////////////////////////////////////////////////////////////
+// Globals
+float kp = 93;                                                  // 145 Proportional gain kU 400-500
+float kd = 100;                                                 // Derivative gain
+float ki = 1100;                                                // 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
+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)
+float desiredAngle=0;                                           // Setpoint. Set unequal zero to drive
+float turnspeed=0;                                              // positive turns 
+
+//////////////////////////////////////////////////////////////////
+// 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
+void callback();                                                // Interrupt triggered function for serial communication
+
+//////////////////////////////////////////////////////////////////
+// Main function
+int main()
+{
+    uint16_t status = imu.begin();                              // Use the begin() function to initialize the LSM9DS0 library.
+
+    xbee.attach(&callback);                                       // Attach interrupt to serial communication
+    calibrate();                                                // Calibrate gyro and accelerometer
+    start.attach(&control, 0.01);                               // Looptime 10ms ca. 100Hz
+    while(1) {                                                  // Stay in this loop forever
+
+    }
+}
+
+/////////////////////////////////////////////////////////////////
+// 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-desiredAngle;  // Complementary filter yields best value for current angle
+    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((abs(pAngle-desiredAngle)>=0.55)&&abs(pAngle-desiredAngle)<=15) {  // If it is tilted enough, but not too much ||abs(imu.gx)>8
+        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-turnspeed;                               // Set the DigitalOuts to run the motors forward
+            motorSpeedRight=speed+turnspeed;
+            motorDirLeft=1;
+            NmotorDirLeft=0;
+            motorDirRight=1;
+            NmotorDirRight=0;
+            break;
+        case 2:                                                 // Backwards
+            motorSpeedLeft=-speed+turnspeed;                              // Set the DigitalOuts to run the motors backward
+            motorSpeedRight=-speed-turnspeed;
+            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 accbias: -0.3 gyrobias: +0.15
+void calibrate()
+{
+    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/100;                                       // Convert sum to average
+    gyroBias=gyroBias/100;                                     // Convert sum to average
+}
+
+/////////////////////////////////////////////////////////////////
+// Callback function to change values/gains
+void callback()
+{
+    // Update kp
+    // Update kd
+    // Update ki
+    // Update OVERALL_SCALAR
+    // Update turnspeed
+    // Update desiredAngle
+    // Send pAngle
+    // Send dAngle
+    // Send iAngle
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Apr 22 16:24:21 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/9ad691361fac
\ No newline at end of file