Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
motion.cpp
- Committer:
- narshu
- Date:
- 2012-04-28
- Revision:
- 10:294b9adbc9d3
- Child:
- 11:ea2112ae3c4a
- Child:
- 12:2981367c63a0
File content as of revision 10:294b9adbc9d3:
#include "motion.h" #include "geometryfuncs.h" Motion::Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin): motors(motorsin), ai(aiin), kalman(kalmanin) { } // motion control thread ------------------------ void Motion::motion_thread() { motors.resetEncoders(); motors.stop(); 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; while (1) { 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(); /* if (pc.readable() == 1) { pc.scanf("%f", &cmd); //Tune PID referece PIDControllerMotorTheta2.setTunings(cmd, 0, 0); } */ // check if target reached ---------------------------------- if ( ( abs(currX - ai.target.x) < POSITION_TOR ) &&( abs(currY - ai.target.y) < POSITION_TOR ) ) { diffDir = rectifyAng(currTheta - ai.target.theta); //diffSpeed = diffDir / PI; PIDControllerMotorTheta.setProcessValue(diffDir); if (abs(diffDir) > ANGLE_TOR) { float tempPidVar = PIDControllerMotorTheta.compute(); motors.setSpeed( -int(tempPidVar*MOVE_SPEED), int(tempPidVar*MOVE_SPEED)); } else { motors.stop(); Thread::wait(4000); ai.thr_AI.signal_set(0x01); } } // adjust motion to reach target ---------------------------- else { // calc direction to target float targetDir = atan2(ai.target.y - currY, ai.target.x - currX); if (!ai.target.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(); //pc.printf("turn,%f\n",diffDir); // calculte the motor speeds if (tempPidVar >= 0) { //turn left speedL = (1-abs(tempPidVar))*MOVE_SPEED; speedR = MOVE_SPEED; } else { //turn right speedR = (1-abs(tempPidVar))*MOVE_SPEED; speedL = MOVE_SPEED; } if (ai.target.facing) motors.setSpeed( int(speedL), int(speedR)); else motors.setSpeed( -int(speedR), -int(speedL)); } } // wait Thread::wait(MOTION_UPDATE_PERIOD); } }