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@2:ad080363a22c, 2015-04-06 (annotated)

Committer:
nicovanduijn
Date:
Mon Apr 06 16:25:18 2015 +0000
Revision:
2:ad080363a22c
Parent:
1:bf71a7bd2d3e
Child:
3:dd0c62b586ea
Starting point;

Who changed what in which revision?

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