Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/Motion/motion.cpp@24:7a3906c2f5d5, 2012-05-04 (annotated)

Committer:
narshu
Date:
Fri May 04 05:23:45 2012 +0000
Revision:
24:7a3906c2f5d5
1st working version with accurate target acquisition.

Who changed what in which revision?

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