Diff: main.cpp

Revision:

2:89ada0b0b923

Parent:

1:a079ae75a86c

Child:

3:89e4ed1324bb

diff -r a079ae75a86c -r 89ada0b0b923 main.cpp
--- a/main.cpp	Wed Apr 22 19:14:42 2015 +0000
+++ b/main.cpp	Tue Apr 28 02:18:55 2015 +0000
@@ -41,15 +41,26 @@
 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
-                                    
+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?
+    float f;
+    char chars[4];
+    int i; // added to receive integers
+} myData;
+
 
 //////////////////////////////////////////////////////////////////
-// Globals
-float kp = 200; //98                                                 // 145 Proportional gain kU 400-500
-float kd = 200;  //200                                               // Derivative gain
-float ki = 1500;   //985                                             // Integrative gain
-float OVERALL_SCALAR = 200;   //160                                  // Overall scalar that speed is divided by
+// Globals // double battery, lab floor: 67/100/600/160 cutoff ..35
+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
 float accBias = 0;                                              // Accelerometer Bias
 float gyroBias = 0;                                             // Gyro Bias
 float accAngle = 0;                                             // Global to hold angle measured by Accelerometer
@@ -59,7 +70,8 @@
 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 
+float turnspeed=0;                                              // positive turns
+myData bytes;
 
 //////////////////////////////////////////////////////////////////
 // Function Prototypes
@@ -67,18 +79,24 @@
 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();
 //////////////////////////////////////////////////////////////////
 // Main function
 int main()
 {
+
+    led1=0;//debug only
     uint16_t status = imu.begin();                              // Use the begin() function to initialize the LSM9DS0 library.
-
-    xbee.attach(&callback);                                       // Attach interrupt to serial communication
+    pc.baud(115200);//pretty high, check if lower possible
+    pc.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
+    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
     }
 }
 
@@ -95,13 +113,13 @@
     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.6)&&abs(pAngle-desiredAngle)<=15) {  // If it is tilted enough, but not too much ||abs(imu.gx)>8
+    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(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
-    
+
 }
 
 //////////////////////////////////////////////////////////////////
@@ -166,13 +184,82 @@
 // Callback function to change values/gains
 void callback()
 {
+    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 
-    // Send dAngle
-    // Send iAngle
+    // Send pAngle BETTER: kp*pAngle
+    // Send dAngle BETTER: kd*dAngle
+    // Send iAngle BETTER: kd*dAngle
+}
+//////////////////////////////////////////////////////////////////
+// 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()
+{
+    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();
 }
\ No newline at end of file