Eurobot_shared - commit!

Users » narshu » Code » Eurobot_shared

commit!

Motion/motion.cpp@0:42026f893a2d, 2012-06-15 (annotated)

Committer:: narshu
Date:: Fri Jun 15 20:40:17 2012 +0000
Revision:: 0:42026f893a2d

Who changed what in which revision?

User	Revision	Line number	New contents of line
narshu	0:42026f893a2d	1	#include "motion.h"
narshu	0:42026f893a2d	2	#include "geometryfuncs.h"
narshu	0:42026f893a2d	3	#include "system.h"
narshu	0:42026f893a2d	4	#include "PID.h"
narshu	0:42026f893a2d	5
narshu	0:42026f893a2d	6	AnalogIn ObsAvoidPin(p20);
narshu	0:42026f893a2d	7
narshu	0:42026f893a2d	8	Motion::Motion(Motors &motorsin, AI &aiin, Kalman &kalmanin):
narshu	0:42026f893a2d	9	thr_motion(mtwrapper,this,osPriorityNormal,1024),
narshu	0:42026f893a2d	10	motors(motorsin),
narshu	0:42026f893a2d	11	ai(aiin),
narshu	0:42026f893a2d	12	kalman(kalmanin) { }
narshu	0:42026f893a2d	13
narshu	0:42026f893a2d	14	// motion control thread ------------------------
narshu	0:42026f893a2d	15	void Motion::motion_thread() {
narshu	0:42026f893a2d	16	motors.resetEncoders();
narshu	0:42026f893a2d	17	motors.setSpeed(5,5);
narshu	0:42026f893a2d	18	motors.stop();
narshu	0:42026f893a2d	19	// Thread::wait(1500);
narshu	0:42026f893a2d	20	//ai.thr_AI.signal_set(0x01);
narshu	0:42026f893a2d	21
narshu	0:42026f893a2d	22	//PID declare
narshu	0:42026f893a2d	23	PID PIDControllerMotorTheta2(FWD_MOVE_P, FWD_MOVE_P/10.0f, 0.000005, MOTION_UPDATE_PERIOD/1000.0f); //Going forward
narshu	0:42026f893a2d	24	PID PIDControllerMotorTheta(SPIN_MOVE_P, SPIN_MOVE_P/10.0f, 0.000005, MOTION_UPDATE_PERIOD/1000.0f); //Spinning on the spot
narshu	0:42026f893a2d	25
narshu	0:42026f893a2d	26	//PID Initialisation
narshu	0:42026f893a2d	27	PIDControllerMotorTheta2.setMode(MANUAL_MODE);
narshu	0:42026f893a2d	28	PIDControllerMotorTheta.setMode(MANUAL_MODE);
narshu	0:42026f893a2d	29
narshu	0:42026f893a2d	30	PIDControllerMotorTheta2.setBias(0);
narshu	0:42026f893a2d	31	PIDControllerMotorTheta.setBias(0);
narshu	0:42026f893a2d	32
narshu	0:42026f893a2d	33	PIDControllerMotorTheta2.setOutputLimits(-1, 1);
narshu	0:42026f893a2d	34	PIDControllerMotorTheta.setOutputLimits(-1, 1);
narshu	0:42026f893a2d	35
narshu	0:42026f893a2d	36	PIDControllerMotorTheta2.setInputLimits(-PI, PI);
narshu	0:42026f893a2d	37	PIDControllerMotorTheta.setInputLimits(-PI, PI);
narshu	0:42026f893a2d	38
narshu	0:42026f893a2d	39	PIDControllerMotorTheta.setSetPoint(0);
narshu	0:42026f893a2d	40	PIDControllerMotorTheta2.setSetPoint(0);
narshu	0:42026f893a2d	41
narshu	0:42026f893a2d	42	float currX, currY,currTheta;
narshu	0:42026f893a2d	43	float speedL,speedR;
narshu	0:42026f893a2d	44	float diffDir;
narshu	0:42026f893a2d	45	float xBuffer, yBuffer;
narshu	0:42026f893a2d	46	float xOriginalBuffer = 0, yOriginalBuffer = 0;
narshu	0:42026f893a2d	47	int initiateFlag = 1;
narshu	0:42026f893a2d	48	int dontSpinFlag = 0;
narshu	0:42026f893a2d	49	int atTargetFlag = 0;
narshu	0:42026f893a2d	50	int atTargetDirectionFlag = 0;
narshu	0:42026f893a2d	51
narshu	0:42026f893a2d	52	while (1) {
narshu	0:42026f893a2d	53	//kalman.statelock.lock();
narshu	0:42026f893a2d	54	if (ai.flag_terminate) {
narshu	0:42026f893a2d	55	// stops motors and teminates the thread
narshu	0:42026f893a2d	56	motors.stop();
narshu	0:42026f893a2d	57	//motors.coastStop();
narshu	0:42026f893a2d	58	terminate();
narshu	0:42026f893a2d	59	}
narshu	0:42026f893a2d	60
narshu	0:42026f893a2d	61	// stops motor
narshu	0:42026f893a2d	62	if ((ai.flag_motorStop) \|\| (ObsAvoidPin > 0.4)) {
narshu	0:42026f893a2d	63	motors.stop();
narshu	0:42026f893a2d	64	} else if (ai.flag_manOverride) {
narshu	0:42026f893a2d	65
narshu	0:42026f893a2d	66	} else {
narshu	0:42026f893a2d	67
narshu	0:42026f893a2d	68
narshu	0:42026f893a2d	69	// get kalman localization estimate ------------------------
narshu	0:42026f893a2d	70	kalman.statelock.lock();
narshu	0:42026f893a2d	71	currX = kalman.X(0)*1000.0f;
narshu	0:42026f893a2d	72	currY = kalman.X(1)*1000.0f;
narshu	0:42026f893a2d	73	currTheta = kalman.X(2);
narshu	0:42026f893a2d	74	kalman.statelock.unlock();
narshu	0:42026f893a2d	75
narshu	0:42026f893a2d	76	// make a local copy of the target
narshu	0:42026f893a2d	77	ai.targetlock.lock();
narshu	0:42026f893a2d	78	AI::Target loctarget = ai.gettarget();
narshu	0:42026f893a2d	79	ai.targetlock.unlock();
narshu	0:42026f893a2d	80	/*
narshu	0:42026f893a2d	81	//PID Tuning Code
narshu	0:42026f893a2d	82	if (pc.readable() == 1) {
narshu	0:42026f893a2d	83	float cmd;
narshu	0:42026f893a2d	84	pc.scanf("%f", &cmd);
narshu	0:42026f893a2d	85	//Tune PID referece
narshu	0:42026f893a2d	86	PIDControllerMotorTheta2.setTunings(cmd, 0, 0);
narshu	0:42026f893a2d	87	}
narshu	0:42026f893a2d	88	*/
narshu	0:42026f893a2d	89
narshu	0:42026f893a2d	90
narshu	0:42026f893a2d	91	if (initiateFlag == 1) {
narshu	0:42026f893a2d	92	xOriginalBuffer = currX;
narshu	0:42026f893a2d	93	yOriginalBuffer = currY;
narshu	0:42026f893a2d	94
narshu	0:42026f893a2d	95	xBuffer = ai.gettarget().x;
narshu	0:42026f893a2d	96	yBuffer = ai.gettarget().y;
narshu	0:42026f893a2d	97
narshu	0:42026f893a2d	98	initiateFlag = 0;
narshu	0:42026f893a2d	99	}
narshu	0:42026f893a2d	100
narshu	0:42026f893a2d	101	if (xBuffer != loctarget.x \|\| yBuffer != loctarget.y) {
narshu	0:42026f893a2d	102	//target changed
narshu	0:42026f893a2d	103	//update xOriginal and yOriginal buffers
narshu	0:42026f893a2d	104	xOriginalBuffer = currX;
narshu	0:42026f893a2d	105	yOriginalBuffer = currY;
narshu	0:42026f893a2d	106
narshu	0:42026f893a2d	107	xBuffer = loctarget.x;
narshu	0:42026f893a2d	108	yBuffer = loctarget.y;
narshu	0:42026f893a2d	109
narshu	0:42026f893a2d	110	atTargetFlag = 0;
narshu	0:42026f893a2d	111	atTargetDirectionFlag = 0;
narshu	0:42026f893a2d	112
narshu	0:42026f893a2d	113	}
narshu	0:42026f893a2d	114
narshu	0:42026f893a2d	115	// check if target reached ----------------------------------
narshu	0:42026f893a2d	116	if (atTargetFlag \|\| hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
narshu	0:42026f893a2d	117
narshu	0:42026f893a2d	118	if (atTargetFlag == 0) {
narshu	0:42026f893a2d	119	motors.stop();
narshu	0:42026f893a2d	120	Thread::wait(100);
narshu	0:42026f893a2d	121	}
narshu	0:42026f893a2d	122
narshu	0:42026f893a2d	123
narshu	0:42026f893a2d	124	if (hypot(currX - loctarget.x, currY - loctarget.y) < POSITION_TOR) {
narshu	0:42026f893a2d	125	atTargetFlag = 1;
narshu	0:42026f893a2d	126	}
narshu	0:42026f893a2d	127	OLED4 = 1;
narshu	0:42026f893a2d	128
narshu	0:42026f893a2d	129	diffDir = rectifyAng(currTheta - loctarget.theta);
narshu	0:42026f893a2d	130	//diffSpeed = diffDir / PI;
narshu	0:42026f893a2d	131
narshu	0:42026f893a2d	132	PIDControllerMotorTheta.setProcessValue(diffDir);
narshu	0:42026f893a2d	133	float tempPidVar = PIDControllerMotorTheta.compute();
narshu	0:42026f893a2d	134	motors.setSpeed( -int(tempPidVarMOVE_SPEED), int(tempPidVarMOVE_SPEED));
narshu	0:42026f893a2d	135
narshu	0:42026f893a2d	136	if (abs(diffDir) < ANGLE_TOR) {
narshu	0:42026f893a2d	137
narshu	0:42026f893a2d	138	if (atTargetDirectionFlag == 0) {
narshu	0:42026f893a2d	139	ai.thr_AI.signal_set(0x01);
narshu	0:42026f893a2d	140	atTargetDirectionFlag = 1;
narshu	0:42026f893a2d	141	}
narshu	0:42026f893a2d	142
narshu	0:42026f893a2d	143	/*
narshu	0:42026f893a2d	144	if (!loctarget.reached) {
narshu	0:42026f893a2d	145	static int counter = 10;
narshu	0:42026f893a2d	146	// guarding counter for reaching target
narshu	0:42026f893a2d	147	if (counter-- == 0) {
narshu	0:42026f893a2d	148	counter = 10;
narshu	0:42026f893a2d	149	ai.target.reached = true;
narshu	0:42026f893a2d	150	ai.thr_AI.signal_set(0x01);
narshu	0:42026f893a2d	151
narshu	0:42026f893a2d	152	}
narshu	0:42026f893a2d	153	}
narshu	0:42026f893a2d	154	*/
narshu	0:42026f893a2d	155	}
narshu	0:42026f893a2d	156	}
narshu	0:42026f893a2d	157
narshu	0:42026f893a2d	158	// adjust motion to reach target ----------------------------
narshu	0:42026f893a2d	159	else {
narshu	0:42026f893a2d	160
narshu	0:42026f893a2d	161	OLED3 = 1;
narshu	0:42026f893a2d	162
narshu	0:42026f893a2d	163	/*
narshu	0:42026f893a2d	164	if ((hypot(xOriginalBuffer - loctarget.x, yOriginalBuffer - loctarget.y) - hypot(xOriginalBuffer - currX, yOriginalBuffer - currY)) < 0) {
narshu	0:42026f893a2d	165	loctarget.facing = !loctarget.facing;
narshu	0:42026f893a2d	166	dontSpinFlag = 1;
narshu	0:42026f893a2d	167	} else {
narshu	0:42026f893a2d	168	dontSpinFlag = 0;
narshu	0:42026f893a2d	169	}
narshu	0:42026f893a2d	170	*/
narshu	0:42026f893a2d	171
narshu	0:42026f893a2d	172	// calc direction to target
narshu	0:42026f893a2d	173	float targetDir = atan2(loctarget.y - currY, loctarget.x - currX);
narshu	0:42026f893a2d	174	if (!loctarget.facing) targetDir = targetDir + PI;
narshu	0:42026f893a2d	175
narshu	0:42026f893a2d	176	//Angle differene in -PI to PI
narshu	0:42026f893a2d	177	diffDir = rectifyAng(currTheta - targetDir);
narshu	0:42026f893a2d	178
narshu	0:42026f893a2d	179	//Set PID process variable
narshu	0:42026f893a2d	180	PIDControllerMotorTheta.setProcessValue(diffDir);
narshu	0:42026f893a2d	181	PIDControllerMotorTheta2.setProcessValue(diffDir);
narshu	0:42026f893a2d	182
narshu	0:42026f893a2d	183	//if diffDIr is neg, spin right
narshu	0:42026f893a2d	184	//if diffDir is pos, spin left
narshu	0:42026f893a2d	185
narshu	0:42026f893a2d	186	if ((abs(diffDir) > ANGLE_TOR*4) && (dontSpinFlag == 0)) { //roughly 32 degrees
narshu	0:42026f893a2d	187	//ANGLE_TOR*4
narshu	0:42026f893a2d	188	float tempPidVar = PIDControllerMotorTheta.compute();
narshu	0:42026f893a2d	189	motors.setSpeed( -int(tempPidVarMOVE_SPEED), int(tempPidVarMOVE_SPEED));
narshu	0:42026f893a2d	190	//pc.printf("spin,%f\n",diffDir);
narshu	0:42026f893a2d	191
narshu	0:42026f893a2d	192	} else {
narshu	0:42026f893a2d	193
narshu	0:42026f893a2d	194	float tempPidVar = PIDControllerMotorTheta2.compute();
narshu	0:42026f893a2d	195	float MoveSpeedLimiter = 1;
narshu	0:42026f893a2d	196	//pc.printf("turn,%f\n",diffDir);
narshu	0:42026f893a2d	197
narshu	0:42026f893a2d	198	float distanceToX = (float)abs(currX - loctarget.x);
narshu	0:42026f893a2d	199	float distanceToY = (float)abs(currY - loctarget.y);
narshu	0:42026f893a2d	200
narshu	0:42026f893a2d	201	float distanceToTarget = hypot(distanceToX, distanceToY);
narshu	0:42026f893a2d	202
narshu	0:42026f893a2d	203	if ((distanceToTarget < 400) && (distanceToTarget > 200) && motors.accelerationRegister == 1) {
narshu	0:42026f893a2d	204	MoveSpeedLimiter = (distanceToTarget)/400;
narshu	0:42026f893a2d	205	} else if (distanceToTarget <= 200) {
narshu	0:42026f893a2d	206	MoveSpeedLimiter = 0.5;
narshu	0:42026f893a2d	207	}
narshu	0:42026f893a2d	208
narshu	0:42026f893a2d	209
narshu	0:42026f893a2d	210
narshu	0:42026f893a2d	211
narshu	0:42026f893a2d	212	// calculte the motor speeds
narshu	0:42026f893a2d	213	if (tempPidVar >= 0) {
narshu	0:42026f893a2d	214	//turn left
narshu	0:42026f893a2d	215	speedL = (1-abs(tempPidVar))MOVE_SPEEDMoveSpeedLimiter;
narshu	0:42026f893a2d	216	speedR = MOVE_SPEED*MoveSpeedLimiter;
narshu	0:42026f893a2d	217
narshu	0:42026f893a2d	218	} else {
narshu	0:42026f893a2d	219	//turn right
narshu	0:42026f893a2d	220	speedR = (1-abs(tempPidVar))MOVE_SPEEDMoveSpeedLimiter;
narshu	0:42026f893a2d	221	speedL = MOVE_SPEED*MoveSpeedLimiter;
narshu	0:42026f893a2d	222	}
narshu	0:42026f893a2d	223
narshu	0:42026f893a2d	224
narshu	0:42026f893a2d	225
narshu	0:42026f893a2d	226
narshu	0:42026f893a2d	227	if (loctarget.facing) motors.setSpeed( int(speedL), int(speedR));
narshu	0:42026f893a2d	228	else motors.setSpeed( -int(speedR), -int(speedL));
narshu	0:42026f893a2d	229
narshu	0:42026f893a2d	230	}
narshu	0:42026f893a2d	231	}
narshu	0:42026f893a2d	232	}
narshu	0:42026f893a2d	233	//kalman.statelock.unlock();
narshu	0:42026f893a2d	234	// wait
narshu	0:42026f893a2d	235	Thread::wait(MOTION_UPDATE_PERIOD);
narshu	0:42026f893a2d	236	}
narshu	0:42026f893a2d	237	}

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	28 Apr 2012
Imports:	1
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	0
Followers:	1

Motion/motion.cpp@0:42026f893a2d, 2012-06-15 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning