Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

motion.cpp@12:2981367c63a0, 2012-04-28 (annotated)

Committer:
narshu
Date:
Sat Apr 28 18:26:26 2012 +0000
Revision:
12:2981367c63a0
Parent:
10:294b9adbc9d3 
Working after moving motion to own file

Who changed what in which revision?

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