Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/Motion/motion.cpp@17:bafcef1c3579, 2012-04-29 (annotated)

Committer:
narshu
Date:
Sun Apr 29 00:09:35 2012 +0000
Revision:
17:bafcef1c3579
uptodate kalman lib; edit this one only!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 17:bafcef1c3579 1 #include "motion.h"
narshu 17:bafcef1c3579 2 #include "geometryfuncs.h"
narshu 17:bafcef1c3579 3 #include "system.h"
narshu 17:bafcef1c3579 4 #include "PID.h"
narshu 17:bafcef1c3579 5
narshu 17:bafcef1c3579 6
narshu 17:bafcef1c3579 7 Motion::Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin):
narshu 17:bafcef1c3579 8 thr_motion(mtwrapper,this,osPriorityNormal,1024),
narshu 17:bafcef1c3579 9 motors(motorsin),
narshu 17:bafcef1c3579 10 ai(aiin),
narshu 17:bafcef1c3579 11 kalman(kalmanin) { }
narshu 17:bafcef1c3579 12
narshu 17:bafcef1c3579 13 // motion control thread ------------------------
narshu 17:bafcef1c3579 14 void Motion::motion_thread() {
narshu 17:bafcef1c3579 15 motors.resetEncoders();
narshu 17:bafcef1c3579 16 motors.setSpeed(5,5);
narshu 17:bafcef1c3579 17 motors.stop();
narshu 17:bafcef1c3579 18 Thread::wait(1500);
narshu 17:bafcef1c3579 19 //ai.thr_AI.signal_set(0x01);
narshu 17:bafcef1c3579 20
narshu 17:bafcef1c3579 21 //PID declare
narshu 17:bafcef1c3579 22 PID PIDControllerMotorTheta2(FWD_MOVE_P, 0, 0, MOTION_UPDATE_PERIOD/1000.0f); //Going forward
narshu 17:bafcef1c3579 23 PID PIDControllerMotorTheta(SPIN_MOVE_P, 0, 0, MOTION_UPDATE_PERIOD/1000.0f); //Spinning on the spot
narshu 17:bafcef1c3579 24
narshu 17:bafcef1c3579 25 //PID Initialisation
narshu 17:bafcef1c3579 26 PIDControllerMotorTheta2.setMode(MANUAL_MODE);
narshu 17:bafcef1c3579 27 PIDControllerMotorTheta.setMode(MANUAL_MODE);
narshu 17:bafcef1c3579 28
narshu 17:bafcef1c3579 29 PIDControllerMotorTheta2.setBias(0);
narshu 17:bafcef1c3579 30 PIDControllerMotorTheta.setBias(0);
narshu 17:bafcef1c3579 31
narshu 17:bafcef1c3579 32 PIDControllerMotorTheta2.setOutputLimits(-1, 1);
narshu 17:bafcef1c3579 33 PIDControllerMotorTheta.setOutputLimits(-1, 1);
narshu 17:bafcef1c3579 34
narshu 17:bafcef1c3579 35 PIDControllerMotorTheta2.setInputLimits(-PI, PI);
narshu 17:bafcef1c3579 36 PIDControllerMotorTheta.setInputLimits(-PI, PI);
narshu 17:bafcef1c3579 37
narshu 17:bafcef1c3579 38 PIDControllerMotorTheta.setSetPoint(0);
narshu 17:bafcef1c3579 39 PIDControllerMotorTheta2.setSetPoint(0);
narshu 17:bafcef1c3579 40
narshu 17:bafcef1c3579 41 float currX, currY,currTheta;
narshu 17:bafcef1c3579 42 float speedL,speedR;
narshu 17:bafcef1c3579 43 float diffDir;
narshu 17:bafcef1c3579 44 int atTargetFlag = 0;
narshu 17:bafcef1c3579 45
narshu 17:bafcef1c3579 46 while (1) {
narshu 17:bafcef1c3579 47 kalman.statelock.lock();
narshu 17:bafcef1c3579 48 if (ai.flag_terminate) {
narshu 17:bafcef1c3579 49 terminate();
narshu 17:bafcef1c3579 50 }
narshu 17:bafcef1c3579 51
narshu 17:bafcef1c3579 52 // get kalman localization estimate ------------------------
narshu 17:bafcef1c3579 53 //kalman.statelock.lock();
narshu 17:bafcef1c3579 54 currX = kalman.X(0)*1000.0f;
narshu 17:bafcef1c3579 55 currY = kalman.X(1)*1000.0f;
narshu 17:bafcef1c3579 56 currTheta = kalman.X(2);
narshu 17:bafcef1c3579 57 //kalman.statelock.unlock();
narshu 17:bafcef1c3579 58
narshu 17:bafcef1c3579 59 AI::Target loctarget = ai.gettarget();
narshu 17:bafcef1c3579 60
narshu 17:bafcef1c3579 61 /*
narshu 17:bafcef1c3579 62 //PID Tuning Code
narshu 17:bafcef1c3579 63 if (pc.readable() == 1) {
narshu 17:bafcef1c3579 64 float cmd;
narshu 17:bafcef1c3579 65 pc.scanf("%f", &cmd);
narshu 17:bafcef1c3579 66 //Tune PID referece
narshu 17:bafcef1c3579 67 PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
narshu 17:bafcef1c3579 68 }
narshu 17:bafcef1c3579 69 */
narshu 17:bafcef1c3579 70
narshu 17:bafcef1c3579 71 // check if target reached ----------------------------------
narshu 17:bafcef1c3579 72 if (atTargetFlag || hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
narshu 17:bafcef1c3579 73
narshu 17:bafcef1c3579 74 atTargetFlag = loctarget.reached;
narshu 17:bafcef1c3579 75 OLED4 = 1;
narshu 17:bafcef1c3579 76
narshu 17:bafcef1c3579 77 diffDir = rectifyAng(currTheta - loctarget.theta);
narshu 17:bafcef1c3579 78 //diffSpeed = diffDir / PI;
narshu 17:bafcef1c3579 79
narshu 17:bafcef1c3579 80 PIDControllerMotorTheta.setProcessValue(diffDir);
narshu 17:bafcef1c3579 81 float tempPidVar = PIDControllerMotorTheta.compute();
narshu 17:bafcef1c3579 82 motors.setSpeed( -int(tempPidVar*MOVE_SPEED), int(tempPidVar*MOVE_SPEED));
narshu 17:bafcef1c3579 83
narshu 17:bafcef1c3579 84 if (abs(diffDir) < ANGLE_TOR) {
narshu 17:bafcef1c3579 85
narshu 17:bafcef1c3579 86 if (!loctarget.reached) {
narshu 17:bafcef1c3579 87 static int counter = 10;
narshu 17:bafcef1c3579 88 if (counter-- == 0){
narshu 17:bafcef1c3579 89 counter = 10;
narshu 17:bafcef1c3579 90 ai.target.reached = true;
narshu 17:bafcef1c3579 91 ai.thr_AI.signal_set(0x01);
narshu 17:bafcef1c3579 92
narshu 17:bafcef1c3579 93 }
narshu 17:bafcef1c3579 94 }
narshu 17:bafcef1c3579 95 }
narshu 17:bafcef1c3579 96 }
narshu 17:bafcef1c3579 97
narshu 17:bafcef1c3579 98 // adjust motion to reach target ----------------------------
narshu 17:bafcef1c3579 99 else {
narshu 17:bafcef1c3579 100
narshu 17:bafcef1c3579 101 OLED4 = 0;
narshu 17:bafcef1c3579 102
narshu 17:bafcef1c3579 103 // calc direction to target
narshu 17:bafcef1c3579 104 float targetDir = atan2(loctarget.y - currY, loctarget.x - currX);
narshu 17:bafcef1c3579 105 if (!loctarget.facing) targetDir = targetDir + PI;
narshu 17:bafcef1c3579 106
narshu 17:bafcef1c3579 107 //Angle differene in -PI to PI
narshu 17:bafcef1c3579 108 diffDir = rectifyAng(currTheta - targetDir);
narshu 17:bafcef1c3579 109
narshu 17:bafcef1c3579 110 //Set PID process variable
narshu 17:bafcef1c3579 111 PIDControllerMotorTheta.setProcessValue(diffDir);
narshu 17:bafcef1c3579 112 PIDControllerMotorTheta2.setProcessValue(diffDir);
narshu 17:bafcef1c3579 113
narshu 17:bafcef1c3579 114 //if diffDIr is neg, spin right
narshu 17:bafcef1c3579 115 //if diffDir is pos, spin left
narshu 17:bafcef1c3579 116
narshu 17:bafcef1c3579 117 if (abs(diffDir) > ANGLE_TOR*4) { //roughly 32 degrees
narshu 17:bafcef1c3579 118 //ANGLE_TOR*4
narshu 17:bafcef1c3579 119 float tempPidVar = PIDControllerMotorTheta.compute();
narshu 17:bafcef1c3579 120 motors.setSpeed( -int(tempPidVar*MOVE_SPEED), int(tempPidVar*MOVE_SPEED));
narshu 17:bafcef1c3579 121 //pc.printf("spin,%f\n",diffDir);
narshu 17:bafcef1c3579 122
narshu 17:bafcef1c3579 123 } else {
narshu 17:bafcef1c3579 124
narshu 17:bafcef1c3579 125 float tempPidVar = PIDControllerMotorTheta2.compute();
narshu 17:bafcef1c3579 126 float MoveSpeedLimiter = 1;
narshu 17:bafcef1c3579 127 //pc.printf("turn,%f\n",diffDir);
narshu 17:bafcef1c3579 128
narshu 17:bafcef1c3579 129 float distanceToX = (float)abs(currX - loctarget.x);
narshu 17:bafcef1c3579 130 float distanceToY = (float)abs(currY - loctarget.y);
narshu 17:bafcef1c3579 131
narshu 17:bafcef1c3579 132 float distanceToTarget = hypot(distanceToX, distanceToY);
narshu 17:bafcef1c3579 133
narshu 17:bafcef1c3579 134 if ((distanceToTarget < 400) && (distanceToTarget > 100)) {
narshu 17:bafcef1c3579 135 MoveSpeedLimiter = (distanceToTarget)/400;
narshu 17:bafcef1c3579 136 }
narshu 17:bafcef1c3579 137 else if((distanceToTarget < 400) && (distanceToTarget > 100)) {
narshu 17:bafcef1c3579 138 MoveSpeedLimiter = 0.25;
narshu 17:bafcef1c3579 139 }
narshu 17:bafcef1c3579 140
narshu 17:bafcef1c3579 141
narshu 17:bafcef1c3579 142
narshu 17:bafcef1c3579 143
narshu 17:bafcef1c3579 144 // calculte the motor speeds
narshu 17:bafcef1c3579 145 if (tempPidVar >= 0) {
narshu 17:bafcef1c3579 146 //turn left
narshu 17:bafcef1c3579 147 speedL = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter;
narshu 17:bafcef1c3579 148 speedR = MOVE_SPEED*MoveSpeedLimiter;
narshu 17:bafcef1c3579 149
narshu 17:bafcef1c3579 150 } else {
narshu 17:bafcef1c3579 151 //turn right
narshu 17:bafcef1c3579 152 speedR = (1-abs(tempPidVar))*MOVE_SPEED*MoveSpeedLimiter;
narshu 17:bafcef1c3579 153 speedL = MOVE_SPEED*MoveSpeedLimiter;
narshu 17:bafcef1c3579 154 }
narshu 17:bafcef1c3579 155
narshu 17:bafcef1c3579 156
narshu 17:bafcef1c3579 157
narshu 17:bafcef1c3579 158
narshu 17:bafcef1c3579 159 if (loctarget.facing) motors.setSpeed( int(speedL), int(speedR));
narshu 17:bafcef1c3579 160 else motors.setSpeed( -int(speedR), -int(speedL));
narshu 17:bafcef1c3579 161
narshu 17:bafcef1c3579 162 }
narshu 17:bafcef1c3579 163 }
narshu 17:bafcef1c3579 164 kalman.statelock.unlock();
narshu 17:bafcef1c3579 165 // wait
narshu 17:bafcef1c3579 166 Thread::wait(MOTION_UPDATE_PERIOD);
narshu 17:bafcef1c3579 167 }
narshu 17:bafcef1c3579 168 }