Diff: main.cpp

Revision:

1:a079ae75a86c

Parent:

0:4b50c71291e9

Child:

2:89ada0b0b923

diff -r 4b50c71291e9 -r a079ae75a86c main.cpp
--- a/main.cpp	Wed Apr 22 16:24:21 2015 +0000
+++ b/main.cpp	Wed Apr 22 19:14:42 2015 +0000
@@ -46,10 +46,10 @@
 
 //////////////////////////////////////////////////////////////////
 // Globals
-float kp = 93;                                                  // 145 Proportional gain kU 400-500
-float kd = 100;                                                 // Derivative gain
-float ki = 1100;                                                // Integrative gain
-float OVERALL_SCALAR = 170;                                     // Overall scalar that speed is divided by
+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
 float accBias = 0;                                              // Accelerometer Bias
 float gyroBias = 0;                                             // Gyro Bias
 float accAngle = 0;                                             // Global to hold angle measured by Accelerometer
@@ -95,7 +95,7 @@
     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.55)&&abs(pAngle-desiredAngle)<=15) {  // If it is tilted enough, but not too much ||abs(imu.gx)>8
+    if((abs(pAngle-desiredAngle)>=0.6)&&abs(pAngle-desiredAngle)<=15) {  // If it is tilted enough, but not too much ||abs(imu.gx)>8
         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
@@ -122,7 +122,7 @@
             NmotorDirRight=0;
             break;
         case 1:                                                 // Forward case
-            motorSpeedLeft=speed-turnspeed;                               // Set the DigitalOuts to run the motors forward
+            motorSpeedLeft=speed-turnspeed;                     // Set the DigitalOuts to run the motors forward
             motorSpeedRight=speed+turnspeed;
             motorDirLeft=1;
             NmotorDirLeft=0;
@@ -130,7 +130,7 @@
             NmotorDirRight=0;
             break;
         case 2:                                                 // Backwards
-            motorSpeedLeft=-speed+turnspeed;                              // Set the DigitalOuts to run the motors backward
+            motorSpeedLeft=-speed+turnspeed;                    // Set the DigitalOuts to run the motors backward
             motorSpeedRight=-speed-turnspeed;
             motorDirLeft=0;
             NmotorDirLeft=1;
@@ -152,27 +152,27 @@
 // Calibrate function to find gyro drift and accelerometer bias accbias: -0.3 gyrobias: +0.15
 void calibrate()
 {
-    for(int i=0; i<100; i++) {                                 // Read a thousand values
+    for(int i=0; i<100; 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/100;                                       // Convert sum to average
-    gyroBias=gyroBias/100;                                     // Convert sum to average
+    accBias=accBias/100;                                        // Convert sum to average
+    gyroBias=gyroBias/100;                                      // Convert sum to average
 }
 
 /////////////////////////////////////////////////////////////////
 // Callback function to change values/gains
 void callback()
 {
-    // Update kp
-    // Update kd
-    // Update ki
-    // Update OVERALL_SCALAR
-    // Update turnspeed
-    // Update desiredAngle
-    // Send pAngle
+    // 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
 }
\ No newline at end of file