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@4:51ea148fc592, 2015-04-30 (annotated)

Committer:
nicovanduijn
Date:
Thu Apr 30 22:32:05 2015 +0000
Revision:
4:51ea148fc592
Parent:
3:89e4ed1324bb 
Final Version with extensive comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nicovanduijn 4:51ea148fc592 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 // Includes
nicovanduijn 0:4b50c71291e9 23 #include "mbed.h" // mbed library
nicovanduijn 0:4b50c71291e9 24 #include "LSM9DS0.h" // 9axis IMU
nicovanduijn 0:4b50c71291e9 25 #include "math.h" // We need to be able to do trig
nicovanduijn 0:4b50c71291e9 26
nicovanduijn 0:4b50c71291e9 27 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 28 // Constants
nicovanduijn 0:4b50c71291e9 29 #define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW
nicovanduijn 0:4b50c71291e9 30 #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW
nicovanduijn 4:51ea148fc592 31 #define LOOPTIME 0.01 // Determines the looptime. 100Hz is pretty stable
nicovanduijn 0:4b50c71291e9 32
nicovanduijn 0:4b50c71291e9 33 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 34 // I/O and object instatiation
nicovanduijn 0:4b50c71291e9 35 PwmOut motorSpeedLeft(p21); // PWM port to control speed of left motor
nicovanduijn 0:4b50c71291e9 36 PwmOut motorSpeedRight(p22); // PWM port to control speed of right motor
nicovanduijn 0:4b50c71291e9 37 DigitalOut motorDirLeft(p24); // Digital pin for direction of left motor
nicovanduijn 0:4b50c71291e9 38 DigitalOut NmotorDirLeft(p23); // Usually inverse of motorDirLeft
nicovanduijn 0:4b50c71291e9 39 DigitalOut motorDirRight(p26); // Digital pin for direction of right motor
nicovanduijn 0:4b50c71291e9 40 DigitalOut NmotorDirRight(p25); // Usually inverse of motorDirRight
nicovanduijn 0:4b50c71291e9 41 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); // Creates on object for IMU
nicovanduijn 0:4b50c71291e9 42 Ticker start; // Initialize ticker to call control function
nicovanduijn 3:89e4ed1324bb 43 Ticker GUI; // Ticker that calls the updateGUI
nicovanduijn 3:89e4ed1324bb 44 DigitalOut led1(LED1); // Use LED1 to provide some runtime info
nicovanduijn 4:51ea148fc592 45 Serial xbee(p13,p14); // Create serial port for Xbee
nicovanduijn 3:89e4ed1324bb 46
nicovanduijn 3:89e4ed1324bb 47 typedef union _data { // Typedef so we can jump from chars to floats
nicovanduijn 2:89ada0b0b923 48 float f;
nicovanduijn 4:51ea148fc592 49 char c[4];
nicovanduijn 2:89ada0b0b923 50 } myData;
nicovanduijn 2:89ada0b0b923 51
nicovanduijn 0:4b50c71291e9 52 //////////////////////////////////////////////////////////////////
nicovanduijn 4:51ea148fc592 53 // Globals
nicovanduijn 4:51ea148fc592 54 float kp = 59; // Proportional gain
nicovanduijn 4:51ea148fc592 55 float kd = 105; // Derivative gain
nicovanduijn 4:51ea148fc592 56 float ki = 670; // Integrative gain
nicovanduijn 4:51ea148fc592 57 float overallScalar = 160; // Overall scalar that speed is divided by
nicovanduijn 0:4b50c71291e9 58 float accBias = 0; // Accelerometer Bias
nicovanduijn 0:4b50c71291e9 59 float gyroBias = 0; // Gyro Bias
nicovanduijn 0:4b50c71291e9 60 float accAngle = 0; // Global to hold angle measured by Accelerometer
nicovanduijn 0:4b50c71291e9 61 float gyroAngle = 0; // This variable holds the amount the angle has changed
nicovanduijn 0:4b50c71291e9 62 float speed = 0; // Variable for motor speed
nicovanduijn 0:4b50c71291e9 63 float iAngle = 0; // Integral value of angle-error (sum of gyro-angles)NOT EQUAL TO gyroAngle
nicovanduijn 0:4b50c71291e9 64 float dAngle = 0; // Derivative value for angle, angular velocity, how fast angle is changing
nicovanduijn 0:4b50c71291e9 65 float pAngle = 0; // Proportional value for angle, current angle (best measurement)
nicovanduijn 0:4b50c71291e9 66 float desiredAngle=0; // Setpoint. Set unequal zero to drive
nicovanduijn 3:89e4ed1324bb 67 float turnspeed=0; // Makes the robot turn
nicovanduijn 3:89e4ed1324bb 68 myData bytes; // Used to convert received/sent chars to ints and floats
nicovanduijn 4:51ea148fc592 69 float cutoff=0.2; // Noise-level cutoff
nicovanduijn 3:89e4ed1324bb 70
nicovanduijn 0:4b50c71291e9 71
nicovanduijn 0:4b50c71291e9 72 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 73 // Function Prototypes
nicovanduijn 0:4b50c71291e9 74 void drive(float); // Function declaration for driving the motors
nicovanduijn 0:4b50c71291e9 75 void calibrate(); // Function to calibrate gyro and accelerometer
nicovanduijn 0:4b50c71291e9 76 void control(); // Function implementing PID control
nicovanduijn 4:51ea148fc592 77 void updateGUI(); // Function that sends values to the GUI
nicovanduijn 4:51ea148fc592 78 void checkValues(); // Function that receives controls from GUI
nicovanduijn 3:89e4ed1324bb 79
nicovanduijn 0:4b50c71291e9 80 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 81 // Main function
nicovanduijn 0:4b50c71291e9 82 int main()
nicovanduijn 0:4b50c71291e9 83 {
nicovanduijn 3:89e4ed1324bb 84 led1=0; // Turn led off
nicovanduijn 0:4b50c71291e9 85 uint16_t status = imu.begin(); // Use the begin() function to initialize the LSM9DS0 library.
nicovanduijn 4:51ea148fc592 86 xbee.baud(115200); // Baudrate
nicovanduijn 0:4b50c71291e9 87 calibrate(); // Calibrate gyro and accelerometer
nicovanduijn 4:51ea148fc592 88 start.attach(&control, LOOPTIME); // Looptime 10ms ca. 100Hz
nicovanduijn 3:89e4ed1324bb 89 GUI.attach(&updateGUI, 0.5); // Update GUI twice a second
nicovanduijn 0:4b50c71291e9 90
nicovanduijn 3:89e4ed1324bb 91 while(1) { // Stay in this loop forever
nicovanduijn 3:89e4ed1324bb 92 led1=!led1; // Blink led1 to make sure the loop is running
nicovanduijn 3:89e4ed1324bb 93 wait_ms(500); // Looptime 500ms
nicovanduijn 0:4b50c71291e9 94 }
nicovanduijn 0:4b50c71291e9 95 }
nicovanduijn 0:4b50c71291e9 96
nicovanduijn 0:4b50c71291e9 97 /////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 98 // Control function, implements PID
nicovanduijn 0:4b50c71291e9 99 void control()
nicovanduijn 0:4b50c71291e9 100 {
nicovanduijn 0:4b50c71291e9 101 dAngle=pAngle; // remember old p-value
nicovanduijn 0:4b50c71291e9 102 imu.readGyro(); // Read the gyro
nicovanduijn 0:4b50c71291e9 103 imu.readAccel(); // Read the Accelerometer
nicovanduijn 0:4b50c71291e9 104 accAngle=(-1)*atan2(imu.ay,imu.az)*180/3.142-90-accBias; // Like this, 0 is upright, forward is negative, backward positive
nicovanduijn 4:51ea148fc592 105 gyroAngle=-(imu.gx-gyroBias)*LOOPTIME; // This is deltaangle, how much angle has changed
nicovanduijn 0:4b50c71291e9 106 pAngle=0.98*(pAngle+gyroAngle)+0.02*accAngle-desiredAngle; // Complementary filter yields best value for current angle
nicovanduijn 0:4b50c71291e9 107 dAngle=pAngle-dAngle; // Ang. Veloc. less noisy than dAngle = -(imu.gx-gyroBias);
nicovanduijn 4:51ea148fc592 108 iAngle+=(pAngle*LOOPTIME); // Integrate the angle (multiply by timestep to get dt!)
nicovanduijn 0:4b50c71291e9 109
nicovanduijn 4:51ea148fc592 110 if((abs(pAngle-desiredAngle)>=cutoff)&&abs(pAngle-desiredAngle)<=15) { // If it is tilted enough, but not too much
nicovanduijn 4:51ea148fc592 111 speed=-(ki*iAngle+kd*dAngle+kp*pAngle)/overallScalar; // drive to correct
nicovanduijn 0:4b50c71291e9 112 if(speed<-1) speed=-1; // Cap if undershoot
nicovanduijn 0:4b50c71291e9 113 else if(speed>1) speed=1; // Cap if overshoot
nicovanduijn 0:4b50c71291e9 114 } else speed=0; // If we've fallen over or are steady on top
nicovanduijn 0:4b50c71291e9 115 drive(speed); // Write speed to the motors
nicovanduijn 4:51ea148fc592 116 checkValues(); // Checks if we need to update some values
nicovanduijn 2:89ada0b0b923 117
nicovanduijn 0:4b50c71291e9 118 }
nicovanduijn 0:4b50c71291e9 119
nicovanduijn 0:4b50c71291e9 120 //////////////////////////////////////////////////////////////////
nicovanduijn 4:51ea148fc592 121 // drive() function to drive the motors at the given speed
nicovanduijn 0:4b50c71291e9 122 void drive(float speed)
nicovanduijn 0:4b50c71291e9 123 {
nicovanduijn 0:4b50c71291e9 124 int direction=0; // Variable to hold direction we want to drive
nicovanduijn 0:4b50c71291e9 125 if (speed>0)direction=1; // Positive speed indicates forward
nicovanduijn 0:4b50c71291e9 126 if (speed<0)direction=2; // Negative speed indicates backwards
nicovanduijn 0:4b50c71291e9 127 if(speed==0)direction=0; // Zero speed indicates stopping
nicovanduijn 0:4b50c71291e9 128 switch( direction) { // Depending on what direction was passed
nicovanduijn 0:4b50c71291e9 129 case 0: // Stop case
nicovanduijn 0:4b50c71291e9 130 motorSpeedLeft=0; // Set the DigitalOuts to stop the motors
nicovanduijn 0:4b50c71291e9 131 motorSpeedRight=0;
nicovanduijn 0:4b50c71291e9 132 motorDirLeft=0;
nicovanduijn 0:4b50c71291e9 133 NmotorDirLeft=0;
nicovanduijn 0:4b50c71291e9 134 motorDirRight=0;
nicovanduijn 0:4b50c71291e9 135 NmotorDirRight=0;
nicovanduijn 0:4b50c71291e9 136 break;
nicovanduijn 0:4b50c71291e9 137 case 1: // Forward case
nicovanduijn 1:a079ae75a86c 138 motorSpeedLeft=speed-turnspeed; // Set the DigitalOuts to run the motors forward
nicovanduijn 0:4b50c71291e9 139 motorSpeedRight=speed+turnspeed;
nicovanduijn 0:4b50c71291e9 140 motorDirLeft=1;
nicovanduijn 0:4b50c71291e9 141 NmotorDirLeft=0;
nicovanduijn 0:4b50c71291e9 142 motorDirRight=1;
nicovanduijn 0:4b50c71291e9 143 NmotorDirRight=0;
nicovanduijn 0:4b50c71291e9 144 break;
nicovanduijn 0:4b50c71291e9 145 case 2: // Backwards
nicovanduijn 1:a079ae75a86c 146 motorSpeedLeft=-speed+turnspeed; // Set the DigitalOuts to run the motors backward
nicovanduijn 0:4b50c71291e9 147 motorSpeedRight=-speed-turnspeed;
nicovanduijn 0:4b50c71291e9 148 motorDirLeft=0;
nicovanduijn 0:4b50c71291e9 149 NmotorDirLeft=1;
nicovanduijn 0:4b50c71291e9 150 motorDirRight=0;
nicovanduijn 0:4b50c71291e9 151 NmotorDirRight=1;
nicovanduijn 0:4b50c71291e9 152 break;
nicovanduijn 0:4b50c71291e9 153 default: // Catch-all (Stop)
nicovanduijn 0:4b50c71291e9 154 motorSpeedLeft=0; // Set the DigitalOuts to stop the motors
nicovanduijn 0:4b50c71291e9 155 motorSpeedRight=0;
nicovanduijn 0:4b50c71291e9 156 motorDirLeft=0;
nicovanduijn 0:4b50c71291e9 157 NmotorDirLeft=0;
nicovanduijn 0:4b50c71291e9 158 motorDirRight=0;
nicovanduijn 0:4b50c71291e9 159 NmotorDirRight=0;
nicovanduijn 0:4b50c71291e9 160 break;
nicovanduijn 0:4b50c71291e9 161 }
nicovanduijn 0:4b50c71291e9 162 }
nicovanduijn 0:4b50c71291e9 163
nicovanduijn 0:4b50c71291e9 164 //////////////////////////////////////////////////////////////////
nicovanduijn 4:51ea148fc592 165 /* Calibrate()
nicovanduijn 4:51ea148fc592 166 * function to find gyro drift and accelerometer bias
nicovanduijn 4:51ea148fc592 167 */
nicovanduijn 0:4b50c71291e9 168 void calibrate()
nicovanduijn 0:4b50c71291e9 169 {
nicovanduijn 4:51ea148fc592 170 for(int i=0; i<100; i++) { // Read one hundred values
nicovanduijn 0:4b50c71291e9 171 imu.readAccel(); // Read the Accelerometer
nicovanduijn 0:4b50c71291e9 172 imu.readGyro(); // Read the gyro
nicovanduijn 0:4b50c71291e9 173 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 174 gyroBias=gyroBias+imu.gx; // Add up all the gyro Biases
nicovanduijn 0:4b50c71291e9 175 }
nicovanduijn 1:a079ae75a86c 176 accBias=accBias/100; // Convert sum to average
nicovanduijn 1:a079ae75a86c 177 gyroBias=gyroBias/100; // Convert sum to average
nicovanduijn 0:4b50c71291e9 178 }
nicovanduijn 0:4b50c71291e9 179
nicovanduijn 3:89e4ed1324bb 180
nicovanduijn 3:89e4ed1324bb 181 ///////////////////////////////////////////////////////////////////
nicovanduijn 3:89e4ed1324bb 182 // updateGUI(), triggered by ticker GUI ever 0.5s, sends data to xbee
nicovanduijn 3:89e4ed1324bb 183 void updateGUI()
nicovanduijn 0:4b50c71291e9 184 {
nicovanduijn 4:51ea148fc592 185 xbee.putc('*'); // Send data validity value
nicovanduijn 3:89e4ed1324bb 186 bytes.f = accBias; // Send accBias
nicovanduijn 4:51ea148fc592 187 xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
nicovanduijn 3:89e4ed1324bb 188 bytes.f = gyroBias; // Send gyroBias
nicovanduijn 4:51ea148fc592 189 xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
nicovanduijn 3:89e4ed1324bb 190 bytes.f = pAngle; // Send P Angle
nicovanduijn 4:51ea148fc592 191 xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
nicovanduijn 3:89e4ed1324bb 192 bytes.f = imu.gx; // Send current angular velocity
nicovanduijn 4:51ea148fc592 193 xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
nicovanduijn 3:89e4ed1324bb 194 bytes.f = turnspeed; // Send turn speed
nicovanduijn 4:51ea148fc592 195 xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
nicovanduijn 4:51ea148fc592 196 bytes.f = (pAngle * kp) / overallScalar; // Send P Value
nicovanduijn 4:51ea148fc592 197 xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
nicovanduijn 4:51ea148fc592 198 bytes.f = (iAngle * ki) / overallScalar; // Send I Value
nicovanduijn 4:51ea148fc592 199 xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
nicovanduijn 4:51ea148fc592 200 bytes.f = (dAngle * kd) / overallScalar; // Send D Value
nicovanduijn 4:51ea148fc592 201 xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
nicovanduijn 4:51ea148fc592 202 xbee.putc('\n'); // Send delimiting character
nicovanduijn 2:89ada0b0b923 203 }
nicovanduijn 2:89ada0b0b923 204 //////////////////////////////////////////////////////////////////
nicovanduijn 3:89e4ed1324bb 205 // checkValues() updates globals received from xbee
nicovanduijn 3:89e4ed1324bb 206 void checkValues()
nicovanduijn 2:89ada0b0b923 207 {
nicovanduijn 3:89e4ed1324bb 208 int i=0; // Integer needed for looping through input buffer
nicovanduijn 3:89e4ed1324bb 209 char buffer[6]; // Buffer to hold all the received data
nicovanduijn 4:51ea148fc592 210 while(xbee.readable()) { // As long as there is stuff in the buffer
nicovanduijn 4:51ea148fc592 211 buffer[i++]=xbee.getc(); // Read from serial
nicovanduijn 3:89e4ed1324bb 212 }
nicovanduijn 3:89e4ed1324bb 213
nicovanduijn 3:89e4ed1324bb 214 if(buffer[0]== '*') { // Check for 'start' character
nicovanduijn 3:89e4ed1324bb 215 switch(buffer[1]) { // Switch depending on what value we update
nicovanduijn 3:89e4ed1324bb 216 case '1': // Updating kp
nicovanduijn 4:51ea148fc592 217 bytes.c[0] = buffer[2];
nicovanduijn 4:51ea148fc592 218 bytes.c[1] = buffer[3];
nicovanduijn 4:51ea148fc592 219 bytes.c[2] = buffer[4];
nicovanduijn 4:51ea148fc592 220 bytes.c[3] = buffer[5];
nicovanduijn 3:89e4ed1324bb 221 kp=bytes.f;
nicovanduijn 3:89e4ed1324bb 222 break;
nicovanduijn 3:89e4ed1324bb 223 case '2': // Updating kd
nicovanduijn 4:51ea148fc592 224 bytes.c[0] = buffer[2];
nicovanduijn 4:51ea148fc592 225 bytes.c[1] = buffer[3];
nicovanduijn 4:51ea148fc592 226 bytes.c[2] = buffer[4];
nicovanduijn 4:51ea148fc592 227 bytes.c[3] = buffer[5];
nicovanduijn 3:89e4ed1324bb 228 kd=bytes.f;
nicovanduijn 3:89e4ed1324bb 229 break;
nicovanduijn 3:89e4ed1324bb 230 case '3': // Updating ki
nicovanduijn 4:51ea148fc592 231 bytes.c[0] = buffer[2];
nicovanduijn 4:51ea148fc592 232 bytes.c[1] = buffer[3];
nicovanduijn 4:51ea148fc592 233 bytes.c[2] = buffer[4];
nicovanduijn 4:51ea148fc592 234 bytes.c[3] = buffer[5];
nicovanduijn 3:89e4ed1324bb 235 ki=bytes.f;
nicovanduijn 3:89e4ed1324bb 236 break;
nicovanduijn 4:51ea148fc592 237 case '4': // Updating overallScalar
nicovanduijn 4:51ea148fc592 238 bytes.c[0] = buffer[2];
nicovanduijn 4:51ea148fc592 239 bytes.c[1] = buffer[3];
nicovanduijn 4:51ea148fc592 240 bytes.c[2] = buffer[4];
nicovanduijn 4:51ea148fc592 241 bytes.c[3] = buffer[5];
nicovanduijn 4:51ea148fc592 242 overallScalar=bytes.f;
nicovanduijn 3:89e4ed1324bb 243 break;
nicovanduijn 3:89e4ed1324bb 244 case '5': // Updating desiredAngle
nicovanduijn 4:51ea148fc592 245 bytes.c[0] = buffer[2];
nicovanduijn 4:51ea148fc592 246 bytes.c[1] = buffer[3];
nicovanduijn 4:51ea148fc592 247 bytes.c[2] = buffer[4];
nicovanduijn 4:51ea148fc592 248 bytes.c[3] = buffer[5];
nicovanduijn 3:89e4ed1324bb 249 desiredAngle=bytes.f;
nicovanduijn 3:89e4ed1324bb 250 break;
nicovanduijn 4:51ea148fc592 251 case '6': // Updating turnspeed
nicovanduijn 4:51ea148fc592 252 bytes.c[0] = buffer[2];
nicovanduijn 4:51ea148fc592 253 bytes.c[1] = buffer[3];
nicovanduijn 4:51ea148fc592 254 bytes.c[2] = buffer[4];
nicovanduijn 4:51ea148fc592 255 bytes.c[3] = buffer[5];
nicovanduijn 3:89e4ed1324bb 256 turnspeed=bytes.f;
nicovanduijn 3:89e4ed1324bb 257 }
nicovanduijn 3:89e4ed1324bb 258 }
nicovanduijn 3:89e4ed1324bb 259
nicovanduijn 0:4b50c71291e9 260 }