ECE 4180 Spring 15
ECE 4180 Mini Project Balancing Robot Nico van Duijn Samer Mabrouk 3/6/2015 This project consists of a robot balancing on two wheels. We use the 9-axis IMU LSM9DS0 to give us Accelerometer and Gyroscope data about the current angle and angular velocity of the robot. This data is then filtered using complementary filters and PID control to drive the two motors. The motors are controlled through digital outputs in their direction and their seepd by PWM using an H-bridge
Revision 3:dd0c62b586ea, committed 2015-04-20
- Comitter:
- nicovanduijn
- Date:
- Mon Apr 20 16:33:09 2015 +0000
- Parent:
- 2:ad080363a22c
- Commit message:
- most current version;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r ad080363a22c -r dd0c62b586ea main.cpp
--- a/main.cpp Mon Apr 06 16:25:18 2015 +0000
+++ b/main.cpp Mon Apr 20 16:33:09 2015 +0000
@@ -1,16 +1,22 @@
/*////////////////////////////////////////////////////////////////
-ECE 4180 Mini Project
+ECE 4180 Final Project
Balancing Robot
+
Nico van Duijn
Samer Mabrouk
-3/6/2015
+Anthony Agnone
+Jay
+4/20/2015
This project consists of a robot balancing on two wheels. We use
the 9-axis IMU LSM9DS0 to give us Accelerometer and Gyroscope data
about the current angle and angular velocity of the robot. This
data is then filtered using complementary filters and PID control
to drive the two motors. The motors are controlled through digital
-outputs in their direction and their seepd by PWM using an H-bridge
+outputs in their direction and their seepd by PWM using an H-bridge.
+The robot receives steering commands via the XBee module which is
+interfaced with from a C# GUI that runs on a desktop computer.
+
*/////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
@@ -23,14 +29,14 @@
// Constants
#define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW
-#define DEBUG // Comment this out for final version
+//#define DEBUG // Comment this out for final version
//////////////////////////////////////////////////////////////////
// I/O and object instatiation
PwmOut motorSpeedLeft(p21); // PWM port to control speed of left motor
PwmOut motorSpeedRight(p22); // PWM port to control speed of right motor
-DigitalOut motorDirLeft(p23); // Digital pin for direction of left motor
-DigitalOut NmotorDirLeft(p24); // Usually inverse of motorDirLeft
+DigitalOut motorDirLeft(p24); // Digital pin for direction of left motor
+DigitalOut NmotorDirLeft(p23); // Usually inverse of motorDirLeft
DigitalOut motorDirRight(p26); // Digital pin for direction of right motor
DigitalOut NmotorDirRight(p25); // Usually inverse of motorDirRight
LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR); // Creates on object for IMU
@@ -41,10 +47,10 @@
//////////////////////////////////////////////////////////////////
// Globals
-float kp = 800; // Proportional gain
-float kd = 90; // Derivative gain
-float ki = 4; // Integrative gain
-float OVERALL_SCALAR = 7000; // Overall scalar that speed is divided by
+float kp = 98; //40 // 145 Proportional gain kU 400-500
+float kd = 200; //2 was quite good // Derivative gain
+float ki = 935; //30 // Integrative gain
+float OVERALL_SCALAR = 170; // Overall scalar that speed is divided by
float accBias = 0; // Accelerometer Bias
float gyroBias = 0; // Gyro Bias
float accAngle = 0; // Global to hold angle measured by Accelerometer
@@ -53,6 +59,8 @@
float iAngle = 0; // Integral value of angle-error (sum of gyro-angles)NOT EQUAL TO gyroAngle
float dAngle = 0; // Derivative value for angle, angular velocity, how fast angle is changing
float pAngle = 0; // Proportional value for angle, current angle (best measurement)
+float desiredAngle=0; // Setpoint. Set unequal zero to drive
+// float turnspeed=0;
//////////////////////////////////////////////////////////////////
// Function Prototypes
@@ -86,19 +94,25 @@
// Control function, implements PID
void control()
{
+ dAngle=pAngle;// remember old p-value
imu.readGyro(); // Read the gyro
imu.readAccel(); // Read the Accelerometer
accAngle=(-1)*atan2(imu.ay,imu.az)*180/3.142-90-accBias; // Like this, 0 is upright, forward is negative, backward positive
gyroAngle=-(imu.gx-gyroBias)*0.01; // This is deltaangle, how much angle has changed
- pAngle=0.98*(pAngle+gyroAngle)+0.02*accAngle; // Complementary filter yields best value for current angle
- dAngle = -(imu.gx-gyroBias); // This is angular velocity
- iAngle=(0.99*iAngle+0.01*gyroAngle); // Sorta- running average-integral thing
- if(abs(pAngle)>=0.6&&abs(pAngle)<=25) { // If it is tilted enough, but not too much
+ pAngle=0.98*(pAngle+gyroAngle)+0.02*accAngle-desiredAngle; // Complementary filter yields best value for current angle
+ //iAngle=(0.9*iAngle+0.1*gyroAngle); DOESNT ACTUALLY INTEGRATE ERROR // Sorta- running average-integral thing
+ dAngle=pAngle-dAngle; //Ang. Veloc. less noisy than dAngle = -(imu.gx-gyroBias);
+ iAngle+=(pAngle*.01);// integrate the angle (multiply by timestep to get dt!)
+ //if((ki*iAngle/OVERALL_SCALAR)>=3)iAngle=3*OVERALL_SCALAR/ki;// if ki dominates three-fold
+ //if((ki*iAngle/OVERALL_SCALAR)<=-3)iAngle=-3*OVERALL_SCALAR/ki;//50 is an arbitrary cap - kind of worked
+ // try angle dev. .55 and find value for imu.gx
+ if(abs(pAngle-desiredAngle)>=0.6&&abs(pAngle-desiredAngle)<=15) { // If it is tilted enough, but not too much
speed=-(ki*iAngle+kd*dAngle+kp*pAngle)/OVERALL_SCALAR; // drive to correct
if(speed<-1) speed=-1; // Cap if undershoot
else if(speed>1) speed=1; // Cap if overshoot
} else speed=0; // If we've fallen over or are steady on top
drive(speed); // Write speed to the motors
+
}
//////////////////////////////////////////////////////////////////
@@ -146,17 +160,17 @@
}
//////////////////////////////////////////////////////////////////
-// Calibrate function to find gyro drift and accelerometer bias
+// Calibrate function to find gyro drift and accelerometer bias accbias: -0.3 gyrobias: +0.15
void calibrate()
{
- for(int i=0; i<1000; i++) { // Read a thousand values
+ for(int i=0; i<100; i++) { // Read a thousand values
imu.readAccel(); // Read the Accelerometer
imu.readGyro(); // Read the gyro
accBias=accBias+(-1)*atan2(imu.ay,imu.az)*180/3.142-90; // Like this, 0 is upright, forward is negative, backward positive
gyroBias=gyroBias+imu.gx; // Add up all the gyro Biases
}
- accBias=accBias/1000; // Convert sum to average
- gyroBias=gyroBias/1000; // Convert sum to average
+ accBias=accBias/100; // Convert sum to average
+ gyroBias=gyroBias/100; // Convert sum to average
#ifdef DEBUG
pc.printf("accBias: %f gyroBias: %f\n",accBias, gyroBias); // Print biases to USB
#endif