Diff: motion.cpp

Revision:

11:ea2112ae3c4a

Parent:

10:294b9adbc9d3

--- a/motion.cpp	Sat Apr 28 18:10:55 2012 +0000
+++ b/motion.cpp	Sat Apr 28 18:21:16 2012 +0000
@@ -1,6 +1,7 @@
 #include "motion.h"
 #include "geometryfuncs.h"
 
+
 Motion::Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin):
     motors(motorsin),
     ai(aiin),
@@ -49,13 +50,14 @@
         kalman.statelock.unlock();
         
        
-       /* 
+       
         if (pc.readable() == 1) {
+        float cmd;
             pc.scanf("%f", &cmd);
             //Tune PID referece
-               PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
+               PIDControllerMotorTheta.setTunings(cmd, 0, 0);
         }
-        */  
+        
 
         // check if target reached ----------------------------------
         if (  ( abs(currX - ai.target.x) < POSITION_TOR )
@@ -96,16 +98,16 @@
             //if diffDIr is neg, spin right
             //if diffDir is pos, spin left
 
-            if (abs(diffDir) > ANGLE_TOR*4) {   //roughly 32 degrees
+            if (abs(diffDir) > 0) {   //roughly 32 degrees
                 //ANGLE_TOR*4
                 float tempPidVar = PIDControllerMotorTheta.compute();
                 motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));
-                // pc.printf("spin,%f\n",diffDir);
+                pc.printf("spin,%f\n",diffDir);
 
             } else {
-             
+             /*
              float tempPidVar = PIDControllerMotorTheta2.compute();
-             //pc.printf("turn,%f\n",diffDir);
+             pc.printf("turn,%f\n",diffDir);
                 // calculte the motor speeds
                 if (tempPidVar >= 0) {
                     //turn left
@@ -119,7 +121,7 @@
                 }
                 if (ai.target.facing) motors.setSpeed( int(speedL),  int(speedR));
                 else motors.setSpeed( -int(speedR),  -int(speedL));
-                
+                */
             }
         }