commit!
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 13 2022 17:39:34 by 1.7.2