Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/Motion/motion.cpp@13:57ea4e520dbd, 2012-04-28 (annotated)

Committer:
narshu
Date:
Sat Apr 28 19:39:08 2012 +0000
Revision:
13:57ea4e520dbd
Child:
14:24f994dc2770
PID tuned. Modified ai signalling logic at target location.

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