Drew Paschal / Mbed 2 deprecated SelfBalancingPID

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Motordriver

main.cpp@3:dd0c62b586ea, 2015-04-20 (annotated)

Committer:
nicovanduijn
Date:
Mon Apr 20 16:33:09 2015 +0000
Revision:
3:dd0c62b586ea
Parent:
2:ad080363a22c
Child:
4:21dfcd81b397
most current version;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nicovanduijn 0:67f5b4041c15 1 /*////////////////////////////////////////////////////////////////
nicovanduijn 3:dd0c62b586ea 2 ECE 4180 Final Project
nicovanduijn 0:67f5b4041c15 3 Balancing Robot
nicovanduijn 3:dd0c62b586ea 4
nicovanduijn 0:67f5b4041c15 5 Nico van Duijn
nicovanduijn 0:67f5b4041c15 6 Samer Mabrouk
nicovanduijn 3:dd0c62b586ea 7 Anthony Agnone
nicovanduijn 3:dd0c62b586ea 8 Jay
nicovanduijn 3:dd0c62b586ea 9 4/20/2015
nicovanduijn 0:67f5b4041c15 10
nicovanduijn 0:67f5b4041c15 11 This project consists of a robot balancing on two wheels. We use
nicovanduijn 0:67f5b4041c15 12 the 9-axis IMU LSM9DS0 to give us Accelerometer and Gyroscope data
nicovanduijn 0:67f5b4041c15 13 about the current angle and angular velocity of the robot. This
nicovanduijn 0:67f5b4041c15 14 data is then filtered using complementary filters and PID control
nicovanduijn 0:67f5b4041c15 15 to drive the two motors. The motors are controlled through digital
nicovanduijn 3:dd0c62b586ea 16 outputs in their direction and their seepd by PWM using an H-bridge.
nicovanduijn 3:dd0c62b586ea 17 The robot receives steering commands via the XBee module which is
nicovanduijn 3:dd0c62b586ea 18 interfaced with from a C# GUI that runs on a desktop computer.
nicovanduijn 3:dd0c62b586ea 19
nicovanduijn 0:67f5b4041c15 20 */////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 21
nicovanduijn 0:67f5b4041c15 22 //////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 23 // Includes
nicovanduijn 0:67f5b4041c15 24 #include "mbed.h" // mbed library
nicovanduijn 0:67f5b4041c15 25 #include "LSM9DS0.h" // 9axis IMU
nicovanduijn 0:67f5b4041c15 26 #include "math.h" // We need to be able to do trig
nicovanduijn 0:67f5b4041c15 27
nicovanduijn 0:67f5b4041c15 28 //////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 29 // Constants
nicovanduijn 0:67f5b4041c15 30 #define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW
nicovanduijn 0:67f5b4041c15 31 #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW
nicovanduijn 3:dd0c62b586ea 32 //#define DEBUG // Comment this out for final version
nicovanduijn 0:67f5b4041c15 33
nicovanduijn 0:67f5b4041c15 34 //////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 35 // I/O and object instatiation
nicovanduijn 0:67f5b4041c15 36 PwmOut motorSpeedLeft(p21); // PWM port to control speed of left motor
nicovanduijn 0:67f5b4041c15 37 PwmOut motorSpeedRight(p22); // PWM port to control speed of right motor
nicovanduijn 3:dd0c62b586ea 38 DigitalOut motorDirLeft(p24); // Digital pin for direction of left motor
nicovanduijn 3:dd0c62b586ea 39 DigitalOut NmotorDirLeft(p23); // Usually inverse of motorDirLeft
nicovanduijn 0:67f5b4041c15 40 DigitalOut motorDirRight(p26); // Digital pin for direction of right motor
nicovanduijn 0:67f5b4041c15 41 DigitalOut NmotorDirRight(p25); // Usually inverse of motorDirRight
nicovanduijn 0:67f5b4041c15 42 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); // Creates on object for IMU
nicovanduijn 0:67f5b4041c15 43 Ticker start; // Initialize ticker to call control function
nicovanduijn 0:67f5b4041c15 44 #ifdef DEBUG
nicovanduijn 0:67f5b4041c15 45 Serial pc(USBTX, USBRX); // Creates virtual Serial connection though USB
nicovanduijn 0:67f5b4041c15 46 #endif
nicovanduijn 0:67f5b4041c15 47
nicovanduijn 0:67f5b4041c15 48 //////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 49 // Globals
nicovanduijn 3:dd0c62b586ea 50 float kp = 98; //40 // 145 Proportional gain kU 400-500
nicovanduijn 3:dd0c62b586ea 51 float kd = 200; //2 was quite good // Derivative gain
nicovanduijn 3:dd0c62b586ea 52 float ki = 935; //30 // Integrative gain
nicovanduijn 3:dd0c62b586ea 53 float OVERALL_SCALAR = 170; // Overall scalar that speed is divided by
nicovanduijn 0:67f5b4041c15 54 float accBias = 0; // Accelerometer Bias
nicovanduijn 0:67f5b4041c15 55 float gyroBias = 0; // Gyro Bias
nicovanduijn 0:67f5b4041c15 56 float accAngle = 0; // Global to hold angle measured by Accelerometer
nicovanduijn 0:67f5b4041c15 57 float gyroAngle = 0; // This variable holds the amount the angle has changed
nicovanduijn 0:67f5b4041c15 58 float speed = 0; // Variable for motor speed
nicovanduijn 0:67f5b4041c15 59 float iAngle = 0; // Integral value of angle-error (sum of gyro-angles)NOT EQUAL TO gyroAngle
nicovanduijn 0:67f5b4041c15 60 float dAngle = 0; // Derivative value for angle, angular velocity, how fast angle is changing
nicovanduijn 0:67f5b4041c15 61 float pAngle = 0; // Proportional value for angle, current angle (best measurement)
nicovanduijn 3:dd0c62b586ea 62 float desiredAngle=0; // Setpoint. Set unequal zero to drive
nicovanduijn 3:dd0c62b586ea 63 // float turnspeed=0;
nicovanduijn 0:67f5b4041c15 64
nicovanduijn 0:67f5b4041c15 65 //////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 66 // Function Prototypes
nicovanduijn 0:67f5b4041c15 67 void drive(float); // Function declaration for driving the motors
nicovanduijn 0:67f5b4041c15 68 void calibrate(); // Function to calibrate gyro and accelerometer
nicovanduijn 0:67f5b4041c15 69 void control(); // Function implementing PID control
nicovanduijn 0:67f5b4041c15 70 #ifdef DEBUG
nicovanduijn 0:67f5b4041c15 71 void callback(); // Interrupt triggered function for serial communication
nicovanduijn 0:67f5b4041c15 72 #endif
nicovanduijn 1:bf71a7bd2d3e 73
nicovanduijn 0:67f5b4041c15 74 //////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 75 // Main function
nicovanduijn 0:67f5b4041c15 76 int main()
nicovanduijn 0:67f5b4041c15 77 {
nicovanduijn 0:67f5b4041c15 78 uint16_t status = imu.begin(); // Use the begin() function to initialize the LSM9DS0 library.
nicovanduijn 0:67f5b4041c15 79 #ifdef DEBUG
nicovanduijn 0:67f5b4041c15 80 pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status); // Make sure communication is working
nicovanduijn 0:67f5b4041c15 81 pc.printf("Should be 0x49D4\n\n"); // Check if we're talking to the right guy
nicovanduijn 0:67f5b4041c15 82 pc.attach(&callback); // Attach interrupt to serial communication
nicovanduijn 0:67f5b4041c15 83 #endif
nicovanduijn 0:67f5b4041c15 84 calibrate(); // Calibrate gyro and accelerometer
nicovanduijn 0:67f5b4041c15 85 start.attach(&control, 0.01); // Looptime 10ms ca. 100Hz
nicovanduijn 0:67f5b4041c15 86 while(1) { // Stay in this loop forever
nicovanduijn 0:67f5b4041c15 87 #ifdef DEBUG
nicovanduijn 0:67f5b4041c15 88 pc.printf("%f\n",speed); // Print to USB the speed
nicovanduijn 0:67f5b4041c15 89 #endif
nicovanduijn 0:67f5b4041c15 90 }
nicovanduijn 0:67f5b4041c15 91 }
nicovanduijn 1:bf71a7bd2d3e 92
nicovanduijn 0:67f5b4041c15 93 /////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 94 // Control function, implements PID
nicovanduijn 0:67f5b4041c15 95 void control()
nicovanduijn 0:67f5b4041c15 96 {
nicovanduijn 3:dd0c62b586ea 97 dAngle=pAngle;// remember old p-value
nicovanduijn 0:67f5b4041c15 98 imu.readGyro(); // Read the gyro
nicovanduijn 0:67f5b4041c15 99 imu.readAccel(); // Read the Accelerometer
nicovanduijn 0:67f5b4041c15 100 accAngle=(-1)*atan2(imu.ay,imu.az)*180/3.142-90-accBias; // Like this, 0 is upright, forward is negative, backward positive
nicovanduijn 0:67f5b4041c15 101 gyroAngle=-(imu.gx-gyroBias)*0.01; // This is deltaangle, how much angle has changed
nicovanduijn 3:dd0c62b586ea 102 pAngle=0.98*(pAngle+gyroAngle)+0.02*accAngle-desiredAngle; // Complementary filter yields best value for current angle
nicovanduijn 3:dd0c62b586ea 103 //iAngle=(0.9*iAngle+0.1*gyroAngle); DOESNT ACTUALLY INTEGRATE ERROR // Sorta- running average-integral thing
nicovanduijn 3:dd0c62b586ea 104 dAngle=pAngle-dAngle; //Ang. Veloc. less noisy than dAngle = -(imu.gx-gyroBias);
nicovanduijn 3:dd0c62b586ea 105 iAngle+=(pAngle*.01);// integrate the angle (multiply by timestep to get dt!)
nicovanduijn 3:dd0c62b586ea 106 //if((ki*iAngle/OVERALL_SCALAR)>=3)iAngle=3*OVERALL_SCALAR/ki;// if ki dominates three-fold
nicovanduijn 3:dd0c62b586ea 107 //if((ki*iAngle/OVERALL_SCALAR)<=-3)iAngle=-3*OVERALL_SCALAR/ki;//50 is an arbitrary cap - kind of worked
nicovanduijn 3:dd0c62b586ea 108 // try angle dev. .55 and find value for imu.gx
nicovanduijn 3:dd0c62b586ea 109 if(abs(pAngle-desiredAngle)>=0.6&&abs(pAngle-desiredAngle)<=15) { // If it is tilted enough, but not too much
nicovanduijn 2:ad080363a22c 110 speed=-(ki*iAngle+kd*dAngle+kp*pAngle)/OVERALL_SCALAR; // drive to correct
nicovanduijn 0:67f5b4041c15 111 if(speed<-1) speed=-1; // Cap if undershoot
nicovanduijn 0:67f5b4041c15 112 else if(speed>1) speed=1; // Cap if overshoot
nicovanduijn 0:67f5b4041c15 113 } else speed=0; // If we've fallen over or are steady on top
nicovanduijn 0:67f5b4041c15 114 drive(speed); // Write speed to the motors
nicovanduijn 3:dd0c62b586ea 115
nicovanduijn 0:67f5b4041c15 116 }
nicovanduijn 1:bf71a7bd2d3e 117
nicovanduijn 0:67f5b4041c15 118 //////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 119 // Drive function
nicovanduijn 0:67f5b4041c15 120 void drive(float speed)
nicovanduijn 0:67f5b4041c15 121 {
nicovanduijn 0:67f5b4041c15 122 int direction=0; // Variable to hold direction we want to drive
nicovanduijn 0:67f5b4041c15 123 if (speed>0)direction=1; // Positive speed indicates forward
nicovanduijn 0:67f5b4041c15 124 if (speed<0)direction=2; // Negative speed indicates backwards
nicovanduijn 0:67f5b4041c15 125 if(speed==0)direction=0; // Zero speed indicates stopping
nicovanduijn 0:67f5b4041c15 126 switch( direction) { // Depending on what direction was passed
nicovanduijn 0:67f5b4041c15 127 case 0: // Stop case
nicovanduijn 0:67f5b4041c15 128 motorSpeedLeft=0; // Set the DigitalOuts to stop the motors
nicovanduijn 0:67f5b4041c15 129 motorSpeedRight=0;
nicovanduijn 0:67f5b4041c15 130 motorDirLeft=0;
nicovanduijn 0:67f5b4041c15 131 NmotorDirLeft=0;
nicovanduijn 0:67f5b4041c15 132 motorDirRight=0;
nicovanduijn 0:67f5b4041c15 133 NmotorDirRight=0;
nicovanduijn 0:67f5b4041c15 134 break;
nicovanduijn 0:67f5b4041c15 135 case 1: // Forward case
nicovanduijn 0:67f5b4041c15 136 motorSpeedLeft=speed; // Set the DigitalOuts to run the motors forward
nicovanduijn 0:67f5b4041c15 137 motorSpeedRight=speed;
nicovanduijn 0:67f5b4041c15 138 motorDirLeft=1;
nicovanduijn 0:67f5b4041c15 139 NmotorDirLeft=0;
nicovanduijn 0:67f5b4041c15 140 motorDirRight=1;
nicovanduijn 0:67f5b4041c15 141 NmotorDirRight=0;
nicovanduijn 0:67f5b4041c15 142 break;
nicovanduijn 0:67f5b4041c15 143 case 2: // Backwards
nicovanduijn 0:67f5b4041c15 144 motorSpeedLeft=-speed; // Set the DigitalOuts to run the motors backward
nicovanduijn 0:67f5b4041c15 145 motorSpeedRight=-speed;
nicovanduijn 0:67f5b4041c15 146 motorDirLeft=0;
nicovanduijn 0:67f5b4041c15 147 NmotorDirLeft=1;
nicovanduijn 0:67f5b4041c15 148 motorDirRight=0;
nicovanduijn 0:67f5b4041c15 149 NmotorDirRight=1;
nicovanduijn 0:67f5b4041c15 150 break;
nicovanduijn 0:67f5b4041c15 151 default: // Catch-all (Stop)
nicovanduijn 0:67f5b4041c15 152 motorSpeedLeft=0; // Set the DigitalOuts to stop the motors
nicovanduijn 0:67f5b4041c15 153 motorSpeedRight=0;
nicovanduijn 0:67f5b4041c15 154 motorDirLeft=0;
nicovanduijn 0:67f5b4041c15 155 NmotorDirLeft=0;
nicovanduijn 0:67f5b4041c15 156 motorDirRight=0;
nicovanduijn 0:67f5b4041c15 157 NmotorDirRight=0;
nicovanduijn 0:67f5b4041c15 158 break;
nicovanduijn 0:67f5b4041c15 159 }
nicovanduijn 0:67f5b4041c15 160 }
nicovanduijn 0:67f5b4041c15 161
nicovanduijn 0:67f5b4041c15 162 //////////////////////////////////////////////////////////////////
nicovanduijn 3:dd0c62b586ea 163 // Calibrate function to find gyro drift and accelerometer bias accbias: -0.3 gyrobias: +0.15
nicovanduijn 0:67f5b4041c15 164 void calibrate()
nicovanduijn 0:67f5b4041c15 165 {
nicovanduijn 3:dd0c62b586ea 166 for(int i=0; i<100; i++) { // Read a thousand values
nicovanduijn 0:67f5b4041c15 167 imu.readAccel(); // Read the Accelerometer
nicovanduijn 0:67f5b4041c15 168 imu.readGyro(); // Read the gyro
nicovanduijn 0:67f5b4041c15 169 accBias=accBias+(-1)*atan2(imu.ay,imu.az)*180/3.142-90; // Like this, 0 is upright, forward is negative, backward positive
nicovanduijn 0:67f5b4041c15 170 gyroBias=gyroBias+imu.gx; // Add up all the gyro Biases
nicovanduijn 0:67f5b4041c15 171 }
nicovanduijn 3:dd0c62b586ea 172 accBias=accBias/100; // Convert sum to average
nicovanduijn 3:dd0c62b586ea 173 gyroBias=gyroBias/100; // Convert sum to average
nicovanduijn 0:67f5b4041c15 174 #ifdef DEBUG
nicovanduijn 0:67f5b4041c15 175 pc.printf("accBias: %f gyroBias: %f\n",accBias, gyroBias); // Print biases to USB
nicovanduijn 0:67f5b4041c15 176 #endif
nicovanduijn 0:67f5b4041c15 177 }
nicovanduijn 0:67f5b4041c15 178
nicovanduijn 0:67f5b4041c15 179 /////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 180 // Callback function to change values/gains
nicovanduijn 1:bf71a7bd2d3e 181 #ifdef DEBUG
nicovanduijn 0:67f5b4041c15 182 void callback()
nicovanduijn 0:67f5b4041c15 183 {
nicovanduijn 0:67f5b4041c15 184 char val; // Needed for Serial communication (need to be global?)
nicovanduijn 0:67f5b4041c15 185 val = pc.getc(); // Reat the value from Serial
nicovanduijn 0:67f5b4041c15 186 pc.printf("%c\n", val); // Print it back to the screen
nicovanduijn 0:67f5b4041c15 187 if( val =='p') { // If character was a 'p'
nicovanduijn 0:67f5b4041c15 188 pc.printf("enter kp \n"); // Adjust kp
nicovanduijn 0:67f5b4041c15 189 val = pc.getc(); // Wait for kp value
nicovanduijn 0:67f5b4041c15 190 if(val == 0x2b) { // If character is a plus sign
nicovanduijn 0:67f5b4041c15 191 kp=kp+10; // Increase kp
nicovanduijn 0:67f5b4041c15 192 } else if (val == 0x2d) { // If recieved character is the minus sign
nicovanduijn 0:67f5b4041c15 193 kp=kp-10; // Decrease kp
nicovanduijn 0:67f5b4041c15 194 } else {
nicovanduijn 0:67f5b4041c15 195 kp = val - 48; // Cast char to float
nicovanduijn 0:67f5b4041c15 196 }
nicovanduijn 0:67f5b4041c15 197 pc.printf(" kp = %f \n",kp); // Print current kp value to screen
nicovanduijn 0:67f5b4041c15 198 } else if( val == 'd') { // Adjust kd
nicovanduijn 0:67f5b4041c15 199 pc.printf("enter kd \n"); // Wait for kd
nicovanduijn 0:67f5b4041c15 200 val= pc.getc(); // Read value from serial
nicovanduijn 0:67f5b4041c15 201 if(val == '+') { // If given plus sign
nicovanduijn 0:67f5b4041c15 202 kd++; // Increase kd
nicovanduijn 0:67f5b4041c15 203 } else if (val == '-') { // If given negative sign
nicovanduijn 0:67f5b4041c15 204 kd--; // Decrease kd
nicovanduijn 0:67f5b4041c15 205 } else { // If given some other ascii (a number?)
nicovanduijn 0:67f5b4041c15 206 kd = val - 48; // Set derivative gain
nicovanduijn 0:67f5b4041c15 207 }
nicovanduijn 0:67f5b4041c15 208 pc.printf(" kd = %f \n",kd); // Print kd back to screen
nicovanduijn 0:67f5b4041c15 209 } else if( val == 'i') { // If given i - integral gain
nicovanduijn 0:67f5b4041c15 210 pc.printf("enter ki \n"); // Prompt on screen to ask for ascii
nicovanduijn 0:67f5b4041c15 211 val= pc.getc(); // Get the input
nicovanduijn 0:67f5b4041c15 212 if(val == '+') { // If given the plus sign
nicovanduijn 0:67f5b4041c15 213 ki++; // Increase ki
nicovanduijn 0:67f5b4041c15 214 } else if (val == '-') { // If given the minus sign
nicovanduijn 0:67f5b4041c15 215 ki--; // Decrease ki
nicovanduijn 0:67f5b4041c15 216 } else { // If given some other ascii
nicovanduijn 0:67f5b4041c15 217 ki = val - 48; // Set ki to that number
nicovanduijn 0:67f5b4041c15 218 }
nicovanduijn 0:67f5b4041c15 219 pc.printf(" ki = %f \n",ki);
nicovanduijn 0:67f5b4041c15 220 } else if( val == 'o') {
nicovanduijn 0:67f5b4041c15 221 pc.printf("enter OVERALL_SCALAR \n");
nicovanduijn 0:67f5b4041c15 222 val= pc.getc();
nicovanduijn 0:67f5b4041c15 223 if(val == '+') {
nicovanduijn 0:67f5b4041c15 224 OVERALL_SCALAR=OVERALL_SCALAR+1000;
nicovanduijn 0:67f5b4041c15 225 } else if (val == '-') {
nicovanduijn 0:67f5b4041c15 226 OVERALL_SCALAR=OVERALL_SCALAR-1000;
nicovanduijn 0:67f5b4041c15 227 } else {
nicovanduijn 0:67f5b4041c15 228 OVERALL_SCALAR=(val-48)*1000;;
nicovanduijn 0:67f5b4041c15 229 }
nicovanduijn 0:67f5b4041c15 230 pc.printf(" OVERALL_SCALAR = %f \n",OVERALL_SCALAR);
nicovanduijn 0:67f5b4041c15 231 }
nicovanduijn 0:67f5b4041c15 232 }
nicovanduijn 0:67f5b4041c15 233 #endif