Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Eurobot_shared/Motion/motion.cpp
- Committer:
- narshu
- Date:
- 2012-04-29
- Revision:
- 17:bafcef1c3579
File content as of revision 17:bafcef1c3579:
#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); } }