Diff: main.cpp

Revision:

19:3ca10fe26131

Parent:

18:4ee32b922251

--- a/main.cpp	Wed Oct 07 13:19:25 2015 +0000
+++ b/main.cpp	Mon Oct 12 08:13:29 2015 +0000
@@ -52,7 +52,9 @@
             else {
                 request = -request_neg;
             }
-                        
+            
+            request=request*maxspeed; // turn [-1, 1] into [-max cm/s, max cm/s]
+                       
             // calculate required rotational velocity from the requested horizontal velocity
             // first get the current position from the motor encoders
             // make them start at 45 degree.
@@ -79,21 +81,22 @@
                     //return 0;
                 //}
                 // 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=currentX+request/200*calcRATE; // should be request*RATE, 200 is a magical number to make request work in cm/s
+                //toY=currentY+0*calcRATE;
                 toY=currentY+0*calcRATE;
-            
+                
                 toLeftAngle = atan(toY/toX)*180/M_PI;
                 toRightAngle = atan(toY/(l-toX))*180/M_PI;
                 
                 // calculate how much the angles should change in this timestep
-                leftDeltaAngle=(toLeftAngle-leftAngle)/10;
-                rightDeltaAngle=(toRightAngle-rightAngle)/10;
+                leftDeltaAngle=(toLeftAngle-leftAngle);
+                rightDeltaAngle=(toRightAngle-rightAngle);
                 
                 // 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, curX: %f, curY: %f \n\r",leftRequest,rightRequest,currentX,currentY);
             calcFlag=false;
         }
         else if (systemOn == true && goFlag == true){