Diff: main.cpp

Revision:

18:4ee32b922251

Parent:

17:034b50f49f46

Child:

19:3ca10fe26131

--- a/main.cpp	Wed Oct 07 12:19:30 2015 +0000
+++ b/main.cpp	Wed Oct 07 13:19:25 2015 +0000
@@ -4,7 +4,6 @@
 #include "QEI.h"
 #include "HIDScope.h"
 #include <math.h>
-//#include "Point.cpp"
 #include "inits.h" // all globals, pin and variable initialization 
 
 void setGoFlag(){
@@ -53,10 +52,10 @@
             else {
                 request = -request_neg;
             }
-            
+                        
             // calculate required rotational velocity from the requested horizontal velocity
             // first get the current position from the motor encoders
-            // make them start at 45 degrees
+            // make them start at 45 degree.
             leftAngle=(leftQei.getPulses()/round)*360+45;
             rightAngle=(rightQei.getPulses()/round)*360+45;
             
@@ -81,9 +80,7 @@
                 //}
                 // calculate the position to go to according the the current position + the distance that should be covered in this timestep
                 toX=currentX+request*calcRATE; // should be request*RATE
-                //toX=5;
-                //toY=pCurrent.posY+0*RATE;
-                toY=currentY;
+                toY=currentY+0*calcRATE;
             
                 toLeftAngle = atan(toY/toX)*180/M_PI;
                 toRightAngle = atan(toY/(l-toX))*180/M_PI;
@@ -97,10 +94,9 @@
                 rightRequest=(rightDeltaAngle/calcRATE);
             }
             //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 && didCalc==false){
+        else if (systemOn == true && goFlag == true){
             
             // set the PID controller to go with that speed.
             leftController.setSetPoint(leftRequest);
@@ -154,9 +150,9 @@
             }
             
             // User feedback
-            scope.set(0, rightRequest);
+            scope.set(0, rightRequest*10);
             scope.set(1, rightPwmDuty);
-            scope.set(2, rightVelocity);
+            scope.set(2, rightVelocity*10);
             scope.set(3, leftAngle);
             scope.set(4, rightAngle);
             scope.send();
@@ -164,13 +160,6 @@
         
             goFlag=false;
         }
-        
-        if (systemOn == true && goFlag == true && didCalc == true){
-            //leftPrevPulses = leftQei.getPulses();
-            //rightPrevPulses = rightQei.getPulses();
-            didCalc=false;   
-            //goFlag=false;
-        }
     }
 
     //Stop motors.