Shuto Naruse / Eurobot_2012_shared

Eurobot_shared pubulished from Eurobot Primary

Motion/motion.cpp@0:434fd09723be, 2012-08-07 (annotated)

Committer:
narshu
Date:
Tue Aug 07 10:25:53 2012 +0000
Revision:
0:434fd09723be
[mbed] converted /Eurobot_2012_Primary/Eurobot_shared

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 0:434fd09723be 1 #include "motion.h"
narshu 0:434fd09723be 2 #include "geometryfuncs.h"
narshu 0:434fd09723be 3 #include "system.h"
narshu 0:434fd09723be 4 #include "PID.h"
narshu 0:434fd09723be 5
narshu 0:434fd09723be 6 AnalogIn ObsAvoidPin(p20);
narshu 0:434fd09723be 7
narshu 0:434fd09723be 8 Motion::Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin):
narshu 0:434fd09723be 9 thr_motion(mtwrapper,this,osPriorityNormal,1024),
narshu 0:434fd09723be 10 motors(motorsin),
narshu 0:434fd09723be 11 ai(aiin),
narshu 0:434fd09723be 12 kalman(kalmanin) { }
narshu 0:434fd09723be 13
narshu 0:434fd09723be 14 // motion control thread ------------------------
narshu 0:434fd09723be 15 void Motion::motion_thread() {
narshu 0:434fd09723be 16 motors.resetEncoders();
narshu 0:434fd09723be 17 motors.setSpeed(5,5);
narshu 0:434fd09723be 18 motors.stop();
narshu 0:434fd09723be 19 // Thread::wait(1500);
narshu 0:434fd09723be 20 //ai.thr_AI.signal_set(0x01);
narshu 0:434fd09723be 21
narshu 0:434fd09723be 22 //PID declare
narshu 0:434fd09723be 23 PID PIDControllerMotorTheta2(FWD_MOVE_P, FWD_MOVE_P/10.0f, 0.000005, MOTION_UPDATE_PERIOD/1000.0f); //Going forward
narshu 0:434fd09723be 24 PID PIDControllerMotorTheta(SPIN_MOVE_P, SPIN_MOVE_P/10.0f, 0.000005, MOTION_UPDATE_PERIOD/1000.0f); //Spinning on the spot
narshu 0:434fd09723be 25
narshu 0:434fd09723be 26 //PID Initialisation
narshu 0:434fd09723be 27 PIDControllerMotorTheta2.setMode(MANUAL_MODE);
narshu 0:434fd09723be 28 PIDControllerMotorTheta.setMode(MANUAL_MODE);
narshu 0:434fd09723be 29
narshu 0:434fd09723be 30 PIDControllerMotorTheta2.setBias(0);
narshu 0:434fd09723be 31 PIDControllerMotorTheta.setBias(0);
narshu 0:434fd09723be 32
narshu 0:434fd09723be 33 PIDControllerMotorTheta2.setOutputLimits(-1, 1);
narshu 0:434fd09723be 34 PIDControllerMotorTheta.setOutputLimits(-1, 1);
narshu 0:434fd09723be 35
narshu 0:434fd09723be 36 PIDControllerMotorTheta2.setInputLimits(-PI, PI);
narshu 0:434fd09723be 37 PIDControllerMotorTheta.setInputLimits(-PI, PI);
narshu 0:434fd09723be 38
narshu 0:434fd09723be 39 PIDControllerMotorTheta.setSetPoint(0);
narshu 0:434fd09723be 40 PIDControllerMotorTheta2.setSetPoint(0);
narshu 0:434fd09723be 41
narshu 0:434fd09723be 42 float currX, currY,currTheta;
narshu 0:434fd09723be 43 float speedL,speedR;
narshu 0:434fd09723be 44 float diffDir;
narshu 0:434fd09723be 45 float xBuffer, yBuffer;
narshu 0:434fd09723be 46 float xOriginalBuffer = 0, yOriginalBuffer = 0;
narshu 0:434fd09723be 47 int initiateFlag = 1;
narshu 0:434fd09723be 48 int dontSpinFlag = 0;
narshu 0:434fd09723be 49 int atTargetFlag = 0;
narshu 0:434fd09723be 50 int atTargetDirectionFlag = 0;
narshu 0:434fd09723be 51
narshu 0:434fd09723be 52 while (1) {
narshu 0:434fd09723be 53 //kalman.statelock.lock();
narshu 0:434fd09723be 54 if (ai.flag_terminate) {
narshu 0:434fd09723be 55 // stops motors and teminates the thread
narshu 0:434fd09723be 56 motors.stop();
narshu 0:434fd09723be 57 //motors.coastStop();
narshu 0:434fd09723be 58 terminate();
narshu 0:434fd09723be 59 }
narshu 0:434fd09723be 60
narshu 0:434fd09723be 61 // stops motor
narshu 0:434fd09723be 62 if ((ai.flag_motorStop) || (ObsAvoidPin > 0.4)) {
narshu 0:434fd09723be 63 motors.stop();
narshu 0:434fd09723be 64 } else if (ai.flag_manOverride) {
narshu 0:434fd09723be 65
narshu 0:434fd09723be 66 } else {
narshu 0:434fd09723be 67
narshu 0:434fd09723be 68
narshu 0:434fd09723be 69 // get kalman localization estimate ------------------------
narshu 0:434fd09723be 70 kalman.statelock.lock();
narshu 0:434fd09723be 71 currX = kalman.X(0)*1000.0f;
narshu 0:434fd09723be 72 currY = kalman.X(1)*1000.0f;
narshu 0:434fd09723be 73 currTheta = kalman.X(2);
narshu 0:434fd09723be 74 kalman.statelock.unlock();
narshu 0:434fd09723be 75
narshu 0:434fd09723be 76 // make a local copy of the target
narshu 0:434fd09723be 77 ai.targetlock.lock();
narshu 0:434fd09723be 78 AI::Target loctarget = ai.gettarget();
narshu 0:434fd09723be 79 ai.targetlock.unlock();
narshu 0:434fd09723be 80 /*
narshu 0:434fd09723be 81 //PID Tuning Code
narshu 0:434fd09723be 82 if (pc.readable() == 1) {
narshu 0:434fd09723be 83 float cmd;
narshu 0:434fd09723be 84 pc.scanf("%f", &cmd);
narshu 0:434fd09723be 85 //Tune PID referece
narshu 0:434fd09723be 86 PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
narshu 0:434fd09723be 87 }
narshu 0:434fd09723be 88 */
narshu 0:434fd09723be 89
narshu 0:434fd09723be 90
narshu 0:434fd09723be 91 if (initiateFlag == 1) {
narshu 0:434fd09723be 92 xOriginalBuffer = currX;
narshu 0:434fd09723be 93 yOriginalBuffer = currY;
narshu 0:434fd09723be 94
narshu 0:434fd09723be 95 xBuffer = ai.gettarget().x;
narshu 0:434fd09723be 96 yBuffer = ai.gettarget().y;
narshu 0:434fd09723be 97
narshu 0:434fd09723be 98 initiateFlag = 0;
narshu 0:434fd09723be 99 }
narshu 0:434fd09723be 100
narshu 0:434fd09723be 101 if (xBuffer != loctarget.x || yBuffer != loctarget.y) {
narshu 0:434fd09723be 102 //target changed
narshu 0:434fd09723be 103 //update xOriginal and yOriginal buffers
narshu 0:434fd09723be 104 xOriginalBuffer = currX;
narshu 0:434fd09723be 105 yOriginalBuffer = currY;
narshu 0:434fd09723be 106
narshu 0:434fd09723be 107 xBuffer = loctarget.x;
narshu 0:434fd09723be 108 yBuffer = loctarget.y;
narshu 0:434fd09723be 109
narshu 0:434fd09723be 110 atTargetFlag = 0;
narshu 0:434fd09723be 111 atTargetDirectionFlag = 0;
narshu 0:434fd09723be 112
narshu 0:434fd09723be 113 }
narshu 0:434fd09723be 114
narshu 0:434fd09723be 115 // check if target reached ----------------------------------
narshu 0:434fd09723be 116 if (atTargetFlag || hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
narshu 0:434fd09723be 117
narshu 0:434fd09723be 118 if (atTargetFlag == 0) {
narshu 0:434fd09723be 119 motors.stop();
narshu 0:434fd09723be 120 Thread::wait(100);
narshu 0:434fd09723be 121 }
narshu 0:434fd09723be 122
narshu 0:434fd09723be 123
narshu 0:434fd09723be 124 if (hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
narshu 0:434fd09723be 125 atTargetFlag = 1;
narshu 0:434fd09723be 126 }
narshu 0:434fd09723be 127 OLED4 = 1;
narshu 0:434fd09723be 128
narshu 0:434fd09723be 129 diffDir = rectifyAng(currTheta - loctarget.theta);
narshu 0:434fd09723be 130 //diffSpeed = diffDir / PI;
narshu 0:434fd09723be 131
narshu 0:434fd09723be 132 PIDControllerMotorTheta.setProcessValue(diffDir);
narshu 0:434fd09723be 133 float tempPidVar = PIDControllerMotorTheta.compute();
narshu 0:434fd09723be 134 motors.setSpeed( -int(tempPidVar*MOVE_SPEED), int(tempPidVar*MOVE_SPEED));
narshu 0:434fd09723be 135
narshu 0:434fd09723be 136 if (abs(diffDir) < ANGLE_TOR) {
narshu 0:434fd09723be 137
narshu 0:434fd09723be 138 if (atTargetDirectionFlag == 0) {
narshu 0:434fd09723be 139 ai.thr_AI.signal_set(0x01);
narshu 0:434fd09723be 140 atTargetDirectionFlag = 1;
narshu 0:434fd09723be 141 }
narshu 0:434fd09723be 142
narshu 0:434fd09723be 143 /*
narshu 0:434fd09723be 144 if (!loctarget.reached) {
narshu 0:434fd09723be 145 static int counter = 10;
narshu 0:434fd09723be 146 // guarding counter for reaching target
narshu 0:434fd09723be 147 if (counter-- == 0) {
narshu 0:434fd09723be 148 counter = 10;
narshu 0:434fd09723be 149 ai.target.reached = true;
narshu 0:434fd09723be 150 ai.thr_AI.signal_set(0x01);
narshu 0:434fd09723be 151
narshu 0:434fd09723be 152 }
narshu 0:434fd09723be 153 }
narshu 0:434fd09723be 154 */
narshu 0:434fd09723be 155 }
narshu 0:434fd09723be 156 }
narshu 0:434fd09723be 157
narshu 0:434fd09723be 158 // adjust motion to reach target ----------------------------
narshu 0:434fd09723be 159 else {
narshu 0:434fd09723be 160
narshu 0:434fd09723be 161 OLED3 = 1;
narshu 0:434fd09723be 162
narshu 0:434fd09723be 163 /*
narshu 0:434fd09723be 164 if ((hypot(xOriginalBuffer - loctarget.x, yOriginalBuffer - loctarget.y) - hypot(xOriginalBuffer - currX, yOriginalBuffer - currY)) < 0) {
narshu 0:434fd09723be 165 loctarget.facing = !loctarget.facing;
narshu 0:434fd09723be 166 dontSpinFlag = 1;
narshu 0:434fd09723be 167 } else {
narshu 0:434fd09723be 168 dontSpinFlag = 0;
narshu 0:434fd09723be 169 }
narshu 0:434fd09723be 170 */
narshu 0:434fd09723be 171
narshu 0:434fd09723be 172 // calc direction to target
narshu 0:434fd09723be 173 float targetDir = atan2(loctarget.y - currY, loctarget.x - currX);
narshu 0:434fd09723be 174 if (!loctarget.facing) targetDir = targetDir + PI;
narshu 0:434fd09723be 175
narshu 0:434fd09723be 176 //Angle differene in -PI to PI
narshu 0:434fd09723be 177 diffDir = rectifyAng(currTheta - targetDir);
narshu 0:434fd09723be 178
narshu 0:434fd09723be 179 //Set PID process variable
narshu 0:434fd09723be 180 PIDControllerMotorTheta.setProcessValue(diffDir);
narshu 0:434fd09723be 181 PIDControllerMotorTheta2.setProcessValue(diffDir);
narshu 0:434fd09723be 182
narshu 0:434fd09723be 183 //if diffDIr is neg, spin right
narshu 0:434fd09723be 184 //if diffDir is pos, spin left
narshu 0:434fd09723be 185
narshu 0:434fd09723be 186 if ((abs(diffDir) > ANGLE_TOR*4) && (dontSpinFlag == 0)) { //roughly 32 degrees
narshu 0:434fd09723be 187 //ANGLE_TOR*4
narshu 0:434fd09723be 188 float tempPidVar = PIDControllerMotorTheta.compute();
narshu 0:434fd09723be 189 motors.setSpeed( -int(tempPidVar*MOVE_SPEED), int(tempPidVar*MOVE_SPEED));
narshu 0:434fd09723be 190 //pc.printf("spin,%f\n",diffDir);
narshu 0:434fd09723be 191
narshu 0:434fd09723be 192 } else {
narshu 0:434fd09723be 193
narshu 0:434fd09723be 194 float tempPidVar = PIDControllerMotorTheta2.compute();
narshu 0:434fd09723be 195 float MoveSpeedLimiter = 1;
narshu 0:434fd09723be 196 //pc.printf("turn,%f\n",diffDir);
narshu 0:434fd09723be 197
narshu 0:434fd09723be 198 float distanceToX = (float)abs(currX - loctarget.x);
narshu 0:434fd09723be 199 float distanceToY = (float)abs(currY - loctarget.y);
narshu 0:434fd09723be 200
narshu 0:434fd09723be 201 float distanceToTarget = hypot(distanceToX, distanceToY);
narshu 0:434fd09723be 202
narshu 0:434fd09723be 203 if ((distanceToTarget < 400) && (distanceToTarget > 200) && motors.accelerationRegister == 1) {
narshu 0:434fd09723be 204 MoveSpeedLimiter = (distanceToTarget)/400;
narshu 0:434fd09723be 205 } else if (distanceToTarget <= 200) {
narshu 0:434fd09723be 206 MoveSpeedLimiter = 0.5;
narshu 0:434fd09723be 207 }
narshu 0:434fd09723be 208
narshu 0:434fd09723be 209
narshu 0:434fd09723be 210
narshu 0:434fd09723be 211
narshu 0:434fd09723be 212 // calculte the motor speeds
narshu 0:434fd09723be 213 if (tempPidVar >= 0) {
narshu 0:434fd09723be 214 //turn left
narshu 0:434fd09723be 215 speedL = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter;
narshu 0:434fd09723be 216 speedR = MOVE_SPEED*MoveSpeedLimiter;
narshu 0:434fd09723be 217
narshu 0:434fd09723be 218 } else {
narshu 0:434fd09723be 219 //turn right
narshu 0:434fd09723be 220 speedR = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter;
narshu 0:434fd09723be 221 speedL = MOVE_SPEED*MoveSpeedLimiter;
narshu 0:434fd09723be 222 }
narshu 0:434fd09723be 223
narshu 0:434fd09723be 224
narshu 0:434fd09723be 225
narshu 0:434fd09723be 226
narshu 0:434fd09723be 227 if (loctarget.facing) motors.setSpeed( int(speedL), int(speedR));
narshu 0:434fd09723be 228 else motors.setSpeed( -int(speedR), -int(speedL));
narshu 0:434fd09723be 229
narshu 0:434fd09723be 230 }
narshu 0:434fd09723be 231 }
narshu 0:434fd09723be 232 }
narshu 0:434fd09723be 233 //kalman.statelock.unlock();
narshu 0:434fd09723be 234 // wait
narshu 0:434fd09723be 235 Thread::wait(MOTION_UPDATE_PERIOD);
narshu 0:434fd09723be 236 }
narshu 0:434fd09723be 237 }