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
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?
User | Revision | Line number | New 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 |