Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

motion.cpp@11:ea2112ae3c4a, 2012-04-28 (annotated)

Committer:
narshu
Date:
Sat Apr 28 18:21:16 2012 +0000
Revision:
11:ea2112ae3c4a
Parent:
10:294b9adbc9d3 
half done chagnes n stuff

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 11:ea2112ae3c4a 4
narshu 10:294b9adbc9d3 5 Motion::Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin):
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 11:ea2112ae3c4a 53
narshu 10:294b9adbc9d3 54 if (pc.readable() == 1) {
narshu 11:ea2112ae3c4a 55 float cmd;
narshu 10:294b9adbc9d3 56 pc.scanf("%f", &cmd);
narshu 10:294b9adbc9d3 57 //Tune PID referece
narshu 11:ea2112ae3c4a 58 PIDControllerMotorTheta.setTunings(cmd, 0, 0);
narshu 10:294b9adbc9d3 59 }
narshu 11:ea2112ae3c4a 60
narshu 10:294b9adbc9d3 61
narshu 10:294b9adbc9d3 62 // check if target reached ----------------------------------
narshu 10:294b9adbc9d3 63 if ( ( abs(currX - ai.target.x) < POSITION_TOR )
narshu 10:294b9adbc9d3 64 &&( abs(currY - ai.target.y) < POSITION_TOR )
narshu 10:294b9adbc9d3 65 ) {
narshu 10:294b9adbc9d3 66
narshu 10:294b9adbc9d3 67 diffDir = rectifyAng(currTheta - ai.target.theta);
narshu 10:294b9adbc9d3 68 //diffSpeed = diffDir / PI;
narshu 10:294b9adbc9d3 69
narshu 10:294b9adbc9d3 70 PIDControllerMotorTheta.setProcessValue(diffDir);
narshu 10:294b9adbc9d3 71
narshu 10:294b9adbc9d3 72 if (abs(diffDir) > ANGLE_TOR) {
narshu 10:294b9adbc9d3 73
narshu 10:294b9adbc9d3 74 float tempPidVar = PIDControllerMotorTheta.compute();
narshu 10:294b9adbc9d3 75 motors.setSpeed( -int(tempPidVar*MOVE_SPEED), int(tempPidVar*MOVE_SPEED));
narshu 10:294b9adbc9d3 76
narshu 10:294b9adbc9d3 77 } else {
narshu 10:294b9adbc9d3 78 motors.stop();
narshu 10:294b9adbc9d3 79 Thread::wait(4000);
narshu 10:294b9adbc9d3 80 ai.thr_AI.signal_set(0x01);
narshu 10:294b9adbc9d3 81 }
narshu 10:294b9adbc9d3 82 }
narshu 10:294b9adbc9d3 83
narshu 10:294b9adbc9d3 84 // adjust motion to reach target ----------------------------
narshu 10:294b9adbc9d3 85 else {
narshu 10:294b9adbc9d3 86
narshu 10:294b9adbc9d3 87 // calc direction to target
narshu 10:294b9adbc9d3 88 float targetDir = atan2(ai.target.y - currY, ai.target.x - currX);
narshu 10:294b9adbc9d3 89 if (!ai.target.facing) targetDir = targetDir - PI;
narshu 10:294b9adbc9d3 90
narshu 10:294b9adbc9d3 91 //Angle differene in -PI to PI
narshu 10:294b9adbc9d3 92 diffDir = rectifyAng(currTheta - targetDir);
narshu 10:294b9adbc9d3 93
narshu 10:294b9adbc9d3 94 //Set PID process variable
narshu 10:294b9adbc9d3 95 PIDControllerMotorTheta.setProcessValue(diffDir);
narshu 10:294b9adbc9d3 96 PIDControllerMotorTheta2.setProcessValue(diffDir);
narshu 10:294b9adbc9d3 97
narshu 10:294b9adbc9d3 98 //if diffDIr is neg, spin right
narshu 10:294b9adbc9d3 99 //if diffDir is pos, spin left
narshu 10:294b9adbc9d3 100
narshu 11:ea2112ae3c4a 101 if (abs(diffDir) > 0) { //roughly 32 degrees
narshu 10:294b9adbc9d3 102 //ANGLE_TOR*4
narshu 10:294b9adbc9d3 103 float tempPidVar = PIDControllerMotorTheta.compute();
narshu 10:294b9adbc9d3 104 motors.setSpeed( -int(tempPidVar*MOVE_SPEED), int(tempPidVar*MOVE_SPEED));
narshu 11:ea2112ae3c4a 105 pc.printf("spin,%f\n",diffDir);
narshu 10:294b9adbc9d3 106
narshu 10:294b9adbc9d3 107 } else {
narshu 11:ea2112ae3c4a 108 /*
narshu 10:294b9adbc9d3 109 float tempPidVar = PIDControllerMotorTheta2.compute();
narshu 11:ea2112ae3c4a 110 pc.printf("turn,%f\n",diffDir);
narshu 10:294b9adbc9d3 111 // calculte the motor speeds
narshu 10:294b9adbc9d3 112 if (tempPidVar >= 0) {
narshu 10:294b9adbc9d3 113 //turn left
narshu 10:294b9adbc9d3 114 speedL = (1-abs(tempPidVar))*MOVE_SPEED;
narshu 10:294b9adbc9d3 115 speedR = MOVE_SPEED;
narshu 10:294b9adbc9d3 116
narshu 10:294b9adbc9d3 117 } else {
narshu 10:294b9adbc9d3 118 //turn right
narshu 10:294b9adbc9d3 119 speedR = (1-abs(tempPidVar))*MOVE_SPEED;
narshu 10:294b9adbc9d3 120 speedL = MOVE_SPEED;
narshu 10:294b9adbc9d3 121 }
narshu 10:294b9adbc9d3 122 if (ai.target.facing) motors.setSpeed( int(speedL), int(speedR));
narshu 10:294b9adbc9d3 123 else motors.setSpeed( -int(speedR), -int(speedL));
narshu 11:ea2112ae3c4a 124 */
narshu 10:294b9adbc9d3 125 }
narshu 10:294b9adbc9d3 126 }
narshu 10:294b9adbc9d3 127
narshu 10:294b9adbc9d3 128 // wait
narshu 10:294b9adbc9d3 129 Thread::wait(MOTION_UPDATE_PERIOD);
narshu 10:294b9adbc9d3 130 }
narshu 10:294b9adbc9d3 131 }