Diff: Eurobot_shared/Motion/motion.cpp

Revision:

17:bafcef1c3579

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Eurobot_shared/Motion/motion.cpp	Sun Apr 29 00:09:35 2012 +0000
@@ -0,0 +1,168 @@
+#include "motion.h"
+#include "geometryfuncs.h"
+#include "system.h"
+#include "PID.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.setSpeed(5,5);
+    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