ECE 4180 Spring 15
/
AwkwardSegway
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.
Fork of
AwkwardSegway
by 
Nico van Duijn
Revision 4:51ea148fc592, committed 2015-04-30
- Comitter:
- nicovanduijn
- Date:
- Thu Apr 30 22:32:05 2015 +0000
- Parent:
- 3:89e4ed1324bb
- Commit message:
- Final Version with extensive comments
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 89e4ed1324bb -r 51ea148fc592 main.cpp
--- a/main.cpp Wed Apr 29 20:10:33 2015 +0000
+++ b/main.cpp Thu Apr 30 22:32:05 2015 +0000
@@ -1,4 +1,4 @@
-/*////////////////////////////////////////////////////////////////
+/*//////////////////////////////////////////////////////////////
ECE 4180 Final Project
Balancing Robot
@@ -16,7 +16,6 @@
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.
-
*/////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
@@ -29,7 +28,7 @@
// 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 LOOPTIME 0.01 // Determines the looptime. 100Hz is pretty stable
//////////////////////////////////////////////////////////////////
// I/O and object instatiation
@@ -43,21 +42,19 @@
Ticker start; // Initialize ticker to call control function
Ticker GUI; // Ticker that calls the updateGUI
DigitalOut led1(LED1); // Use LED1 to provide some runtime info
-Serial xbee(p13,p14); // Create serial port for Xbee
+Serial xbee(p13,p14); // Create serial port for Xbee
typedef union _data { // Typedef so we can jump from chars to floats
float f;
- char chars[4];
- int i;
+ char c[4];
} myData;
//////////////////////////////////////////////////////////////////
-// Globals // double battery, lab floor: 67/100/600/160 cutoff ..35
-// positive turns
-float kp = 59; //98 // 145 Proportional gain kU 400-500
-float kd = 105; //200 // Derivative gain
-float ki = 670; //985 // Integrative gain
-float OVERALL_SCALAR = 160; //160 // Overall scalar that speed is divided by
+// Globals
+float kp = 59; // Proportional gain
+float kd = 105; // Derivative gain
+float ki = 670; // Integrative gain
+float overallScalar = 160; // 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
@@ -69,6 +66,7 @@
float desiredAngle=0; // Setpoint. Set unequal zero to drive
float turnspeed=0; // Makes the robot turn
myData bytes; // Used to convert received/sent chars to ints and floats
+float cutoff=0.2; // Noise-level cutoff
//////////////////////////////////////////////////////////////////
@@ -76,8 +74,8 @@
void drive(float); // Function declaration for driving the motors
void calibrate(); // Function to calibrate gyro and accelerometer
void control(); // Function implementing PID control
-void updateGUI();
-void checkValues();
+void updateGUI(); // Function that sends values to the GUI
+void checkValues(); // Function that receives controls from GUI
//////////////////////////////////////////////////////////////////
// Main function
@@ -85,9 +83,9 @@
{
led1=0; // Turn led off
uint16_t status = imu.begin(); // Use the begin() function to initialize the LSM9DS0 library.
- xbee.baud(115200); // Baudrate. Pretty high, check if lower possible
+ xbee.baud(115200); // Baudrate
calibrate(); // Calibrate gyro and accelerometer
- start.attach(&control, 0.01); // Looptime 10ms ca. 100Hz
+ start.attach(&control, LOOPTIME); // Looptime 10ms ca. 100Hz
GUI.attach(&updateGUI, 0.5); // Update GUI twice a second
while(1) { // Stay in this loop forever
@@ -104,23 +102,23 @@
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
+ gyroAngle=-(imu.gx-gyroBias)*LOOPTIME; // 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!)
+ iAngle+=(pAngle*LOOPTIME); // Integrate the angle (multiply by timestep to get dt!)
- if((abs(pAngle-desiredAngle)>=0.4)&&abs(pAngle-desiredAngle)<=15) { // If it is tilted enough, but not too much ||abs(imu.gx)>10
- speed=-(ki*iAngle+kd*dAngle+kp*pAngle)/OVERALL_SCALAR; // drive to correct
+ if((abs(pAngle-desiredAngle)>=cutoff)&&abs(pAngle-desiredAngle)<=15) { // If it is tilted enough, but not too much
+ speed=-(ki*iAngle+kd*dAngle+kp*pAngle)/overallScalar; // 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
- checkValues(); // Checks if we need to update some values
+ checkValues(); // Checks if we need to update some values
}
//////////////////////////////////////////////////////////////////
-// Drive function
+// drive() function to drive the motors at the given speed
void drive(float speed)
{
int direction=0; // Variable to hold direction we want to drive
@@ -164,10 +162,12 @@
}
//////////////////////////////////////////////////////////////////
-// Calibrate function to find gyro drift and accelerometer bias accbias: -0.3 gyrobias: +0.15
+/* Calibrate()
+* function to find gyro drift and accelerometer bias
+*/
void calibrate()
{
- for(int i=0; i<100; i++) { // Read a thousand values
+ for(int i=0; i<100; i++) { // Read one hundred 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
@@ -182,24 +182,24 @@
// updateGUI(), triggered by ticker GUI ever 0.5s, sends data to xbee
void updateGUI()
{
- xbee.putc('*'); // Send data validity value
+ xbee.putc('*'); // Send data validity value
bytes.f = accBias; // Send accBias
- xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
+ xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
bytes.f = gyroBias; // Send gyroBias
- xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
+ xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
bytes.f = pAngle; // Send P Angle
- xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
+ xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
bytes.f = imu.gx; // Send current angular velocity
- xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
+ xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
bytes.f = turnspeed; // Send turn speed
- xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
- bytes.f = pAngle * kp; // Send P Value
- xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
- bytes.f = iAngle * ki; // Send I Value
- xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
- bytes.f = dAngle * kd; // Send D Value
- xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
- xbee.putc('\n'); // Send delimiting character
+ xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
+ bytes.f = (pAngle * kp) / overallScalar; // Send P Value
+ xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
+ bytes.f = (iAngle * ki) / overallScalar; // Send I Value
+ xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
+ bytes.f = (dAngle * kd) / overallScalar; // Send D Value
+ xbee.printf("%c%c%c%c", bytes.c[0], bytes.c[1], bytes.c[2], bytes.c[3]);
+ xbee.putc('\n'); // Send delimiting character
}
//////////////////////////////////////////////////////////////////
// checkValues() updates globals received from xbee
@@ -207,53 +207,52 @@
{
int i=0; // Integer needed for looping through input buffer
char buffer[6]; // Buffer to hold all the received data
-
- while(xbee.readable()) { // As long as there is stuff in the buffer
- buffer[i++]=xbee.getc(); // Read from serial
+ while(xbee.readable()) { // As long as there is stuff in the buffer
+ buffer[i++]=xbee.getc(); // Read from serial
}
if(buffer[0]== '*') { // Check for 'start' character
switch(buffer[1]) { // Switch depending on what value we update
case '1': // Updating kp
- bytes.chars[0] = buffer[2];
- bytes.chars[1] = buffer[3];
- bytes.chars[2] = buffer[4];
- bytes.chars[3] = buffer[5];
+ bytes.c[0] = buffer[2];
+ bytes.c[1] = buffer[3];
+ bytes.c[2] = buffer[4];
+ bytes.c[3] = buffer[5];
kp=bytes.f;
break;
case '2': // Updating kd
- bytes.chars[0] = buffer[2];
- bytes.chars[1] = buffer[3];
- bytes.chars[2] = buffer[4];
- bytes.chars[3] = buffer[5];
+ bytes.c[0] = buffer[2];
+ bytes.c[1] = buffer[3];
+ bytes.c[2] = buffer[4];
+ bytes.c[3] = buffer[5];
kd=bytes.f;
break;
case '3': // Updating ki
- bytes.chars[0] = buffer[2];
- bytes.chars[1] = buffer[3];
- bytes.chars[2] = buffer[4];
- bytes.chars[3] = buffer[5];
+ bytes.c[0] = buffer[2];
+ bytes.c[1] = buffer[3];
+ bytes.c[2] = buffer[4];
+ bytes.c[3] = buffer[5];
ki=bytes.f;
break;
- case '4': // Updating OVERALL_SCALAR
- bytes.chars[0] = buffer[2];
- bytes.chars[1] = buffer[3];
- bytes.chars[2] = buffer[4];
- bytes.chars[3] = buffer[5];
- OVERALL_SCALAR=bytes.f;
+ case '4': // Updating overallScalar
+ bytes.c[0] = buffer[2];
+ bytes.c[1] = buffer[3];
+ bytes.c[2] = buffer[4];
+ bytes.c[3] = buffer[5];
+ overallScalar=bytes.f;
break;
case '5': // Updating desiredAngle
- bytes.chars[0] = buffer[2];
- bytes.chars[1] = buffer[3];
- bytes.chars[2] = buffer[4];
- bytes.chars[3] = buffer[5];
+ bytes.c[0] = buffer[2];
+ bytes.c[1] = buffer[3];
+ bytes.c[2] = buffer[4];
+ bytes.c[3] = buffer[5];
desiredAngle=bytes.f;
break;
- case '6': // Updating turnspeed
- bytes.chars[0] = buffer[2];
- bytes.chars[1] = buffer[3];
- bytes.chars[2] = buffer[4];
- bytes.chars[3] = buffer[5];
+ case '6': // Updating turnspeed
+ bytes.c[0] = buffer[2];
+ bytes.c[1] = buffer[3];
+ bytes.c[2] = buffer[4];
+ bytes.c[3] = buffer[5];
turnspeed=bytes.f;
}
}