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@1:a079ae75a86c, 2015-04-22 (annotated)

Committer:
nicovanduijn
Date:
Wed Apr 22 19:14:42 2015 +0000
Revision:
1:a079ae75a86c
Parent:
0:4b50c71291e9
Child:
2:89ada0b0b923
ranges of globals to be updated are in comments of callback function

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 0:4b50c71291e9 44 Serial xbee(p13, p14); // Creates virtual serial for xbee
nicovanduijn 0:4b50c71291e9 45
nicovanduijn 0:4b50c71291e9 46
nicovanduijn 0:4b50c71291e9 47 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 48 // Globals
nicovanduijn 1:a079ae75a86c 49 float kp = 200; //98 // 145 Proportional gain kU 400-500
nicovanduijn 1:a079ae75a86c 50 float kd = 200; //200 // Derivative gain
nicovanduijn 1:a079ae75a86c 51 float ki = 1500; //985 // Integrative gain
nicovanduijn 1:a079ae75a86c 52 float OVERALL_SCALAR = 200; //160 // Overall scalar that speed is divided by
nicovanduijn 0:4b50c71291e9 53 float accBias = 0; // Accelerometer Bias
nicovanduijn 0:4b50c71291e9 54 float gyroBias = 0; // Gyro Bias
nicovanduijn 0:4b50c71291e9 55 float accAngle = 0; // Global to hold angle measured by Accelerometer
nicovanduijn 0:4b50c71291e9 56 float gyroAngle = 0; // This variable holds the amount the angle has changed
nicovanduijn 0:4b50c71291e9 57 float speed = 0; // Variable for motor speed
nicovanduijn 0:4b50c71291e9 58 float iAngle = 0; // Integral value of angle-error (sum of gyro-angles)NOT EQUAL TO gyroAngle
nicovanduijn 0:4b50c71291e9 59 float dAngle = 0; // Derivative value for angle, angular velocity, how fast angle is changing
nicovanduijn 0:4b50c71291e9 60 float pAngle = 0; // Proportional value for angle, current angle (best measurement)
nicovanduijn 0:4b50c71291e9 61 float desiredAngle=0; // Setpoint. Set unequal zero to drive
nicovanduijn 0:4b50c71291e9 62 float turnspeed=0; // positive turns
nicovanduijn 0:4b50c71291e9 63
nicovanduijn 0:4b50c71291e9 64 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 65 // Function Prototypes
nicovanduijn 0:4b50c71291e9 66 void drive(float); // Function declaration for driving the motors
nicovanduijn 0:4b50c71291e9 67 void calibrate(); // Function to calibrate gyro and accelerometer
nicovanduijn 0:4b50c71291e9 68 void control(); // Function implementing PID control
nicovanduijn 0:4b50c71291e9 69 void callback(); // Interrupt triggered function for serial communication
nicovanduijn 0:4b50c71291e9 70
nicovanduijn 0:4b50c71291e9 71 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 72 // Main function
nicovanduijn 0:4b50c71291e9 73 int main()
nicovanduijn 0:4b50c71291e9 74 {
nicovanduijn 0:4b50c71291e9 75 uint16_t status = imu.begin(); // Use the begin() function to initialize the LSM9DS0 library.
nicovanduijn 0:4b50c71291e9 76
nicovanduijn 0:4b50c71291e9 77 xbee.attach(&callback); // Attach interrupt to serial communication
nicovanduijn 0:4b50c71291e9 78 calibrate(); // Calibrate gyro and accelerometer
nicovanduijn 0:4b50c71291e9 79 start.attach(&control, 0.01); // Looptime 10ms ca. 100Hz
nicovanduijn 0:4b50c71291e9 80 while(1) { // Stay in this loop forever
nicovanduijn 0:4b50c71291e9 81
nicovanduijn 0:4b50c71291e9 82 }
nicovanduijn 0:4b50c71291e9 83 }
nicovanduijn 0:4b50c71291e9 84
nicovanduijn 0:4b50c71291e9 85 /////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 86 // Control function, implements PID
nicovanduijn 0:4b50c71291e9 87 void control()
nicovanduijn 0:4b50c71291e9 88 {
nicovanduijn 0:4b50c71291e9 89 dAngle=pAngle; // remember old p-value
nicovanduijn 0:4b50c71291e9 90 imu.readGyro(); // Read the gyro
nicovanduijn 0:4b50c71291e9 91 imu.readAccel(); // Read the Accelerometer
nicovanduijn 0:4b50c71291e9 92 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 93 gyroAngle=-(imu.gx-gyroBias)*0.01; // This is deltaangle, how much angle has changed
nicovanduijn 0:4b50c71291e9 94 pAngle=0.98*(pAngle+gyroAngle)+0.02*accAngle-desiredAngle; // Complementary filter yields best value for current angle
nicovanduijn 0:4b50c71291e9 95 dAngle=pAngle-dAngle; // Ang. Veloc. less noisy than dAngle = -(imu.gx-gyroBias);
nicovanduijn 0:4b50c71291e9 96 iAngle+=(pAngle*.01); // integrate the angle (multiply by timestep to get dt!)
nicovanduijn 0:4b50c71291e9 97
nicovanduijn 1:a079ae75a86c 98 if((abs(pAngle-desiredAngle)>=0.6)&&abs(pAngle-desiredAngle)<=15) { // If it is tilted enough, but not too much ||abs(imu.gx)>8
nicovanduijn 0:4b50c71291e9 99 speed=-(ki*iAngle+kd*dAngle+kp*pAngle)/OVERALL_SCALAR; // drive to correct
nicovanduijn 0:4b50c71291e9 100 if(speed<-1) speed=-1; // Cap if undershoot
nicovanduijn 0:4b50c71291e9 101 else if(speed>1) speed=1; // Cap if overshoot
nicovanduijn 0:4b50c71291e9 102 } else speed=0; // If we've fallen over or are steady on top
nicovanduijn 0:4b50c71291e9 103 drive(speed); // Write speed to the motors
nicovanduijn 0:4b50c71291e9 104
nicovanduijn 0:4b50c71291e9 105 }
nicovanduijn 0:4b50c71291e9 106
nicovanduijn 0:4b50c71291e9 107 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 108 // Drive function
nicovanduijn 0:4b50c71291e9 109 void drive(float speed)
nicovanduijn 0:4b50c71291e9 110 {
nicovanduijn 0:4b50c71291e9 111 int direction=0; // Variable to hold direction we want to drive
nicovanduijn 0:4b50c71291e9 112 if (speed>0)direction=1; // Positive speed indicates forward
nicovanduijn 0:4b50c71291e9 113 if (speed<0)direction=2; // Negative speed indicates backwards
nicovanduijn 0:4b50c71291e9 114 if(speed==0)direction=0; // Zero speed indicates stopping
nicovanduijn 0:4b50c71291e9 115 switch( direction) { // Depending on what direction was passed
nicovanduijn 0:4b50c71291e9 116 case 0: // Stop case
nicovanduijn 0:4b50c71291e9 117 motorSpeedLeft=0; // Set the DigitalOuts to stop the motors
nicovanduijn 0:4b50c71291e9 118 motorSpeedRight=0;
nicovanduijn 0:4b50c71291e9 119 motorDirLeft=0;
nicovanduijn 0:4b50c71291e9 120 NmotorDirLeft=0;
nicovanduijn 0:4b50c71291e9 121 motorDirRight=0;
nicovanduijn 0:4b50c71291e9 122 NmotorDirRight=0;
nicovanduijn 0:4b50c71291e9 123 break;
nicovanduijn 0:4b50c71291e9 124 case 1: // Forward case
nicovanduijn 1:a079ae75a86c 125 motorSpeedLeft=speed-turnspeed; // Set the DigitalOuts to run the motors forward
nicovanduijn 0:4b50c71291e9 126 motorSpeedRight=speed+turnspeed;
nicovanduijn 0:4b50c71291e9 127 motorDirLeft=1;
nicovanduijn 0:4b50c71291e9 128 NmotorDirLeft=0;
nicovanduijn 0:4b50c71291e9 129 motorDirRight=1;
nicovanduijn 0:4b50c71291e9 130 NmotorDirRight=0;
nicovanduijn 0:4b50c71291e9 131 break;
nicovanduijn 0:4b50c71291e9 132 case 2: // Backwards
nicovanduijn 1:a079ae75a86c 133 motorSpeedLeft=-speed+turnspeed; // Set the DigitalOuts to run the motors backward
nicovanduijn 0:4b50c71291e9 134 motorSpeedRight=-speed-turnspeed;
nicovanduijn 0:4b50c71291e9 135 motorDirLeft=0;
nicovanduijn 0:4b50c71291e9 136 NmotorDirLeft=1;
nicovanduijn 0:4b50c71291e9 137 motorDirRight=0;
nicovanduijn 0:4b50c71291e9 138 NmotorDirRight=1;
nicovanduijn 0:4b50c71291e9 139 break;
nicovanduijn 0:4b50c71291e9 140 default: // Catch-all (Stop)
nicovanduijn 0:4b50c71291e9 141 motorSpeedLeft=0; // Set the DigitalOuts to stop the motors
nicovanduijn 0:4b50c71291e9 142 motorSpeedRight=0;
nicovanduijn 0:4b50c71291e9 143 motorDirLeft=0;
nicovanduijn 0:4b50c71291e9 144 NmotorDirLeft=0;
nicovanduijn 0:4b50c71291e9 145 motorDirRight=0;
nicovanduijn 0:4b50c71291e9 146 NmotorDirRight=0;
nicovanduijn 0:4b50c71291e9 147 break;
nicovanduijn 0:4b50c71291e9 148 }
nicovanduijn 0:4b50c71291e9 149 }
nicovanduijn 0:4b50c71291e9 150
nicovanduijn 0:4b50c71291e9 151 //////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 152 // Calibrate function to find gyro drift and accelerometer bias accbias: -0.3 gyrobias: +0.15
nicovanduijn 0:4b50c71291e9 153 void calibrate()
nicovanduijn 0:4b50c71291e9 154 {
nicovanduijn 1:a079ae75a86c 155 for(int i=0; i<100; i++) { // Read a thousand values
nicovanduijn 0:4b50c71291e9 156 imu.readAccel(); // Read the Accelerometer
nicovanduijn 0:4b50c71291e9 157 imu.readGyro(); // Read the gyro
nicovanduijn 0:4b50c71291e9 158 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 159 gyroBias=gyroBias+imu.gx; // Add up all the gyro Biases
nicovanduijn 0:4b50c71291e9 160 }
nicovanduijn 1:a079ae75a86c 161 accBias=accBias/100; // Convert sum to average
nicovanduijn 1:a079ae75a86c 162 gyroBias=gyroBias/100; // Convert sum to average
nicovanduijn 0:4b50c71291e9 163 }
nicovanduijn 0:4b50c71291e9 164
nicovanduijn 0:4b50c71291e9 165 /////////////////////////////////////////////////////////////////
nicovanduijn 0:4b50c71291e9 166 // Callback function to change values/gains
nicovanduijn 0:4b50c71291e9 167 void callback()
nicovanduijn 0:4b50c71291e9 168 {
nicovanduijn 1:a079ae75a86c 169 // Update kp min: 0 max: 3000
nicovanduijn 1:a079ae75a86c 170 // Update kd min: 0 max: 3000
nicovanduijn 1:a079ae75a86c 171 // Update ki min: 0 max: 3000
nicovanduijn 1:a079ae75a86c 172 // Update OVERALL_SCALAR min: 0 max: 3000
nicovanduijn 1:a079ae75a86c 173 // Update turnspeed min: -0.5 max: 0.5
nicovanduijn 1:a079ae75a86c 174 // Update desiredAngle min: -3 max: 3
nicovanduijn 1:a079ae75a86c 175 // Send pAngle
nicovanduijn 0:4b50c71291e9 176 // Send dAngle
nicovanduijn 0:4b50c71291e9 177 // Send iAngle
nicovanduijn 0:4b50c71291e9 178 }