Diff: Motion/motion.cpp

Revision:

0:434fd09723be

diff -r 000000000000 -r 434fd09723be Motion/motion.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion/motion.cpp	Tue Aug 07 10:25:53 2012 +0000
@@ -0,0 +1,237 @@
+#include "motion.h"
+#include "geometryfuncs.h"
+#include "system.h"
+#include "PID.h"
+
+AnalogIn ObsAvoidPin(p20);
+
+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, FWD_MOVE_P/10.0f, 0.000005, MOTION_UPDATE_PERIOD/1000.0f);     //Going forward
+    PID PIDControllerMotorTheta(SPIN_MOVE_P, SPIN_MOVE_P/10.0f, 0.000005, 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;
+    float xBuffer, yBuffer;
+    float xOriginalBuffer = 0, yOriginalBuffer = 0;
+    int initiateFlag = 1;
+    int dontSpinFlag = 0;
+    int atTargetFlag = 0;
+    int atTargetDirectionFlag = 0;
+
+    while (1) {
+        //kalman.statelock.lock();
+        if (ai.flag_terminate) {
+            // stops motors and teminates the thread
+            motors.stop();
+            //motors.coastStop();
+            terminate();
+        }
+
+        // stops motor
+        if ((ai.flag_motorStop) || (ObsAvoidPin > 0.4)) {
+            motors.stop();
+        } else if (ai.flag_manOverride) {
+
+        } else {
+
+
+            // 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();
+
+            // make a local copy of the target
+            ai.targetlock.lock();
+            AI::Target loctarget = ai.gettarget();
+            ai.targetlock.unlock();
+            /*
+            //PID Tuning Code
+            if (pc.readable() == 1) {
+                float cmd;
+                pc.scanf("%f", &cmd);
+                //Tune PID referece
+                PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
+            }
+            */
+
+
+            if (initiateFlag == 1) {
+                xOriginalBuffer = currX;
+                yOriginalBuffer = currY;
+
+                xBuffer = ai.gettarget().x;
+                yBuffer = ai.gettarget().y;
+
+                initiateFlag = 0;
+            }
+
+            if (xBuffer != loctarget.x || yBuffer != loctarget.y) {
+                //target changed
+                //update xOriginal and yOriginal buffers
+                xOriginalBuffer = currX;
+                yOriginalBuffer = currY;
+
+                xBuffer = loctarget.x;
+                yBuffer = loctarget.y;
+
+                atTargetFlag = 0;
+                atTargetDirectionFlag = 0;
+
+            }
+
+            // check if target reached ----------------------------------
+            if (atTargetFlag || hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
+
+                if (atTargetFlag == 0) {
+                    motors.stop();
+                    Thread::wait(100);
+                }
+
+
+                if (hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
+                    atTargetFlag = 1;
+                }
+                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 (atTargetDirectionFlag == 0) {
+                        ai.thr_AI.signal_set(0x01);
+                        atTargetDirectionFlag = 1;
+                    }
+
+                    /*
+                    if (!loctarget.reached) {
+                        static int counter = 10;
+                        // guarding counter for reaching target
+                        if (counter-- == 0) {
+                            counter = 10;
+                            ai.target.reached = true;
+                            ai.thr_AI.signal_set(0x01);
+
+                        }
+                    }
+                    */
+                }
+            }
+
+            // adjust motion to reach target ----------------------------
+            else {
+
+                OLED3 = 1;
+
+                /*
+                if ((hypot(xOriginalBuffer - loctarget.x, yOriginalBuffer - loctarget.y) - hypot(xOriginalBuffer - currX, yOriginalBuffer - currY)) < 0) {
+                    loctarget.facing = !loctarget.facing;
+                    dontSpinFlag = 1;
+                } else {
+                    dontSpinFlag = 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) && (dontSpinFlag == 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);
+
+                } 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 > 200) && motors.accelerationRegister == 1) {
+                        MoveSpeedLimiter = (distanceToTarget)/400;
+                    } else if (distanceToTarget <= 200) {
+                        MoveSpeedLimiter  = 0.5;
+                    }
+
+
+
+
+                    // 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