ECE 4180 Spring 15
Robot that balances on two wheels
Fork of
AwkwardSegway
by 
Nico van Duijn
Diff: main.cpp
- Revision:
- 4:51ea148fc592
- Parent:
- 3:89e4ed1324bb
--- 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;
}
}