Nico van Duijn
Robot that balances on two wheels
Fork of
EpilepticSegway
by 
ECE 4180 Spring 15
Diff: main.cpp
- Revision:
- 3:89e4ed1324bb
- Parent:
- 2:89ada0b0b923
- Child:
- 4:51ea148fc592
--- a/main.cpp Tue Apr 28 02:18:55 2015 +0000
+++ b/main.cpp Wed Apr 29 20:10:33 2015 +0000
@@ -29,7 +29,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 DEBUG // Comment this out for final version
//////////////////////////////////////////////////////////////////
// I/O and object instatiation
@@ -41,22 +41,19 @@
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
-Ticker GUI;
-//Serial xbee(p13, p14); // Creates virtual serial for xbee
-Serial pc(USBTX,USBRX);// later only use xbee
-Timer t; //debugging only
-DigitalOut led1(LED1); // debug only
-DigitalOut led2(LED2); // debug only
-DigitalOut led3(LED3); // debug only
-typedef union _data { // maybe there is a better way?
+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
+
+typedef union _data { // Typedef so we can jump from chars to floats
float f;
char chars[4];
- int i; // added to receive integers
+ int i;
} 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
@@ -70,33 +67,32 @@
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
-myData bytes;
+float turnspeed=0; // Makes the robot turn
+myData bytes; // Used to convert received/sent chars to ints and floats
+
//////////////////////////////////////////////////////////////////
// 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
-void sendFloatAsBytes();
void updateGUI();
+void checkValues();
+
//////////////////////////////////////////////////////////////////
// Main function
int main()
{
-
- led1=0;//debug only
+ led1=0; // Turn led off
uint16_t status = imu.begin(); // Use the begin() function to initialize the LSM9DS0 library.
- pc.baud(115200);//pretty high, check if lower possible
- pc.attach(&callback); // Attach interrupt to serial communication
+ xbee.baud(115200); // Baudrate. Pretty high, check if lower possible
calibrate(); // Calibrate gyro and accelerometer
start.attach(&control, 0.01); // Looptime 10ms ca. 100Hz
- GUI.attach(&updateGUI, 0.5); // update GUI twice a second
+ GUI.attach(&updateGUI, 0.5); // Update GUI twice a second
- while(1) { // Stay in this loop forever
- led1=!led1;
- wait_ms(500); // blink twice a second
+ while(1) { // Stay in this loop forever
+ led1=!led1; // Blink led1 to make sure the loop is running
+ wait_ms(500); // Looptime 500ms
}
}
@@ -119,6 +115,7 @@
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
}
@@ -180,86 +177,85 @@
gyroBias=gyroBias/100; // Convert sum to average
}
-/////////////////////////////////////////////////////////////////
-// Callback function to change values/gains
-void callback()
+
+///////////////////////////////////////////////////////////////////
+// updateGUI(), triggered by ticker GUI ever 0.5s, sends data to xbee
+void updateGUI()
{
- led2=!led2; // Blink led2 so we know what's happening
- char buffer[24]; //create a buffer
- int i=0;
- while(pc.readable()) { // read the whole damn thing
- buffer[i]=pc.getc();
- i++;
- }
-// update kp
- bytes.chars[0]=buffer[0];
- bytes.chars[1]=buffer[1];
- bytes.chars[2]=buffer[2];
- bytes.chars[3]=buffer[3];
- kp=bytes.i;
-// update ki
- bytes.chars[0]=buffer[4];
- bytes.chars[1]=buffer[5];
- bytes.chars[2]=buffer[6];
- bytes.chars[3]=buffer[7];
- ki=bytes.i;
-// update kd
- bytes.chars[0]=buffer[8];
- bytes.chars[1]=buffer[9];
- bytes.chars[2]=buffer[10];
- bytes.chars[3]=buffer[11];
- kd=bytes.i;
-// update OVERALL_SCALAR
- bytes.chars[0]=buffer[12];
- bytes.chars[1]=buffer[13];
- bytes.chars[2]=buffer[14];
- bytes.chars[3]=buffer[15];
- OVERALL_SCALAR=bytes.i;
-// update turnspeed
- bytes.chars[0]=buffer[16];
- bytes.chars[1]=buffer[17];
- bytes.chars[2]=buffer[18];
- bytes.chars[3]=buffer[19];
- turnspeed=bytes.i;
-// update desiredAngle
- bytes.chars[0]=buffer[20];
- bytes.chars[1]=buffer[21];
- bytes.chars[2]=buffer[22];
- bytes.chars[3]=buffer[23];
- desiredAngle=bytes.i;
-
-
- // Update kp min: 0 max: 3000
- // Update kd min: 0 max: 3000
- // Update ki min: 0 max: 3000
- // Update OVERALL_SCALAR min: 0 max: 3000
- // Update turnspeed min: -0.5 max: 0.5
- // Update desiredAngle min: -3 max: 3
- // Send pAngle BETTER: kp*pAngle
- // Send dAngle BETTER: kd*dAngle
- // Send iAngle BETTER: kd*dAngle
+ 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]);
+ bytes.f = gyroBias; // Send gyroBias
+ xbee.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[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]);
+ 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]);
+ 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
}
//////////////////////////////////////////////////////////////////
-// maybe get rid of this function and put it into updateGUI to run faster
-void sendFloatAsBytes()
-{
- pc.printf("%c%c%c%c", bytes.chars[0], bytes.chars[1], bytes.chars[2], bytes.chars[3]);
-}
-
-///////////////////////////////////////////////////////////////////
-// triggered by ticker GUI ever 0.5s, sends data to PC
-void updateGUI()
+// checkValues() updates globals received from xbee
+void checkValues()
{
- led3=!led3; // blink so we know what's going on
- pc.putc('6'); // 6 is for pTerm
- bytes.f=pAngle; // later kp*pAngle
- sendFloatAsBytes();
- wait_us(10); // waiting a little bit is key
- pc.putc('7');
- bytes.f=iAngle;
- sendFloatAsBytes();
- wait_us(10);
- pc.putc('8');
- bytes.f=dAngle;
- sendFloatAsBytes();
+ 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
+ }
+
+ 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];
+ 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];
+ 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];
+ 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;
+ 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];
+ 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];
+ turnspeed=bytes.f;
+ }
+ }
+
}
\ No newline at end of file