Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/Motion/motion.cpp@15:acae5c0e9ca8, 2012-04-28 (annotated)

Committer:
narshu
Date:
Sat Apr 28 22:21:03 2012 +0000
Revision:
15:acae5c0e9ca8
Parent:
14:24f994dc2770 
functioning motion control code

Who changed what in which revision?

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