Shuto Naruse
Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Diff: Eurobot_shared/Motion/motion.cpp
- Revision:
- 25:143b19c1fb05
- Parent:
- 24:7a3906c2f5d5
- Child:
- 26:0995f61cb7b8
--- a/Eurobot_shared/Motion/motion.cpp Fri May 04 05:23:45 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,238 +0,0 @@
-#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.coastStop();
- terminate();
- }
-
- // stops motor
- if ((ai.flag_motorStop) || (ObsAvoidPin > 0.5)) {
- 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("m1Cmd,%f\n",diffDir);
-
- } else {
-
-
- float tempPidVar = PIDControllerMotorTheta2.compute();
- float MoveSpeedLimiter = 1;
- //pc.printf("m1Speed,%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)) {
- 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