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@0:67f5b4041c15, 2015-03-06 (annotated)

Committer:
nicovanduijn
Date:
Fri Mar 06 17:09:31 2015 +0000
Revision:
0:67f5b4041c15
Child:
1:bf71a7bd2d3e
working! May be improved slightly by adjusting kp, kd, ki and overall_scalar. But works very well as-is!

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 0:67f5b4041c15 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 0:67f5b4041c15 65 //////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 66 // Main function
nicovanduijn 0:67f5b4041c15 67 int main()
nicovanduijn 0:67f5b4041c15 68 {
nicovanduijn 0:67f5b4041c15 69 uint16_t status = imu.begin(); // Use the begin() function to initialize the LSM9DS0 library.
nicovanduijn 0:67f5b4041c15 70 #ifdef DEBUG
nicovanduijn 0:67f5b4041c15 71 pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status); // Make sure communication is working
nicovanduijn 0:67f5b4041c15 72 pc.printf("Should be 0x49D4\n\n"); // Check if we're talking to the right guy
nicovanduijn 0:67f5b4041c15 73 pc.attach(&callback); // Attach interrupt to serial communication
nicovanduijn 0:67f5b4041c15 74 #endif
nicovanduijn 0:67f5b4041c15 75 calibrate(); // Calibrate gyro and accelerometer
nicovanduijn 0:67f5b4041c15 76 start.attach(&control, 0.01); // Looptime 10ms ca. 100Hz
nicovanduijn 0:67f5b4041c15 77 while(1) { // Stay in this loop forever
nicovanduijn 0:67f5b4041c15 78 #ifdef DEBUG
nicovanduijn 0:67f5b4041c15 79 pc.printf("%f\n",speed); // Print to USB the speed
nicovanduijn 0:67f5b4041c15 80 #endif
nicovanduijn 0:67f5b4041c15 81 }
nicovanduijn 0:67f5b4041c15 82 }
nicovanduijn 0:67f5b4041c15 83 /////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 84 // Control function, implements PID
nicovanduijn 0:67f5b4041c15 85 void control()
nicovanduijn 0:67f5b4041c15 86 {
nicovanduijn 0:67f5b4041c15 87 imu.readGyro(); // Read the gyro
nicovanduijn 0:67f5b4041c15 88 imu.readAccel(); // Read the Accelerometer
nicovanduijn 0:67f5b4041c15 89 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 90 gyroAngle=-(imu.gx-gyroBias)*0.01; // This is deltaangle, how much angle has changed
nicovanduijn 0:67f5b4041c15 91 pAngle=0.98*(pAngle+gyroAngle)+0.02*accAngle; // Complementary filter yields best value for current angle
nicovanduijn 0:67f5b4041c15 92 dAngle = -(imu.gx-gyroBias); // This is angular velocity
nicovanduijn 0:67f5b4041c15 93 iAngle=(0.99*iAngle+0.01*gyroAngle); // Sorta- running average-integral thing
nicovanduijn 0:67f5b4041c15 94 if(abs(pAngle)>=0.6&&abs(pAngle)<=25) { // If it is tilted enough, but not too much
nicovanduijn 0:67f5b4041c15 95 speed=-(ki*iAngle+kd*dAngle+kp*pAngle)/OVERALL_SCALAR; // drive to correct
nicovanduijn 0:67f5b4041c15 96 if(speed<-1) speed=-1; // Cap if undershoot
nicovanduijn 0:67f5b4041c15 97 else if(speed>1) speed=1; // Cap if overshoot
nicovanduijn 0:67f5b4041c15 98 } else speed=0; // If we've fallen over or are steady on top
nicovanduijn 0:67f5b4041c15 99 drive(speed); // Write speed to the motors
nicovanduijn 0:67f5b4041c15 100 }
nicovanduijn 0:67f5b4041c15 101 //////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 102 // Drive function
nicovanduijn 0:67f5b4041c15 103 void drive(float speed)
nicovanduijn 0:67f5b4041c15 104 {
nicovanduijn 0:67f5b4041c15 105 int direction=0; // Variable to hold direction we want to drive
nicovanduijn 0:67f5b4041c15 106 if (speed>0)direction=1; // Positive speed indicates forward
nicovanduijn 0:67f5b4041c15 107 if (speed<0)direction=2; // Negative speed indicates backwards
nicovanduijn 0:67f5b4041c15 108 if(speed==0)direction=0; // Zero speed indicates stopping
nicovanduijn 0:67f5b4041c15 109 switch( direction) { // Depending on what direction was passed
nicovanduijn 0:67f5b4041c15 110 case 0: // Stop case
nicovanduijn 0:67f5b4041c15 111 motorSpeedLeft=0; // Set the DigitalOuts to stop the motors
nicovanduijn 0:67f5b4041c15 112 motorSpeedRight=0;
nicovanduijn 0:67f5b4041c15 113 motorDirLeft=0;
nicovanduijn 0:67f5b4041c15 114 NmotorDirLeft=0;
nicovanduijn 0:67f5b4041c15 115 motorDirRight=0;
nicovanduijn 0:67f5b4041c15 116 NmotorDirRight=0;
nicovanduijn 0:67f5b4041c15 117 break;
nicovanduijn 0:67f5b4041c15 118 case 1: // Forward case
nicovanduijn 0:67f5b4041c15 119 motorSpeedLeft=speed; // Set the DigitalOuts to run the motors forward
nicovanduijn 0:67f5b4041c15 120 motorSpeedRight=speed;
nicovanduijn 0:67f5b4041c15 121 motorDirLeft=1;
nicovanduijn 0:67f5b4041c15 122 NmotorDirLeft=0;
nicovanduijn 0:67f5b4041c15 123 motorDirRight=1;
nicovanduijn 0:67f5b4041c15 124 NmotorDirRight=0;
nicovanduijn 0:67f5b4041c15 125 break;
nicovanduijn 0:67f5b4041c15 126 case 2: // Backwards
nicovanduijn 0:67f5b4041c15 127 motorSpeedLeft=-speed; // Set the DigitalOuts to run the motors backward
nicovanduijn 0:67f5b4041c15 128 motorSpeedRight=-speed;
nicovanduijn 0:67f5b4041c15 129 motorDirLeft=0;
nicovanduijn 0:67f5b4041c15 130 NmotorDirLeft=1;
nicovanduijn 0:67f5b4041c15 131 motorDirRight=0;
nicovanduijn 0:67f5b4041c15 132 NmotorDirRight=1;
nicovanduijn 0:67f5b4041c15 133 break;
nicovanduijn 0:67f5b4041c15 134 default: // Catch-all (Stop)
nicovanduijn 0:67f5b4041c15 135 motorSpeedLeft=0; // Set the DigitalOuts to stop the motors
nicovanduijn 0:67f5b4041c15 136 motorSpeedRight=0;
nicovanduijn 0:67f5b4041c15 137 motorDirLeft=0;
nicovanduijn 0:67f5b4041c15 138 NmotorDirLeft=0;
nicovanduijn 0:67f5b4041c15 139 motorDirRight=0;
nicovanduijn 0:67f5b4041c15 140 NmotorDirRight=0;
nicovanduijn 0:67f5b4041c15 141 break;
nicovanduijn 0:67f5b4041c15 142 }
nicovanduijn 0:67f5b4041c15 143 }
nicovanduijn 0:67f5b4041c15 144
nicovanduijn 0:67f5b4041c15 145 //////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 146 // Calibrate function to find gyro drift and accelerometer bias
nicovanduijn 0:67f5b4041c15 147 void calibrate()
nicovanduijn 0:67f5b4041c15 148 {
nicovanduijn 0:67f5b4041c15 149 for(int i=0; i<1000; i++) { // Read a thousand values
nicovanduijn 0:67f5b4041c15 150 imu.readAccel(); // Read the Accelerometer
nicovanduijn 0:67f5b4041c15 151 imu.readGyro(); // Read the gyro
nicovanduijn 0:67f5b4041c15 152 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 153 gyroBias=gyroBias+imu.gx; // Add up all the gyro Biases
nicovanduijn 0:67f5b4041c15 154 }
nicovanduijn 0:67f5b4041c15 155 accBias=accBias/1000; // Convert sum to average
nicovanduijn 0:67f5b4041c15 156 gyroBias=gyroBias/1000; // Convert sum to average
nicovanduijn 0:67f5b4041c15 157 #ifdef DEBUG
nicovanduijn 0:67f5b4041c15 158 pc.printf("accBias: %f gyroBias: %f\n",accBias, gyroBias); // Print biases to USB
nicovanduijn 0:67f5b4041c15 159 #endif
nicovanduijn 0:67f5b4041c15 160 }
nicovanduijn 0:67f5b4041c15 161
nicovanduijn 0:67f5b4041c15 162 #ifdef DEBUG
nicovanduijn 0:67f5b4041c15 163 /////////////////////////////////////////////////////////////////
nicovanduijn 0:67f5b4041c15 164 // Callback function to change values/gains
nicovanduijn 0:67f5b4041c15 165 void callback()
nicovanduijn 0:67f5b4041c15 166 {
nicovanduijn 0:67f5b4041c15 167 char val; // Needed for Serial communication (need to be global?)
nicovanduijn 0:67f5b4041c15 168 val = pc.getc(); // Reat the value from Serial
nicovanduijn 0:67f5b4041c15 169 pc.printf("%c\n", val); // Print it back to the screen
nicovanduijn 0:67f5b4041c15 170 if( val =='p') { // If character was a 'p'
nicovanduijn 0:67f5b4041c15 171 pc.printf("enter kp \n"); // Adjust kp
nicovanduijn 0:67f5b4041c15 172 val = pc.getc(); // Wait for kp value
nicovanduijn 0:67f5b4041c15 173 if(val == 0x2b) { // If character is a plus sign
nicovanduijn 0:67f5b4041c15 174 kp=kp+10; // Increase kp
nicovanduijn 0:67f5b4041c15 175 } else if (val == 0x2d) { // If recieved character is the minus sign
nicovanduijn 0:67f5b4041c15 176 kp=kp-10; // Decrease kp
nicovanduijn 0:67f5b4041c15 177 } else {
nicovanduijn 0:67f5b4041c15 178 kp = val - 48; // Cast char to float
nicovanduijn 0:67f5b4041c15 179 }
nicovanduijn 0:67f5b4041c15 180 pc.printf(" kp = %f \n",kp); // Print current kp value to screen
nicovanduijn 0:67f5b4041c15 181 } else if( val == 'd') { // Adjust kd
nicovanduijn 0:67f5b4041c15 182 pc.printf("enter kd \n"); // Wait for kd
nicovanduijn 0:67f5b4041c15 183 val= pc.getc(); // Read value from serial
nicovanduijn 0:67f5b4041c15 184 if(val == '+') { // If given plus sign
nicovanduijn 0:67f5b4041c15 185 kd++; // Increase kd
nicovanduijn 0:67f5b4041c15 186 } else if (val == '-') { // If given negative sign
nicovanduijn 0:67f5b4041c15 187 kd--; // Decrease kd
nicovanduijn 0:67f5b4041c15 188 } else { // If given some other ascii (a number?)
nicovanduijn 0:67f5b4041c15 189 kd = val - 48; // Set derivative gain
nicovanduijn 0:67f5b4041c15 190 }
nicovanduijn 0:67f5b4041c15 191 pc.printf(" kd = %f \n",kd); // Print kd back to screen
nicovanduijn 0:67f5b4041c15 192 } else if( val == 'i') { // If given i - integral gain
nicovanduijn 0:67f5b4041c15 193 pc.printf("enter ki \n"); // Prompt on screen to ask for ascii
nicovanduijn 0:67f5b4041c15 194 val= pc.getc(); // Get the input
nicovanduijn 0:67f5b4041c15 195 if(val == '+') { // If given the plus sign
nicovanduijn 0:67f5b4041c15 196 ki++; // Increase ki
nicovanduijn 0:67f5b4041c15 197 } else if (val == '-') { // If given the minus sign
nicovanduijn 0:67f5b4041c15 198 ki--; // Decrease ki
nicovanduijn 0:67f5b4041c15 199 } else { // If given some other ascii
nicovanduijn 0:67f5b4041c15 200 ki = val - 48; // Set ki to that number
nicovanduijn 0:67f5b4041c15 201 }
nicovanduijn 0:67f5b4041c15 202 pc.printf(" ki = %f \n",ki);
nicovanduijn 0:67f5b4041c15 203 } else if( val == 'o') {
nicovanduijn 0:67f5b4041c15 204 pc.printf("enter OVERALL_SCALAR \n");
nicovanduijn 0:67f5b4041c15 205 val= pc.getc();
nicovanduijn 0:67f5b4041c15 206 if(val == '+') {
nicovanduijn 0:67f5b4041c15 207 OVERALL_SCALAR=OVERALL_SCALAR+1000;
nicovanduijn 0:67f5b4041c15 208 } else if (val == '-') {
nicovanduijn 0:67f5b4041c15 209 OVERALL_SCALAR=OVERALL_SCALAR-1000;
nicovanduijn 0:67f5b4041c15 210 } else {
nicovanduijn 0:67f5b4041c15 211 OVERALL_SCALAR=(val-48)*1000;;
nicovanduijn 0:67f5b4041c15 212 }
nicovanduijn 0:67f5b4041c15 213 pc.printf(" OVERALL_SCALAR = %f \n",OVERALL_SCALAR);
nicovanduijn 0:67f5b4041c15 214 }
nicovanduijn 0:67f5b4041c15 215 }
nicovanduijn 0:67f5b4041c15 216 #endif