ECE 4180 Spring 15
/
balance2
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.
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /*//////////////////////////////////////////////////////////////// 00002 ECE 4180 Final Project 00003 Balancing Robot 00004 00005 Nico van Duijn 00006 Samer Mabrouk 00007 Anthony Agnone 00008 Jay 00009 4/20/2015 00010 00011 This project consists of a robot balancing on two wheels. We use 00012 the 9-axis IMU LSM9DS0 to give us Accelerometer and Gyroscope data 00013 about the current angle and angular velocity of the robot. This 00014 data is then filtered using complementary filters and PID control 00015 to drive the two motors. The motors are controlled through digital 00016 outputs in their direction and their seepd by PWM using an H-bridge. 00017 The robot receives steering commands via the XBee module which is 00018 interfaced with from a C# GUI that runs on a desktop computer. 00019 00020 *///////////////////////////////////////////////////////////////// 00021 00022 ////////////////////////////////////////////////////////////////// 00023 // Includes 00024 #include "mbed.h" // mbed library 00025 #include "LSM9DS0.h" // 9axis IMU 00026 #include "math.h" // We need to be able to do trig 00027 00028 ////////////////////////////////////////////////////////////////// 00029 // Constants 00030 #define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW 00031 #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW 00032 //#define DEBUG // Comment this out for final version 00033 00034 ////////////////////////////////////////////////////////////////// 00035 // I/O and object instatiation 00036 PwmOut motorSpeedLeft(p21); // PWM port to control speed of left motor 00037 PwmOut motorSpeedRight(p22); // PWM port to control speed of right motor 00038 DigitalOut motorDirLeft(p24); // Digital pin for direction of left motor 00039 DigitalOut NmotorDirLeft(p23); // Usually inverse of motorDirLeft 00040 DigitalOut motorDirRight(p26); // Digital pin for direction of right motor 00041 DigitalOut NmotorDirRight(p25); // Usually inverse of motorDirRight 00042 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); // Creates on object for IMU 00043 Ticker start; // Initialize ticker to call control function 00044 #ifdef DEBUG 00045 Serial pc(USBTX, USBRX); // Creates virtual Serial connection though USB 00046 #endif 00047 00048 ////////////////////////////////////////////////////////////////// 00049 // Globals 00050 float kp = 98; //40 // 145 Proportional gain kU 400-500 00051 float kd = 200; //2 was quite good // Derivative gain 00052 float ki = 935; //30 // Integrative gain 00053 float OVERALL_SCALAR = 170; // Overall scalar that speed is divided by 00054 float accBias = 0; // Accelerometer Bias 00055 float gyroBias = 0; // Gyro Bias 00056 float accAngle = 0; // Global to hold angle measured by Accelerometer 00057 float gyroAngle = 0; // This variable holds the amount the angle has changed 00058 float speed = 0; // Variable for motor speed 00059 float iAngle = 0; // Integral value of angle-error (sum of gyro-angles)NOT EQUAL TO gyroAngle 00060 float dAngle = 0; // Derivative value for angle, angular velocity, how fast angle is changing 00061 float pAngle = 0; // Proportional value for angle, current angle (best measurement) 00062 float desiredAngle=0; // Setpoint. Set unequal zero to drive 00063 // float turnspeed=0; 00064 00065 ////////////////////////////////////////////////////////////////// 00066 // Function Prototypes 00067 void drive(float); // Function declaration for driving the motors 00068 void calibrate(); // Function to calibrate gyro and accelerometer 00069 void control(); // Function implementing PID control 00070 #ifdef DEBUG 00071 void callback(); // Interrupt triggered function for serial communication 00072 #endif 00073 00074 ////////////////////////////////////////////////////////////////// 00075 // Main function 00076 int main() 00077 { 00078 uint16_t status = imu.begin(); // Use the begin() function to initialize the LSM9DS0 library. 00079 #ifdef DEBUG 00080 pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status); // Make sure communication is working 00081 pc.printf("Should be 0x49D4\n\n"); // Check if we're talking to the right guy 00082 pc.attach(&callback); // Attach interrupt to serial communication 00083 #endif 00084 calibrate(); // Calibrate gyro and accelerometer 00085 start.attach(&control, 0.01); // Looptime 10ms ca. 100Hz 00086 while(1) { // Stay in this loop forever 00087 #ifdef DEBUG 00088 pc.printf("%f\n",speed); // Print to USB the speed 00089 #endif 00090 } 00091 } 00092 00093 ///////////////////////////////////////////////////////////////// 00094 // Control function, implements PID 00095 void control() 00096 { 00097 dAngle=pAngle;// remember old p-value 00098 imu.readGyro(); // Read the gyro 00099 imu.readAccel(); // Read the Accelerometer 00100 accAngle=(-1)*atan2(imu.ay,imu.az)*180/3.142-90-accBias; // Like this, 0 is upright, forward is negative, backward positive 00101 gyroAngle=-(imu.gx-gyroBias)*0.01; // This is deltaangle, how much angle has changed 00102 pAngle=0.98*(pAngle+gyroAngle)+0.02*accAngle-desiredAngle; // Complementary filter yields best value for current angle 00103 //iAngle=(0.9*iAngle+0.1*gyroAngle); DOESNT ACTUALLY INTEGRATE ERROR // Sorta- running average-integral thing 00104 dAngle=pAngle-dAngle; //Ang. Veloc. less noisy than dAngle = -(imu.gx-gyroBias); 00105 iAngle+=(pAngle*.01);// integrate the angle (multiply by timestep to get dt!) 00106 //if((ki*iAngle/OVERALL_SCALAR)>=3)iAngle=3*OVERALL_SCALAR/ki;// if ki dominates three-fold 00107 //if((ki*iAngle/OVERALL_SCALAR)<=-3)iAngle=-3*OVERALL_SCALAR/ki;//50 is an arbitrary cap - kind of worked 00108 // try angle dev. .55 and find value for imu.gx 00109 if(abs(pAngle-desiredAngle)>=0.6&&abs(pAngle-desiredAngle)<=15) { // If it is tilted enough, but not too much 00110 speed=-(ki*iAngle+kd*dAngle+kp*pAngle)/OVERALL_SCALAR; // drive to correct 00111 if(speed<-1) speed=-1; // Cap if undershoot 00112 else if(speed>1) speed=1; // Cap if overshoot 00113 } else speed=0; // If we've fallen over or are steady on top 00114 drive(speed); // Write speed to the motors 00115 00116 } 00117 00118 ////////////////////////////////////////////////////////////////// 00119 // Drive function 00120 void drive(float speed) 00121 { 00122 int direction=0; // Variable to hold direction we want to drive 00123 if (speed>0)direction=1; // Positive speed indicates forward 00124 if (speed<0)direction=2; // Negative speed indicates backwards 00125 if(speed==0)direction=0; // Zero speed indicates stopping 00126 switch( direction) { // Depending on what direction was passed 00127 case 0: // Stop case 00128 motorSpeedLeft=0; // Set the DigitalOuts to stop the motors 00129 motorSpeedRight=0; 00130 motorDirLeft=0; 00131 NmotorDirLeft=0; 00132 motorDirRight=0; 00133 NmotorDirRight=0; 00134 break; 00135 case 1: // Forward case 00136 motorSpeedLeft=speed; // Set the DigitalOuts to run the motors forward 00137 motorSpeedRight=speed; 00138 motorDirLeft=1; 00139 NmotorDirLeft=0; 00140 motorDirRight=1; 00141 NmotorDirRight=0; 00142 break; 00143 case 2: // Backwards 00144 motorSpeedLeft=-speed; // Set the DigitalOuts to run the motors backward 00145 motorSpeedRight=-speed; 00146 motorDirLeft=0; 00147 NmotorDirLeft=1; 00148 motorDirRight=0; 00149 NmotorDirRight=1; 00150 break; 00151 default: // Catch-all (Stop) 00152 motorSpeedLeft=0; // Set the DigitalOuts to stop the motors 00153 motorSpeedRight=0; 00154 motorDirLeft=0; 00155 NmotorDirLeft=0; 00156 motorDirRight=0; 00157 NmotorDirRight=0; 00158 break; 00159 } 00160 } 00161 00162 ////////////////////////////////////////////////////////////////// 00163 // Calibrate function to find gyro drift and accelerometer bias accbias: -0.3 gyrobias: +0.15 00164 void calibrate() 00165 { 00166 for(int i=0; i<100; i++) { // Read a thousand values 00167 imu.readAccel(); // Read the Accelerometer 00168 imu.readGyro(); // Read the gyro 00169 accBias=accBias+(-1)*atan2(imu.ay,imu.az)*180/3.142-90; // Like this, 0 is upright, forward is negative, backward positive 00170 gyroBias=gyroBias+imu.gx; // Add up all the gyro Biases 00171 } 00172 accBias=accBias/100; // Convert sum to average 00173 gyroBias=gyroBias/100; // Convert sum to average 00174 #ifdef DEBUG 00175 pc.printf("accBias: %f gyroBias: %f\n",accBias, gyroBias); // Print biases to USB 00176 #endif 00177 } 00178 00179 ///////////////////////////////////////////////////////////////// 00180 // Callback function to change values/gains 00181 #ifdef DEBUG 00182 void callback() 00183 { 00184 char val; // Needed for Serial communication (need to be global?) 00185 val = pc.getc(); // Reat the value from Serial 00186 pc.printf("%c\n", val); // Print it back to the screen 00187 if( val =='p') { // If character was a 'p' 00188 pc.printf("enter kp \n"); // Adjust kp 00189 val = pc.getc(); // Wait for kp value 00190 if(val == 0x2b) { // If character is a plus sign 00191 kp=kp+10; // Increase kp 00192 } else if (val == 0x2d) { // If recieved character is the minus sign 00193 kp=kp-10; // Decrease kp 00194 } else { 00195 kp = val - 48; // Cast char to float 00196 } 00197 pc.printf(" kp = %f \n",kp); // Print current kp value to screen 00198 } else if( val == 'd') { // Adjust kd 00199 pc.printf("enter kd \n"); // Wait for kd 00200 val= pc.getc(); // Read value from serial 00201 if(val == '+') { // If given plus sign 00202 kd++; // Increase kd 00203 } else if (val == '-') { // If given negative sign 00204 kd--; // Decrease kd 00205 } else { // If given some other ascii (a number?) 00206 kd = val - 48; // Set derivative gain 00207 } 00208 pc.printf(" kd = %f \n",kd); // Print kd back to screen 00209 } else if( val == 'i') { // If given i - integral gain 00210 pc.printf("enter ki \n"); // Prompt on screen to ask for ascii 00211 val= pc.getc(); // Get the input 00212 if(val == '+') { // If given the plus sign 00213 ki++; // Increase ki 00214 } else if (val == '-') { // If given the minus sign 00215 ki--; // Decrease ki 00216 } else { // If given some other ascii 00217 ki = val - 48; // Set ki to that number 00218 } 00219 pc.printf(" ki = %f \n",ki); 00220 } else if( val == 'o') { 00221 pc.printf("enter OVERALL_SCALAR \n"); 00222 val= pc.getc(); 00223 if(val == '+') { 00224 OVERALL_SCALAR=OVERALL_SCALAR+1000; 00225 } else if (val == '-') { 00226 OVERALL_SCALAR=OVERALL_SCALAR-1000; 00227 } else { 00228 OVERALL_SCALAR=(val-48)*1000;; 00229 } 00230 pc.printf(" OVERALL_SCALAR = %f \n",OVERALL_SCALAR); 00231 } 00232 } 00233 #endif
Generated on Tue Jul 12 2022 14:28:27 by
1.7.2