Shuto Naruse
/
Eurobot2012_Secondary
Eurobot2012_Secondary
Fork of Eurobot_2012_Secondary by
Eurobot_shared/Motion/motion.cpp
- Committer:
- narshu
- Date:
- 2012-10-17
- Revision:
- 1:cc2a9eb0bd55
File content as of revision 1:cc2a9eb0bd55:
#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); } }