Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/Motion/motion.cpp@26:0995f61cb7b8, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:22:47 2012 +0000
Revision:
26:0995f61cb7b8
Eurobot 2012 Primary;

Who changed what in which revision?

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