ECE 4180 Spring 15 / Mbed 2 deprecated balance2

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

Dependencies:   LSM9DS0 mbed

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 
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