Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

motion.cpp@10:294b9adbc9d3, 2012-04-28 (annotated)

Committer:
narshu
Date:
Sat Apr 28 18:10:55 2012 +0000
Revision:
10:294b9adbc9d3
Child:
11:ea2112ae3c4a
Child:
12:2981367c63a0
Moved motion stuff to own file, and created class

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