Nico van Duijn / Mbed 2 deprecated AwkwardSegway

Robot that balances on two wheels

Dependencies:   LSM9DS0 mbed

Fork of EpilepticSegway by ECE 4180 Spring 15

main.cpp@2:89ada0b0b923, 2015-04-28 (annotated)

Committer:
nicovanduijn
Date:
Tue Apr 28 02:18:55 2015 +0000
Revision:
2:89ada0b0b923
Parent:
1:a079ae75a86c
Child:
3:89e4ed1324bb
modified a lot. communications still iffy, but well-commented;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nicovanduijn 0:4b50c71291e9 1 /*////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 2 ECE 4180 Final Project
nicovanduijn 0:4b50c71291e9 3 Balancing Robot
nicovanduijn 0:4b50c71291e9 4
nicovanduijn 0:4b50c71291e9 5 Nico van Duijn
nicovanduijn 0:4b50c71291e9 6 Samer Mabrouk
nicovanduijn 0:4b50c71291e9 7 Anthony Agnone
nicovanduijn 0:4b50c71291e9 8 Jay Danner
nicovanduijn 0:4b50c71291e9 9 4/20/2015
nicovanduijn 0:4b50c71291e9 10
nicovanduijn 0:4b50c71291e9 11 This project consists of a robot balancing on two wheels. We use
nicovanduijn 0:4b50c71291e9 12 the 9-axis IMU LSM9DS0 to give us Accelerometer and Gyroscope data
nicovanduijn 0:4b50c71291e9 13 about the current angle and angular velocity of the robot. This
nicovanduijn 0:4b50c71291e9 14 data is then filtered using complementary filters and PID control
nicovanduijn 0:4b50c71291e9 15 to drive the two motors. The motors are controlled through digital
nicovanduijn 0:4b50c71291e9 16 outputs in their direction and their seepd by PWM using an H-bridge.
nicovanduijn 0:4b50c71291e9 17 The robot receives steering commands via the XBee module which is
nicovanduijn 0:4b50c71291e9 18 interfaced with from a C# GUI that runs on a desktop computer.
nicovanduijn 0:4b50c71291e9 19
nicovanduijn 0:4b50c71291e9 20 */////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 21
nicovanduijn 0:4b50c71291e9 22 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 23 // Includes
nicovanduijn 0:4b50c71291e9 24 #include "mbed.h" // mbed library
nicovanduijn 0:4b50c71291e9 25 #include "LSM9DS0.h" // 9axis IMU
nicovanduijn 0:4b50c71291e9 26 #include "math.h" // We need to be able to do trig
nicovanduijn 0:4b50c71291e9 27
nicovanduijn 0:4b50c71291e9 28 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 29 // Constants
nicovanduijn 0:4b50c71291e9 30 #define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW
nicovanduijn 0:4b50c71291e9 31 #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW
nicovanduijn 0:4b50c71291e9 32 //#define DEBUG // Comment this out for final version
nicovanduijn 0:4b50c71291e9 33
nicovanduijn 0:4b50c71291e9 34 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 35 // I/O and object instatiation
nicovanduijn 0:4b50c71291e9 36 PwmOut motorSpeedLeft(p21); // PWM port to control speed of left motor
nicovanduijn 0:4b50c71291e9 37 PwmOut motorSpeedRight(p22); // PWM port to control speed of right motor
nicovanduijn 0:4b50c71291e9 38 DigitalOut motorDirLeft(p24); // Digital pin for direction of left motor
nicovanduijn 0:4b50c71291e9 39 DigitalOut NmotorDirLeft(p23); // Usually inverse of motorDirLeft
nicovanduijn 0:4b50c71291e9 40 DigitalOut motorDirRight(p26); // Digital pin for direction of right motor
nicovanduijn 0:4b50c71291e9 41 DigitalOut NmotorDirRight(p25); // Usually inverse of motorDirRight
nicovanduijn 0:4b50c71291e9 42 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); // Creates on object for IMU
nicovanduijn 0:4b50c71291e9 43 Ticker start; // Initialize ticker to call control function
nicovanduijn 2:89ada0b0b923 44 Ticker GUI;
nicovanduijn 2:89ada0b0b923 45 //Serial xbee(p13, p14); // Creates virtual serial for xbee
nicovanduijn 2:89ada0b0b923 46 Serial pc(USBTX,USBRX);// later only use xbee
nicovanduijn 2:89ada0b0b923 47 Timer t; //debugging only
nicovanduijn 2:89ada0b0b923 48 DigitalOut led1(LED1); // debug only
nicovanduijn 2:89ada0b0b923 49 DigitalOut led2(LED2); // debug only
nicovanduijn 2:89ada0b0b923 50 DigitalOut led3(LED3); // debug only
nicovanduijn 2:89ada0b0b923 51 typedef union _data { // maybe there is a better way?
nicovanduijn 2:89ada0b0b923 52 float f;
nicovanduijn 2:89ada0b0b923 53 char chars[4];
nicovanduijn 2:89ada0b0b923 54 int i; // added to receive integers
nicovanduijn 2:89ada0b0b923 55 } myData;
nicovanduijn 2:89ada0b0b923 56
nicovanduijn 0:4b50c71291e9 57
nicovanduijn 0:4b50c71291e9 58 //////////////////////////////////////////////////////////////////
nicovanduijn 2:89ada0b0b923 59 // Globals // double battery, lab floor: 67/100/600/160 cutoff ..35
nicovanduijn 2:89ada0b0b923 60 float kp = 59; //98 // 145 Proportional gain kU 400-500
nicovanduijn 2:89ada0b0b923 61 float kd = 105; //200 // Derivative gain
nicovanduijn 2:89ada0b0b923 62 float ki = 670; //985 // Integrative gain
nicovanduijn 2:89ada0b0b923 63 float OVERALL_SCALAR = 160; //160 // Overall scalar that speed is divided by
nicovanduijn 0:4b50c71291e9 64 float accBias = 0; // Accelerometer Bias
nicovanduijn 0:4b50c71291e9 65 float gyroBias = 0; // Gyro Bias
nicovanduijn 0:4b50c71291e9 66 float accAngle = 0; // Global to hold angle measured by Accelerometer
nicovanduijn 0:4b50c71291e9 67 float gyroAngle = 0; // This variable holds the amount the angle has changed
nicovanduijn 0:4b50c71291e9 68 float speed = 0; // Variable for motor speed
nicovanduijn 0:4b50c71291e9 69 float iAngle = 0; // Integral value of angle-error (sum of gyro-angles)NOT EQUAL TO gyroAngle
nicovanduijn 0:4b50c71291e9 70 float dAngle = 0; // Derivative value for angle, angular velocity, how fast angle is changing
nicovanduijn 0:4b50c71291e9 71 float pAngle = 0; // Proportional value for angle, current angle (best measurement)
nicovanduijn 0:4b50c71291e9 72 float desiredAngle=0; // Setpoint. Set unequal zero to drive
nicovanduijn 2:89ada0b0b923 73 float turnspeed=0; // positive turns
nicovanduijn 2:89ada0b0b923 74 myData bytes;
nicovanduijn 0:4b50c71291e9 75
nicovanduijn 0:4b50c71291e9 76 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 77 // Function Prototypes
nicovanduijn 0:4b50c71291e9 78 void drive(float); // Function declaration for driving the motors
nicovanduijn 0:4b50c71291e9 79 void calibrate(); // Function to calibrate gyro and accelerometer
nicovanduijn 0:4b50c71291e9 80 void control(); // Function implementing PID control
nicovanduijn 0:4b50c71291e9 81 void callback(); // Interrupt triggered function for serial communication
nicovanduijn 2:89ada0b0b923 82 void sendFloatAsBytes();
nicovanduijn 2:89ada0b0b923 83 void updateGUI();
nicovanduijn 0:4b50c71291e9 84 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 85 // Main function
nicovanduijn 0:4b50c71291e9 86 int main()
nicovanduijn 0:4b50c71291e9 87 {
nicovanduijn 2:89ada0b0b923 88
nicovanduijn 2:89ada0b0b923 89 led1=0;//debug only
nicovanduijn 0:4b50c71291e9 90 uint16_t status = imu.begin(); // Use the begin() function to initialize the LSM9DS0 library.
nicovanduijn 2:89ada0b0b923 91 pc.baud(115200);//pretty high, check if lower possible
nicovanduijn 2:89ada0b0b923 92 pc.attach(&callback); // Attach interrupt to serial communication
nicovanduijn 0:4b50c71291e9 93 calibrate(); // Calibrate gyro and accelerometer
nicovanduijn 0:4b50c71291e9 94 start.attach(&control, 0.01); // Looptime 10ms ca. 100Hz
nicovanduijn 2:89ada0b0b923 95 GUI.attach(&updateGUI, 0.5); // update GUI twice a second
nicovanduijn 0:4b50c71291e9 96
nicovanduijn 2:89ada0b0b923 97 while(1) { // Stay in this loop forever
nicovanduijn 2:89ada0b0b923 98 led1=!led1;
nicovanduijn 2:89ada0b0b923 99 wait_ms(500); // blink twice a second
nicovanduijn 0:4b50c71291e9 100 }
nicovanduijn 0:4b50c71291e9 101 }
nicovanduijn 0:4b50c71291e9 102
nicovanduijn 0:4b50c71291e9 103 /////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 104 // Control function, implements PID
nicovanduijn 0:4b50c71291e9 105 void control()
nicovanduijn 0:4b50c71291e9 106 {
nicovanduijn 0:4b50c71291e9 107 dAngle=pAngle; // remember old p-value
nicovanduijn 0:4b50c71291e9 108 imu.readGyro(); // Read the gyro
nicovanduijn 0:4b50c71291e9 109 imu.readAccel(); // Read the Accelerometer
nicovanduijn 0:4b50c71291e9 110 accAngle=(-1)*atan2(imu.ay,imu.az)*180/3.142-90-accBias; // Like this, 0 is upright, forward is negative, backward positive
nicovanduijn 0:4b50c71291e9 111 gyroAngle=-(imu.gx-gyroBias)*0.01; // This is deltaangle, how much angle has changed
nicovanduijn 0:4b50c71291e9 112 pAngle=0.98*(pAngle+gyroAngle)+0.02*accAngle-desiredAngle; // Complementary filter yields best value for current angle
nicovanduijn 0:4b50c71291e9 113 dAngle=pAngle-dAngle; // Ang. Veloc. less noisy than dAngle = -(imu.gx-gyroBias);
nicovanduijn 0:4b50c71291e9 114 iAngle+=(pAngle*.01); // integrate the angle (multiply by timestep to get dt!)
nicovanduijn 0:4b50c71291e9 115
nicovanduijn 2:89ada0b0b923 116 if((abs(pAngle-desiredAngle)>=0.4)&&abs(pAngle-desiredAngle)<=15) { // If it is tilted enough, but not too much ||abs(imu.gx)>10
nicovanduijn 0:4b50c71291e9 117 speed=-(ki*iAngle+kd*dAngle+kp*pAngle)/OVERALL_SCALAR; // drive to correct
nicovanduijn 0:4b50c71291e9 118 if(speed<-1) speed=-1; // Cap if undershoot
nicovanduijn 0:4b50c71291e9 119 else if(speed>1) speed=1; // Cap if overshoot
nicovanduijn 0:4b50c71291e9 120 } else speed=0; // If we've fallen over or are steady on top
nicovanduijn 0:4b50c71291e9 121 drive(speed); // Write speed to the motors
nicovanduijn 2:89ada0b0b923 122
nicovanduijn 0:4b50c71291e9 123 }
nicovanduijn 0:4b50c71291e9 124
nicovanduijn 0:4b50c71291e9 125 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 126 // Drive function
nicovanduijn 0:4b50c71291e9 127 void drive(float speed)
nicovanduijn 0:4b50c71291e9 128 {
nicovanduijn 0:4b50c71291e9 129 int direction=0; // Variable to hold direction we want to drive
nicovanduijn 0:4b50c71291e9 130 if (speed>0)direction=1; // Positive speed indicates forward
nicovanduijn 0:4b50c71291e9 131 if (speed<0)direction=2; // Negative speed indicates backwards
nicovanduijn 0:4b50c71291e9 132 if(speed==0)direction=0; // Zero speed indicates stopping
nicovanduijn 0:4b50c71291e9 133 switch( direction) { // Depending on what direction was passed
nicovanduijn 0:4b50c71291e9 134 case 0: // Stop case
nicovanduijn 0:4b50c71291e9 135 motorSpeedLeft=0; // Set the DigitalOuts to stop the motors
nicovanduijn 0:4b50c71291e9 136 motorSpeedRight=0;
nicovanduijn 0:4b50c71291e9 137 motorDirLeft=0;
nicovanduijn 0:4b50c71291e9 138 NmotorDirLeft=0;
nicovanduijn 0:4b50c71291e9 139 motorDirRight=0;
nicovanduijn 0:4b50c71291e9 140 NmotorDirRight=0;
nicovanduijn 0:4b50c71291e9 141 break;
nicovanduijn 0:4b50c71291e9 142 case 1: // Forward case
nicovanduijn 1:a079ae75a86c 143 motorSpeedLeft=speed-turnspeed; // Set the DigitalOuts to run the motors forward
nicovanduijn 0:4b50c71291e9 144 motorSpeedRight=speed+turnspeed;
nicovanduijn 0:4b50c71291e9 145 motorDirLeft=1;
nicovanduijn 0:4b50c71291e9 146 NmotorDirLeft=0;
nicovanduijn 0:4b50c71291e9 147 motorDirRight=1;
nicovanduijn 0:4b50c71291e9 148 NmotorDirRight=0;
nicovanduijn 0:4b50c71291e9 149 break;
nicovanduijn 0:4b50c71291e9 150 case 2: // Backwards
nicovanduijn 1:a079ae75a86c 151 motorSpeedLeft=-speed+turnspeed; // Set the DigitalOuts to run the motors backward
nicovanduijn 0:4b50c71291e9 152 motorSpeedRight=-speed-turnspeed;
nicovanduijn 0:4b50c71291e9 153 motorDirLeft=0;
nicovanduijn 0:4b50c71291e9 154 NmotorDirLeft=1;
nicovanduijn 0:4b50c71291e9 155 motorDirRight=0;
nicovanduijn 0:4b50c71291e9 156 NmotorDirRight=1;
nicovanduijn 0:4b50c71291e9 157 break;
nicovanduijn 0:4b50c71291e9 158 default: // Catch-all (Stop)
nicovanduijn 0:4b50c71291e9 159 motorSpeedLeft=0; // Set the DigitalOuts to stop the motors
nicovanduijn 0:4b50c71291e9 160 motorSpeedRight=0;
nicovanduijn 0:4b50c71291e9 161 motorDirLeft=0;
nicovanduijn 0:4b50c71291e9 162 NmotorDirLeft=0;
nicovanduijn 0:4b50c71291e9 163 motorDirRight=0;
nicovanduijn 0:4b50c71291e9 164 NmotorDirRight=0;
nicovanduijn 0:4b50c71291e9 165 break;
nicovanduijn 0:4b50c71291e9 166 }
nicovanduijn 0:4b50c71291e9 167 }
nicovanduijn 0:4b50c71291e9 168
nicovanduijn 0:4b50c71291e9 169 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 170 // Calibrate function to find gyro drift and accelerometer bias accbias: -0.3 gyrobias: +0.15
nicovanduijn 0:4b50c71291e9 171 void calibrate()
nicovanduijn 0:4b50c71291e9 172 {
nicovanduijn 1:a079ae75a86c 173 for(int i=0; i<100; i++) { // Read a thousand values
nicovanduijn 0:4b50c71291e9 174 imu.readAccel(); // Read the Accelerometer
nicovanduijn 0:4b50c71291e9 175 imu.readGyro(); // Read the gyro
nicovanduijn 0:4b50c71291e9 176 accBias=accBias+(-1)*atan2(imu.ay,imu.az)*180/3.142-90; // Like this, 0 is upright, forward is negative, backward positive
nicovanduijn 0:4b50c71291e9 177 gyroBias=gyroBias+imu.gx; // Add up all the gyro Biases
nicovanduijn 0:4b50c71291e9 178 }
nicovanduijn 1:a079ae75a86c 179 accBias=accBias/100; // Convert sum to average
nicovanduijn 1:a079ae75a86c 180 gyroBias=gyroBias/100; // Convert sum to average
nicovanduijn 0:4b50c71291e9 181 }
nicovanduijn 0:4b50c71291e9 182
nicovanduijn 0:4b50c71291e9 183 /////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 184 // Callback function to change values/gains
nicovanduijn 0:4b50c71291e9 185 void callback()
nicovanduijn 0:4b50c71291e9 186 {
nicovanduijn 2:89ada0b0b923 187 led2=!led2; // Blink led2 so we know what's happening
nicovanduijn 2:89ada0b0b923 188 char buffer[24]; //create a buffer
nicovanduijn 2:89ada0b0b923 189 int i=0;
nicovanduijn 2:89ada0b0b923 190 while(pc.readable()) { // read the whole damn thing
nicovanduijn 2:89ada0b0b923 191 buffer[i]=pc.getc();
nicovanduijn 2:89ada0b0b923 192 i++;
nicovanduijn 2:89ada0b0b923 193 }
nicovanduijn 2:89ada0b0b923 194 // update kp
nicovanduijn 2:89ada0b0b923 195 bytes.chars[0]=buffer[0];
nicovanduijn 2:89ada0b0b923 196 bytes.chars[1]=buffer[1];
nicovanduijn 2:89ada0b0b923 197 bytes.chars[2]=buffer[2];
nicovanduijn 2:89ada0b0b923 198 bytes.chars[3]=buffer[3];
nicovanduijn 2:89ada0b0b923 199 kp=bytes.i;
nicovanduijn 2:89ada0b0b923 200 // update ki
nicovanduijn 2:89ada0b0b923 201 bytes.chars[0]=buffer[4];
nicovanduijn 2:89ada0b0b923 202 bytes.chars[1]=buffer[5];
nicovanduijn 2:89ada0b0b923 203 bytes.chars[2]=buffer[6];
nicovanduijn 2:89ada0b0b923 204 bytes.chars[3]=buffer[7];
nicovanduijn 2:89ada0b0b923 205 ki=bytes.i;
nicovanduijn 2:89ada0b0b923 206 // update kd
nicovanduijn 2:89ada0b0b923 207 bytes.chars[0]=buffer[8];
nicovanduijn 2:89ada0b0b923 208 bytes.chars[1]=buffer[9];
nicovanduijn 2:89ada0b0b923 209 bytes.chars[2]=buffer[10];
nicovanduijn 2:89ada0b0b923 210 bytes.chars[3]=buffer[11];
nicovanduijn 2:89ada0b0b923 211 kd=bytes.i;
nicovanduijn 2:89ada0b0b923 212 // update OVERALL_SCALAR
nicovanduijn 2:89ada0b0b923 213 bytes.chars[0]=buffer[12];
nicovanduijn 2:89ada0b0b923 214 bytes.chars[1]=buffer[13];
nicovanduijn 2:89ada0b0b923 215 bytes.chars[2]=buffer[14];
nicovanduijn 2:89ada0b0b923 216 bytes.chars[3]=buffer[15];
nicovanduijn 2:89ada0b0b923 217 OVERALL_SCALAR=bytes.i;
nicovanduijn 2:89ada0b0b923 218 // update turnspeed
nicovanduijn 2:89ada0b0b923 219 bytes.chars[0]=buffer[16];
nicovanduijn 2:89ada0b0b923 220 bytes.chars[1]=buffer[17];
nicovanduijn 2:89ada0b0b923 221 bytes.chars[2]=buffer[18];
nicovanduijn 2:89ada0b0b923 222 bytes.chars[3]=buffer[19];
nicovanduijn 2:89ada0b0b923 223 turnspeed=bytes.i;
nicovanduijn 2:89ada0b0b923 224 // update desiredAngle
nicovanduijn 2:89ada0b0b923 225 bytes.chars[0]=buffer[20];
nicovanduijn 2:89ada0b0b923 226 bytes.chars[1]=buffer[21];
nicovanduijn 2:89ada0b0b923 227 bytes.chars[2]=buffer[22];
nicovanduijn 2:89ada0b0b923 228 bytes.chars[3]=buffer[23];
nicovanduijn 2:89ada0b0b923 229 desiredAngle=bytes.i;
nicovanduijn 2:89ada0b0b923 230
nicovanduijn 2:89ada0b0b923 231
nicovanduijn 1:a079ae75a86c 232 // Update kp min: 0 max: 3000
nicovanduijn 1:a079ae75a86c 233 // Update kd min: 0 max: 3000
nicovanduijn 1:a079ae75a86c 234 // Update ki min: 0 max: 3000
nicovanduijn 1:a079ae75a86c 235 // Update OVERALL_SCALAR min: 0 max: 3000
nicovanduijn 1:a079ae75a86c 236 // Update turnspeed min: -0.5 max: 0.5
nicovanduijn 1:a079ae75a86c 237 // Update desiredAngle min: -3 max: 3
nicovanduijn 2:89ada0b0b923 238 // Send pAngle BETTER: kp*pAngle
nicovanduijn 2:89ada0b0b923 239 // Send dAngle BETTER: kd*dAngle
nicovanduijn 2:89ada0b0b923 240 // Send iAngle BETTER: kd*dAngle
nicovanduijn 2:89ada0b0b923 241 }
nicovanduijn 2:89ada0b0b923 242 //////////////////////////////////////////////////////////////////
nicovanduijn 2:89ada0b0b923 243 // maybe get rid of this function and put it into updateGUI to run faster
nicovanduijn 2:89ada0b0b923 244 void sendFloatAsBytes()
nicovanduijn 2:89ada0b0b923 245 {
nicovanduijn 2:89ada0b0b923 246 pc.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
nicovanduijn 2:89ada0b0b923 247 }
nicovanduijn 2:89ada0b0b923 248
nicovanduijn 2:89ada0b0b923 249 ///////////////////////////////////////////////////////////////////
nicovanduijn 2:89ada0b0b923 250 // triggered by ticker GUI ever 0.5s, sends data to PC
nicovanduijn 2:89ada0b0b923 251 void updateGUI()
nicovanduijn 2:89ada0b0b923 252 {
nicovanduijn 2:89ada0b0b923 253 led3=!led3; // blink so we know what's going on
nicovanduijn 2:89ada0b0b923 254 pc.putc('6'); // 6 is for pTerm
nicovanduijn 2:89ada0b0b923 255 bytes.f=pAngle; // later kp*pAngle
nicovanduijn 2:89ada0b0b923 256 sendFloatAsBytes();
nicovanduijn 2:89ada0b0b923 257 wait_us(10); // waiting a little bit is key
nicovanduijn 2:89ada0b0b923 258 pc.putc('7');
nicovanduijn 2:89ada0b0b923 259 bytes.f=iAngle;
nicovanduijn 2:89ada0b0b923 260 sendFloatAsBytes();
nicovanduijn 2:89ada0b0b923 261 wait_us(10);
nicovanduijn 2:89ada0b0b923 262 pc.putc('8');
nicovanduijn 2:89ada0b0b923 263 bytes.f=dAngle;
nicovanduijn 2:89ada0b0b923 264 sendFloatAsBytes();
nicovanduijn 0:4b50c71291e9 265 }