Nico van Duijn
Robot that balances on two wheels
Fork of
EpilepticSegway
by 
ECE 4180 Spring 15
Diff: main.cpp
- Revision:
- 0:4b50c71291e9
- Child:
- 1:a079ae75a86c
diff -r 000000000000 -r 4b50c71291e9 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Apr 22 16:24:21 2015 +0000
@@ -0,0 +1,178 @@
+/*////////////////////////////////////////////////////////////////
+ECE 4180 Final Project
+Balancing Robot
+
+Nico van Duijn
+Samer Mabrouk
+Anthony Agnone
+Jay Danner
+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.
+The robot receives steering commands via the XBee module which is
+interfaced with from a C# GUI that runs on a desktop computer.
+
+*/////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////
+// Includes
+#include "mbed.h" // mbed library
+#include "LSM9DS0.h" // 9axis IMU
+#include "math.h" // We need to be able to do trig
+
+//////////////////////////////////////////////////////////////////
+// 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
+
+//////////////////////////////////////////////////////////////////
+// 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(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
+Ticker start; // Initialize ticker to call control function
+Serial xbee(p13, p14); // Creates virtual serial for xbee
+
+
+//////////////////////////////////////////////////////////////////
+// Globals
+float kp = 93; // 145 Proportional gain kU 400-500
+float kd = 100; // Derivative gain
+float ki = 1100; // 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
+float gyroAngle = 0; // This variable holds the amount the angle has changed
+float speed = 0; // Variable for motor speed
+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; // positive turns
+
+//////////////////////////////////////////////////////////////////
+// Function Prototypes
+void drive(float); // Function declaration for driving the motors
+void calibrate(); // Function to calibrate gyro and accelerometer
+void control(); // Function implementing PID control
+void callback(); // Interrupt triggered function for serial communication
+
+//////////////////////////////////////////////////////////////////
+// Main function
+int main()
+{
+ uint16_t status = imu.begin(); // Use the begin() function to initialize the LSM9DS0 library.
+
+ xbee.attach(&callback); // Attach interrupt to serial communication
+ calibrate(); // Calibrate gyro and accelerometer
+ start.attach(&control, 0.01); // Looptime 10ms ca. 100Hz
+ while(1) { // Stay in this loop forever
+
+ }
+}
+
+/////////////////////////////////////////////////////////////////
+// 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-desiredAngle; // Complementary filter yields best value for current angle
+ 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((abs(pAngle-desiredAngle)>=0.55)&&abs(pAngle-desiredAngle)<=15) { // If it is tilted enough, but not too much ||abs(imu.gx)>8
+ 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
+
+}
+
+//////////////////////////////////////////////////////////////////
+// Drive function
+void drive(float speed)
+{
+ int direction=0; // Variable to hold direction we want to drive
+ if (speed>0)direction=1; // Positive speed indicates forward
+ if (speed<0)direction=2; // Negative speed indicates backwards
+ if(speed==0)direction=0; // Zero speed indicates stopping
+ switch( direction) { // Depending on what direction was passed
+ case 0: // Stop case
+ motorSpeedLeft=0; // Set the DigitalOuts to stop the motors
+ motorSpeedRight=0;
+ motorDirLeft=0;
+ NmotorDirLeft=0;
+ motorDirRight=0;
+ NmotorDirRight=0;
+ break;
+ case 1: // Forward case
+ motorSpeedLeft=speed-turnspeed; // Set the DigitalOuts to run the motors forward
+ motorSpeedRight=speed+turnspeed;
+ motorDirLeft=1;
+ NmotorDirLeft=0;
+ motorDirRight=1;
+ NmotorDirRight=0;
+ break;
+ case 2: // Backwards
+ motorSpeedLeft=-speed+turnspeed; // Set the DigitalOuts to run the motors backward
+ motorSpeedRight=-speed-turnspeed;
+ motorDirLeft=0;
+ NmotorDirLeft=1;
+ motorDirRight=0;
+ NmotorDirRight=1;
+ break;
+ default: // Catch-all (Stop)
+ motorSpeedLeft=0; // Set the DigitalOuts to stop the motors
+ motorSpeedRight=0;
+ motorDirLeft=0;
+ NmotorDirLeft=0;
+ motorDirRight=0;
+ NmotorDirRight=0;
+ break;
+ }
+}
+
+//////////////////////////////////////////////////////////////////
+// Calibrate function to find gyro drift and accelerometer bias accbias: -0.3 gyrobias: +0.15
+void calibrate()
+{
+ 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/100; // Convert sum to average
+ gyroBias=gyroBias/100; // Convert sum to average
+}
+
+/////////////////////////////////////////////////////////////////
+// Callback function to change values/gains
+void callback()
+{
+ // Update kp
+ // Update kd
+ // Update ki
+ // Update OVERALL_SCALAR
+ // Update turnspeed
+ // Update desiredAngle
+ // Send pAngle
+ // Send dAngle
+ // Send iAngle
+}
\ No newline at end of file