ECE 4180 Spring 15
/
balance2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 0:67f5b4041c15
- Child:
- 1:bf71a7bd2d3e
diff -r 000000000000 -r 67f5b4041c15 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Mar 06 17:09:31 2015 +0000
@@ -0,0 +1,216 @@
+/*////////////////////////////////////////////////////////////////
+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
+*/////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////
+// 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(p23); // Digital pin for direction of left motor
+DigitalOut NmotorDirLeft(p24); // 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
+#ifdef DEBUG
+Serial pc(USBTX, USBRX); // Creates virtual Serial connection though USB
+#endif
+
+//////////////////////////////////////////////////////////////////
+// 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 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)
+
+//////////////////////////////////////////////////////////////////
+// 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
+#ifdef DEBUG
+void callback(); // Interrupt triggered function for serial communication
+#endif
+//////////////////////////////////////////////////////////////////
+// Main function
+int main()
+{
+ uint16_t status = imu.begin(); // Use the begin() function to initialize the LSM9DS0 library.
+#ifdef DEBUG
+ pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status); // Make sure communication is working
+ pc.printf("Should be 0x49D4\n\n"); // Check if we're talking to the right guy
+ pc.attach(&callback); // Attach interrupt to serial communication
+#endif
+ calibrate(); // Calibrate gyro and accelerometer
+ start.attach(&control, 0.01); // Looptime 10ms ca. 100Hz
+ while(1) { // Stay in this loop forever
+#ifdef DEBUG
+ pc.printf("%f\n",speed); // Print to USB the speed
+#endif
+ }
+}
+/////////////////////////////////////////////////////////////////
+// Control function, implements PID
+void control()
+{
+ 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
+ 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; // Set the DigitalOuts to run the motors forward
+ motorSpeedRight=speed;
+ motorDirLeft=1;
+ NmotorDirLeft=0;
+ motorDirRight=1;
+ NmotorDirRight=0;
+ break;
+ case 2: // Backwards
+ motorSpeedLeft=-speed; // Set the DigitalOuts to run the motors backward
+ motorSpeedRight=-speed;
+ 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
+void calibrate()
+{
+ for(int i=0; i<1000; 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
+#ifdef DEBUG
+ pc.printf("accBias: %f gyroBias: %f\n",accBias, gyroBias); // Print biases to USB
+#endif
+}
+
+#ifdef DEBUG
+/////////////////////////////////////////////////////////////////
+// Callback function to change values/gains
+void callback()
+{
+ char val; // Needed for Serial communication (need to be global?)
+ val = pc.getc(); // Reat the value from Serial
+ pc.printf("%c\n", val); // Print it back to the screen
+ if( val =='p') { // If character was a 'p'
+ pc.printf("enter kp \n"); // Adjust kp
+ val = pc.getc(); // Wait for kp value
+ if(val == 0x2b) { // If character is a plus sign
+ kp=kp+10; // Increase kp
+ } else if (val == 0x2d) { // If recieved character is the minus sign
+ kp=kp-10; // Decrease kp
+ } else {
+ kp = val - 48; // Cast char to float
+ }
+ pc.printf(" kp = %f \n",kp); // Print current kp value to screen
+ } else if( val == 'd') { // Adjust kd
+ pc.printf("enter kd \n"); // Wait for kd
+ val= pc.getc(); // Read value from serial
+ if(val == '+') { // If given plus sign
+ kd++; // Increase kd
+ } else if (val == '-') { // If given negative sign
+ kd--; // Decrease kd
+ } else { // If given some other ascii (a number?)
+ kd = val - 48; // Set derivative gain
+ }
+ pc.printf(" kd = %f \n",kd); // Print kd back to screen
+ } else if( val == 'i') { // If given i - integral gain
+ pc.printf("enter ki \n"); // Prompt on screen to ask for ascii
+ val= pc.getc(); // Get the input
+ if(val == '+') { // If given the plus sign
+ ki++; // Increase ki
+ } else if (val == '-') { // If given the minus sign
+ ki--; // Decrease ki
+ } else { // If given some other ascii
+ ki = val - 48; // Set ki to that number
+ }
+ pc.printf(" ki = %f \n",ki);
+ } else if( val == 'o') {
+ pc.printf("enter OVERALL_SCALAR \n");
+ val= pc.getc();
+ if(val == '+') {
+ OVERALL_SCALAR=OVERALL_SCALAR+1000;
+ } else if (val == '-') {
+ OVERALL_SCALAR=OVERALL_SCALAR-1000;
+ } else {
+ OVERALL_SCALAR=(val-48)*1000;;
+ }
+ pc.printf(" OVERALL_SCALAR = %f \n",OVERALL_SCALAR);
+ }
+}
+#endif
\ No newline at end of file