Diff: Eurobot_shared/Motion/motion.cpp

Revision:

16:b3dd4e0b3100

Parent:

15:acae5c0e9ca8

Child:

17:bafcef1c3579

--- a/Eurobot_shared/Motion/motion.cpp	Sat Apr 28 22:21:03 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,166 +0,0 @@
-#include "motion.h"
-#include "geometryfuncs.h"
-#include "system.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();
-    Thread::wait(1500);
-    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;
-    int atTargetFlag = 0;
-
-    while (1) {
-        kalman.statelock.lock();
-        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();
-        
-        AI::Target loctarget = ai.gettarget();
-
-        /*
-        //PID Tuning Code
-        if (pc.readable() == 1) {
-            float cmd;
-            pc.scanf("%f", &cmd);
-            //Tune PID referece
-            PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
-        }
-        */
-        
-        // check if target reached ----------------------------------
-        if (atTargetFlag || hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
-                
-            atTargetFlag = loctarget.reached;
-            OLED4 = 1;
-            
-            diffDir = rectifyAng(currTheta - loctarget.theta);
-            //diffSpeed = diffDir / PI;
-
-            PIDControllerMotorTheta.setProcessValue(diffDir);
-            float tempPidVar = PIDControllerMotorTheta.compute();
-            motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));
-
-            if (abs(diffDir) < ANGLE_TOR) {
-
-                if (!loctarget.reached) {
-                    static int counter = 10;
-                    if (counter-- == 0){
-                    counter = 10;
-                    ai.target.reached = true;
-                    ai.thr_AI.signal_set(0x01);
-                    
-                    }
-                }
-            }
-        }
-
-        // adjust motion to reach target ----------------------------
-        else {
-        
-            OLED4 = 0;
-
-            // calc direction to target
-            float targetDir = atan2(loctarget.y - currY, loctarget.x - currX);
-            if (!loctarget.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();
-                float MoveSpeedLimiter = 1;
-                //pc.printf("turn,%f\n",diffDir);
-
-                float distanceToX = (float)abs(currX - loctarget.x);
-                float distanceToY = (float)abs(currY - loctarget.y);
-
-                float distanceToTarget = hypot(distanceToX, distanceToY);
-
-                if ((distanceToTarget < 400) && (distanceToTarget > 100)) {
-                    MoveSpeedLimiter = (distanceToTarget)/400;
-                }
-                else if((distanceToTarget < 400) && (distanceToTarget > 100)) {
-                    MoveSpeedLimiter  = 0.25;
-                }
-                
-                
-                
-
-                // calculte the motor speeds
-                if (tempPidVar >= 0) {
-                    //turn left
-                    speedL = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter;
-                    speedR = MOVE_SPEED*MoveSpeedLimiter;
-
-                } else {
-                    //turn right
-                    speedR = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter;
-                    speedL = MOVE_SPEED*MoveSpeedLimiter;
-                }
-
-
-
-
-                if (loctarget.facing) motors.setSpeed( int(speedL),  int(speedR));
-                else motors.setSpeed( -int(speedR),  -int(speedL));
-
-            }
-        }
-        kalman.statelock.unlock();
-        // wait
-        Thread::wait(MOTION_UPDATE_PERIOD);
-    }
-}
\ No newline at end of file