Diff: motion.cpp

Revision:

13:57ea4e520dbd

Parent:

12:2981367c63a0

Parent:

11:ea2112ae3c4a

Child:

14:24f994dc2770

--- a/motion.cpp	Sat Apr 28 18:26:26 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,130 +0,0 @@
-#include "motion.h"
-#include "geometryfuncs.h"
-
-Motion::Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin):
-    thr_motion(mtwrapper,this,osPriorityNormal,1024),
-    motors(motorsin),
-    ai(aiin),
-    kalman(kalmanin) { }
-
-// motion control thread ------------------------
-void Motion::motion_thread() {
-    motors.resetEncoders();
-    motors.stop();
-    ai.thr_AI.signal_set(0x01);
-
-    //PID declare
-    PID PIDControllerMotorTheta2(FWD_MOVE_P, 0, 0, MOTION_UPDATE_PERIOD/1000.0f);     //Going forward
-    PID PIDControllerMotorTheta(SPIN_MOVE_P, 0, 0, MOTION_UPDATE_PERIOD/1000.0f);    //Spinning on the spot
-
-    //PID Initialisation
-    PIDControllerMotorTheta2.setMode(MANUAL_MODE);
-    PIDControllerMotorTheta.setMode(MANUAL_MODE);
-
-    PIDControllerMotorTheta2.setBias(0);
-    PIDControllerMotorTheta.setBias(0);
-
-    PIDControllerMotorTheta2.setOutputLimits(-1, 1);
-    PIDControllerMotorTheta.setOutputLimits(-1, 1);
-
-    PIDControllerMotorTheta2.setInputLimits(-PI, PI);
-    PIDControllerMotorTheta.setInputLimits(-PI, PI);
-
-    PIDControllerMotorTheta.setSetPoint(0);
-    PIDControllerMotorTheta2.setSetPoint(0);
-
-    float currX, currY,currTheta;
-    float speedL,speedR;
-    float diffDir;
-    
-    while (1) {
-        if (ai.flag_terminate) {
-            terminate();
-        }
-
-        // get kalman localization estimate ------------------------
-        kalman.statelock.lock();
-        currX = kalman.X(0)*1000.0f;
-        currY = kalman.X(1)*1000.0f;
-        currTheta = kalman.X(2);
-        kalman.statelock.unlock();
-        
-       
-       /* 
-        if (pc.readable() == 1) {
-            pc.scanf("%f", &cmd);
-            //Tune PID referece
-               PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
-        }
-        */  
-
-        // check if target reached ----------------------------------
-        if (  ( abs(currX - ai.target.x) < POSITION_TOR )
-                &&( abs(currY - ai.target.y) < POSITION_TOR )
-           ) {
-
-            diffDir = rectifyAng(currTheta - ai.target.theta);
-            //diffSpeed = diffDir / PI;
-
-            PIDControllerMotorTheta.setProcessValue(diffDir);
-
-            if (abs(diffDir) > ANGLE_TOR) {
-            
-                float tempPidVar = PIDControllerMotorTheta.compute();
-                motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));
-
-            } else {
-                motors.stop();
-                Thread::wait(4000);
-                ai.thr_AI.signal_set(0x01);
-            }
-        }
-
-        // adjust motion to reach target ----------------------------
-        else {
-
-            // calc direction to target
-            float targetDir = atan2(ai.target.y - currY, ai.target.x - currX);
-            if (!ai.target.facing) targetDir =  targetDir - PI;
-
-            //Angle differene in -PI to PI
-            diffDir = rectifyAng(currTheta - targetDir);
-
-            //Set PID process variable
-            PIDControllerMotorTheta.setProcessValue(diffDir);
-            PIDControllerMotorTheta2.setProcessValue(diffDir);
-
-            //if diffDIr is neg, spin right
-            //if diffDir is pos, spin left
-
-            if (abs(diffDir) > ANGLE_TOR*4) {   //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);
-
-            } else {
-             
-             float tempPidVar = PIDControllerMotorTheta2.compute();
-             //pc.printf("turn,%f\n",diffDir);
-                // calculte the motor speeds
-                if (tempPidVar >= 0) {
-                    //turn left
-                    speedL = (1-abs(tempPidVar))*MOVE_SPEED;
-                    speedR = MOVE_SPEED;
-
-                } else {
-                    //turn right
-                    speedR = (1-abs(tempPidVar))*MOVE_SPEED;
-                    speedL = MOVE_SPEED;
-                }
-                if (ai.target.facing) motors.setSpeed( int(speedL),  int(speedR));
-                else motors.setSpeed( -int(speedR),  -int(speedL));
-                
-            }
-        }
-
-        // wait
-        Thread::wait(MOTION_UPDATE_PERIOD);
-    }
-}
\ No newline at end of file