motion.cpp

00001 #include "motion.h"
00002 #include "geometryfuncs.h"
00003 #include "system.h"
00004 #include "PID.h"
00005 
00006 AnalogIn ObsAvoidPin(p20);
00007 
00008 Motion::Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin):
00009         thr_motion(mtwrapper,this,osPriorityNormal,1024),
00010         motors(motorsin),
00011         ai(aiin),
00012         kalman(kalmanin) { }
00013 
00014 // motion control thread ------------------------
00015 void Motion::motion_thread() {
00016     motors.resetEncoders();
00017     motors.setSpeed(5,5);
00018     motors.stop();
00019     // Thread::wait(1500);
00020     //ai.thr_AI.signal_set(0x01);
00021 
00022     //PID declare
00023     PID PIDControllerMotorTheta2(FWD_MOVE_P, FWD_MOVE_P/10.0f, 0.000005, MOTION_UPDATE_PERIOD/1000.0f);     //Going forward
00024     PID PIDControllerMotorTheta(SPIN_MOVE_P, SPIN_MOVE_P/10.0f, 0.000005, MOTION_UPDATE_PERIOD/1000.0f);    //Spinning on the spot
00025 
00026     //PID Initialisation
00027     PIDControllerMotorTheta2.setMode(MANUAL_MODE);
00028     PIDControllerMotorTheta.setMode(MANUAL_MODE);
00029 
00030     PIDControllerMotorTheta2.setBias(0);
00031     PIDControllerMotorTheta.setBias(0);
00032 
00033     PIDControllerMotorTheta2.setOutputLimits(-1, 1);
00034     PIDControllerMotorTheta.setOutputLimits(-1, 1);
00035 
00036     PIDControllerMotorTheta2.setInputLimits(-PI, PI);
00037     PIDControllerMotorTheta.setInputLimits(-PI, PI);
00038 
00039     PIDControllerMotorTheta.setSetPoint(0);
00040     PIDControllerMotorTheta2.setSetPoint(0);
00041 
00042     float currX, currY,currTheta;
00043     float speedL,speedR;
00044     float diffDir;
00045     float xBuffer, yBuffer;
00046     float xOriginalBuffer = 0, yOriginalBuffer = 0;
00047     int initiateFlag = 1;
00048     int dontSpinFlag = 0;
00049     int atTargetFlag = 0;
00050     int atTargetDirectionFlag = 0;
00051 
00052     while (1) {
00053         //kalman.statelock.lock();
00054         if (ai.flag_terminate) {
00055             // stops motors and teminates the thread
00056             motors.stop();
00057             //motors.coastStop();
00058             terminate();
00059         }
00060 
00061         // stops motor
00062         if ((ai.flag_motorStop) || (ObsAvoidPin > 0.4)) {
00063             motors.stop();
00064         } else if (ai.flag_manOverride) {
00065 
00066         } else {
00067 
00068 
00069             // get kalman localization estimate ------------------------
00070             kalman.statelock.lock();
00071             currX = kalman.X(0)*1000.0f;
00072             currY = kalman.X(1)*1000.0f;
00073             currTheta = kalman.X(2);
00074             kalman.statelock.unlock();
00075 
00076             // make a local copy of the target
00077             ai.targetlock.lock();
00078             AI::Target loctarget = ai.gettarget();
00079             ai.targetlock.unlock();
00080             /*
00081             //PID Tuning Code
00082             if (pc.readable() == 1) {
00083                 float cmd;
00084                 pc.scanf("%f", &cmd);
00085                 //Tune PID referece
00086                 PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
00087             }
00088             */
00089 
00090 
00091             if (initiateFlag == 1) {
00092                 xOriginalBuffer = currX;
00093                 yOriginalBuffer = currY;
00094 
00095                 xBuffer = ai.gettarget().x;
00096                 yBuffer = ai.gettarget().y;
00097 
00098                 initiateFlag = 0;
00099             }
00100 
00101             if (xBuffer != loctarget.x || yBuffer != loctarget.y) {
00102                 //target changed
00103                 //update xOriginal and yOriginal buffers
00104                 xOriginalBuffer = currX;
00105                 yOriginalBuffer = currY;
00106 
00107                 xBuffer = loctarget.x;
00108                 yBuffer = loctarget.y;
00109 
00110                 atTargetFlag = 0;
00111                 atTargetDirectionFlag = 0;
00112 
00113             }
00114 
00115             // check if target reached ----------------------------------
00116             if (atTargetFlag || hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
00117 
00118                 if (atTargetFlag == 0) {
00119                     motors.stop();
00120                     Thread::wait(100);
00121                 }
00122 
00123 
00124                 if (hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
00125                     atTargetFlag = 1;
00126                 }
00127                 OLED4 = 1;
00128 
00129                 diffDir = rectifyAng(currTheta - loctarget.theta);
00130                 //diffSpeed = diffDir / PI;
00131 
00132                 PIDControllerMotorTheta.setProcessValue(diffDir);
00133                 float tempPidVar = PIDControllerMotorTheta.compute();
00134                 motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));
00135 
00136                 if (abs(diffDir) < ANGLE_TOR) {
00137 
00138                     if (atTargetDirectionFlag == 0) {
00139                         ai.thr_AI.signal_set(0x01);
00140                         atTargetDirectionFlag = 1;
00141                     }
00142 
00143                     /*
00144                     if (!loctarget.reached) {
00145                         static int counter = 10;
00146                         // guarding counter for reaching target
00147                         if (counter-- == 0) {
00148                             counter = 10;
00149                             ai.target.reached = true;
00150                             ai.thr_AI.signal_set(0x01);
00151 
00152                         }
00153                     }
00154                     */
00155                 }
00156             }
00157 
00158             // adjust motion to reach target ----------------------------
00159             else {
00160 
00161                 OLED3 = 1;
00162 
00163                 /*
00164                 if ((hypot(xOriginalBuffer - loctarget.x, yOriginalBuffer - loctarget.y) - hypot(xOriginalBuffer - currX, yOriginalBuffer - currY)) < 0) {
00165                     loctarget.facing = !loctarget.facing;
00166                     dontSpinFlag = 1;
00167                 } else {
00168                     dontSpinFlag = 0;
00169                 }
00170                 */
00171 
00172                 // calc direction to target
00173                 float targetDir = atan2(loctarget.y - currY, loctarget.x - currX);
00174                 if (!loctarget.facing) targetDir =  targetDir + PI;
00175 
00176                 //Angle differene in -PI to PI
00177                 diffDir = rectifyAng(currTheta - targetDir);
00178 
00179                 //Set PID process variable
00180                 PIDControllerMotorTheta.setProcessValue(diffDir);
00181                 PIDControllerMotorTheta2.setProcessValue(diffDir);
00182 
00183                 //if diffDIr is neg, spin right
00184                 //if diffDir is pos, spin left
00185 
00186                 if ((abs(diffDir) > ANGLE_TOR*4) && (dontSpinFlag == 0)) {   //roughly 32 degrees
00187                     //ANGLE_TOR*4
00188                     float tempPidVar = PIDControllerMotorTheta.compute();
00189                     motors.setSpeed( -int(tempPidVar*MOVE_SPEED),  int(tempPidVar*MOVE_SPEED));
00190                     //pc.printf("spin,%f\n",diffDir);
00191 
00192                 } else {
00193 
00194                     float tempPidVar = PIDControllerMotorTheta2.compute();
00195                     float MoveSpeedLimiter = 1;
00196                     //pc.printf("turn,%f\n",diffDir);
00197 
00198                     float distanceToX = (float)abs(currX - loctarget.x);
00199                     float distanceToY = (float)abs(currY - loctarget.y);
00200 
00201                     float distanceToTarget = hypot(distanceToX, distanceToY);
00202 
00203                     if ((distanceToTarget < 400) && (distanceToTarget > 200) && (motors.accelerationRegister == 1)) {
00204                         MoveSpeedLimiter = (distanceToTarget)/400;
00205                     } else if (distanceToTarget <= 200) {
00206                         MoveSpeedLimiter  = 0.5;
00207                     }
00208 
00209 
00210 
00211 
00212                     // calculte the motor speeds
00213                     if (tempPidVar >= 0) {
00214                         //turn left
00215                         speedL = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter;
00216                         speedR = MOVE_SPEED*MoveSpeedLimiter;
00217 
00218                     } else {
00219                         //turn right
00220                         speedR = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter;
00221                         speedL = MOVE_SPEED*MoveSpeedLimiter;
00222                     }
00223 
00224 
00225 
00226 
00227                     if (loctarget.facing) motors.setSpeed( int(speedL),  int(speedR));
00228                     else motors.setSpeed( -int(speedR),  -int(speedL));
00229 
00230                 }
00231             }
00232         }
00233         //kalman.statelock.unlock();
00234         // wait
00235         Thread::wait(MOTION_UPDATE_PERIOD);
00236     }
00237 }