Diff: main.cpp

Revision:

17:034b50f49f46

Parent:

16:e9945e3b4712

Child:

18:4ee32b922251

diff -r e9945e3b4712 -r 034b50f49f46 main.cpp
--- a/main.cpp	Wed Oct 07 11:45:27 2015 +0000
+++ b/main.cpp	Wed Oct 07 12:19:30 2015 +0000
@@ -29,7 +29,7 @@
     rightMotor=0;
 }
 int main() {
-    
+    velocityTimer.start();
     // initialize (defined in inits.h)
     initMotors();
     initPIDs();
@@ -89,17 +89,19 @@
                 toRightAngle = atan(toY/(l-toX))*180/M_PI;
                 
                 // calculate how much the angles should change in this timestep
-                leftDeltaAngle=toLeftAngle-leftAngle;
-                rightDeltaAngle=toRightAngle-rightAngle;
+                leftDeltaAngle=(toLeftAngle-leftAngle)/10;
+                rightDeltaAngle=(toRightAngle-rightAngle)/10;
                 
                 // calculate the neccesairy velocities to make these angles happen.
                 leftRequest=(leftDeltaAngle/calcRATE);
                 rightRequest=(rightDeltaAngle/calcRATE);
             }
-            pc.printf("leftrequest: %f, rightrequest %f, leftAngle: %f, rightAngle: %f \n\r",leftRequest,rightRequest,leftAngle,rightAngle);
+            //pc.printf("leftrequest: %f, rightrequest %f, leftAngle: %f, rightAngle: %f \n\r",leftRequest,rightRequest,leftAngle,rightAngle);
+            didCalc=true;
             calcFlag=false;
         }
-        else if (systemOn == true && goFlag == true){
+        else if (systemOn == true && goFlag == true && didCalc==false){
+            
             // set the PID controller to go with that speed.
             leftController.setSetPoint(leftRequest);
             rightController.setSetPoint(rightRequest);
@@ -109,14 +111,19 @@
             // *******************
             // Velocity calculation
             // left
+            thistime=velocityTimer.read();
+            looptime=thistime-lasttime;
+            lasttime=thistime;
+            //pc.printf("looptime: %f",looptime);
+            
             leftPulses = leftQei.getPulses();
-            leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE;
+            leftVelocity = ((float)(leftPulses - leftPrevPulses))/ looptime;
             leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000
             leftPrevPulses = leftPulses;
             
             // right
             rightPulses = rightQei.getPulses();
-            rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE;
+            rightVelocity = ((float)(rightPulses - rightPrevPulses))/ looptime;
             rightVelocity = rightVelocity/30000; // scale to 0 - 1, max velocity = 30000
             rightPrevPulses = rightPulses;
             
@@ -157,6 +164,13 @@
         
             goFlag=false;
         }
+        
+        if (systemOn == true && goFlag == true && didCalc == true){
+            //leftPrevPulses = leftQei.getPulses();
+            //rightPrevPulses = rightQei.getPulses();
+            didCalc=false;   
+            //goFlag=false;
+        }
     }
 
     //Stop motors.