Eurobot_2012_Secondary - Eurobot_2012_Secondary

Users » narshu » Code » Eurobot_2012_Secondary

Shuto Naruse / Mbed 2 deprecated Eurobot_2012_Secondary

Eurobot_2012_Secondary

main.cpp@0:fbfafa6bf5f9, 2012-04-20 (annotated)

Committer:: narshu
Date:: Fri Apr 20 21:32:24 2012 +0000
Revision:: 0:fbfafa6bf5f9

Who changed what in which revision?

User	Revision	Line number	New contents of line
narshu	0:fbfafa6bf5f9	1	#include "mbed.h"
narshu	0:fbfafa6bf5f9	2	#include "rtos.h"
narshu	0:fbfafa6bf5f9	3	#include "TSH.h"
narshu	0:fbfafa6bf5f9	4	#include "Kalman.h"
narshu	0:fbfafa6bf5f9	5	#include "globals.h"
narshu	0:fbfafa6bf5f9	6	#include "motors.h"
narshu	0:fbfafa6bf5f9	7	#include "math.h"
narshu	0:fbfafa6bf5f9	8	#include "system.h"
narshu	0:fbfafa6bf5f9	9	#include "geometryfuncs.h"
narshu	0:fbfafa6bf5f9	10
narshu	0:fbfafa6bf5f9	11	//#include <iostream>
narshu	0:fbfafa6bf5f9	12
narshu	0:fbfafa6bf5f9	13	//Interface declaration
narshu	0:fbfafa6bf5f9	14	//I2C i2c(p28, p27); // sda, scl
narshu	0:fbfafa6bf5f9	15	TSI2C i2c(p28, p27);
narshu	0:fbfafa6bf5f9	16	Serial pc(USBTX, USBRX); // tx, rx
narshu	0:fbfafa6bf5f9	17	Serial IRturret(p13, p14);
narshu	0:fbfafa6bf5f9	18
narshu	0:fbfafa6bf5f9	19	DigitalOut OLED1(LED1);
narshu	0:fbfafa6bf5f9	20	DigitalOut OLED2(LED2);
narshu	0:fbfafa6bf5f9	21	DigitalOut OLED3(LED3);
narshu	0:fbfafa6bf5f9	22	DigitalOut OLED4(LED4);
narshu	0:fbfafa6bf5f9	23
narshu	0:fbfafa6bf5f9	24	Motors motors(i2c);
narshu	0:fbfafa6bf5f9	25	Kalman kalman(motors);
narshu	0:fbfafa6bf5f9	26
narshu	0:fbfafa6bf5f9	27	float targetX = 1000, targetY = 1000, targetTheta = 0;
narshu	0:fbfafa6bf5f9	28
narshu	0:fbfafa6bf5f9	29	// bytes packing/unpacking for IR turret serial comm
narshu	0:fbfafa6bf5f9	30	union IRValue_t {
narshu	0:fbfafa6bf5f9	31	float IR_floats[3];
narshu	0:fbfafa6bf5f9	32	int IR_ints[3];
narshu	0:fbfafa6bf5f9	33	unsigned char IR_chars[12];
narshu	0:fbfafa6bf5f9	34	} IRValues;
narshu	0:fbfafa6bf5f9	35
narshu	0:fbfafa6bf5f9	36	char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC};
narshu	0:fbfafa6bf5f9	37	int Alignment_ptr = 0;
narshu	0:fbfafa6bf5f9	38	bool data_flag = false;
narshu	0:fbfafa6bf5f9	39	int buff_pointer = 0;
narshu	0:fbfafa6bf5f9	40	bool angleInit = false;
narshu	0:fbfafa6bf5f9	41	float angleOffset = 0;
narshu	0:fbfafa6bf5f9	42
narshu	0:fbfafa6bf5f9	43	void vIRValueISR (void);
narshu	0:fbfafa6bf5f9	44	void vKalmanInit(void);
narshu	0:fbfafa6bf5f9	45
narshu	0:fbfafa6bf5f9	46	//TODO mutex on kalman state, and on motor commands (i.e. on the i2c bus)
narshu	0:fbfafa6bf5f9	47	//NOTE! Recieving data with RF12B now DISABLED due to interferance with rtos!
narshu	0:fbfafa6bf5f9	48
narshu	0:fbfafa6bf5f9	49
narshu	0:fbfafa6bf5f9	50	void vMotorThread(void const *argument);
narshu	0:fbfafa6bf5f9	51	void vPrintState(void const *argument);
narshu	0:fbfafa6bf5f9	52	void ai_thread (void const *argument);
narshu	0:fbfafa6bf5f9	53	void motion_thread(void const *argument);
narshu	0:fbfafa6bf5f9	54
narshu	0:fbfafa6bf5f9	55
narshu	0:fbfafa6bf5f9	56	float getAngle (float x, float y);
narshu	0:fbfafa6bf5f9	57	void getIRValue(void const *argument);
narshu	0:fbfafa6bf5f9	58
narshu	0:fbfafa6bf5f9	59	// Thread pointers
narshu	0:fbfafa6bf5f9	60	Thread *AI_Thread_Ptr;
narshu	0:fbfafa6bf5f9	61	Thread *Motion_Thread_Ptr;
narshu	0:fbfafa6bf5f9	62
narshu	0:fbfafa6bf5f9	63	Mutex targetlock;
narshu	0:fbfafa6bf5f9	64	bool flag_terminate = false;
narshu	0:fbfafa6bf5f9	65
narshu	0:fbfafa6bf5f9	66	float temp = 0;
narshu	0:fbfafa6bf5f9	67
narshu	0:fbfafa6bf5f9	68	//Main loop
narshu	0:fbfafa6bf5f9	69	int main() {
narshu	0:fbfafa6bf5f9	70	pc.baud(115200);
narshu	0:fbfafa6bf5f9	71	IRturret.baud(115200);
narshu	0:fbfafa6bf5f9	72	IRturret.format(8,Serial::Odd,1);
narshu	0:fbfafa6bf5f9	73	IRturret.attach(&vIRValueISR,Serial::RxIrq);
narshu	0:fbfafa6bf5f9	74	vKalmanInit();
narshu	0:fbfafa6bf5f9	75
narshu	0:fbfafa6bf5f9	76	//Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
narshu	0:fbfafa6bf5f9	77	Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
narshu	0:fbfafa6bf5f9	78
narshu	0:fbfafa6bf5f9	79	Thread thr_AI(ai_thread,NULL,osPriorityNormal,1024);
narshu	0:fbfafa6bf5f9	80	Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024);
narshu	0:fbfafa6bf5f9	81	AI_Thread_Ptr = &thr_AI;
narshu	0:fbfafa6bf5f9	82	Motion_Thread_Ptr = &thr_motion;
narshu	0:fbfafa6bf5f9	83
narshu	0:fbfafa6bf5f9	84	//measure cpu usage. output updated once per second to symbol cpupercent
narshu	0:fbfafa6bf5f9	85	//Thread mCPUthread(measureCPUidle, NULL, osPriorityIdle, 1024); //check if stack overflow with such a small staack
narshu	0:fbfafa6bf5f9	86
narshu	0:fbfafa6bf5f9	87
narshu	0:fbfafa6bf5f9	88	pc.printf("We got to main! ;D\r\n");
narshu	0:fbfafa6bf5f9	89
narshu	0:fbfafa6bf5f9	90	//REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
narshu	0:fbfafa6bf5f9	91	while (1) {
narshu	0:fbfafa6bf5f9	92	// do nothing
narshu	0:fbfafa6bf5f9	93	Thread::wait(osWaitForever);
narshu	0:fbfafa6bf5f9	94	}
narshu	0:fbfafa6bf5f9	95	}
narshu	0:fbfafa6bf5f9	96
narshu	0:fbfafa6bf5f9	97
narshu	0:fbfafa6bf5f9	98	void vMotorThread(void const *argument) {
narshu	0:fbfafa6bf5f9	99	motors.resetEncoders();
narshu	0:fbfafa6bf5f9	100	while (1) {
narshu	0:fbfafa6bf5f9	101	motors.setSpeed(20,20);
narshu	0:fbfafa6bf5f9	102	Thread::wait(2000);
narshu	0:fbfafa6bf5f9	103	motors.stop();
narshu	0:fbfafa6bf5f9	104	Thread::wait(5000);
narshu	0:fbfafa6bf5f9	105	motors.setSpeed(-20,-20);
narshu	0:fbfafa6bf5f9	106	Thread::wait(2000);
narshu	0:fbfafa6bf5f9	107	motors.stop();
narshu	0:fbfafa6bf5f9	108	Thread::wait(5000);
narshu	0:fbfafa6bf5f9	109	motors.setSpeed(-20,20);
narshu	0:fbfafa6bf5f9	110	Thread::wait(2000);
narshu	0:fbfafa6bf5f9	111	motors.stop();
narshu	0:fbfafa6bf5f9	112	Thread::wait(5000);
narshu	0:fbfafa6bf5f9	113	motors.setSpeed(20,-20);
narshu	0:fbfafa6bf5f9	114	Thread::wait(2000);
narshu	0:fbfafa6bf5f9	115	motors.stop();
narshu	0:fbfafa6bf5f9	116	Thread::wait(5000);
narshu	0:fbfafa6bf5f9	117	}
narshu	0:fbfafa6bf5f9	118	}
narshu	0:fbfafa6bf5f9	119
narshu	0:fbfafa6bf5f9	120
narshu	0:fbfafa6bf5f9	121
narshu	0:fbfafa6bf5f9	122	void vPrintState(void const *argument) {
narshu	0:fbfafa6bf5f9	123	float state[3];
narshu	0:fbfafa6bf5f9	124	float SonarMeasures[3];
narshu	0:fbfafa6bf5f9	125	float IRMeasures[3];
narshu	0:fbfafa6bf5f9	126
narshu	0:fbfafa6bf5f9	127
narshu	0:fbfafa6bf5f9	128	while (1) {
narshu	0:fbfafa6bf5f9	129	kalman.statelock.lock();
narshu	0:fbfafa6bf5f9	130	state[0] = kalman.X(0);
narshu	0:fbfafa6bf5f9	131	state[1] = kalman.X(1);
narshu	0:fbfafa6bf5f9	132	state[2] = kalman.X(2);
narshu	0:fbfafa6bf5f9	133	SonarMeasures[0] = kalman.SonarMeasures[0];
narshu	0:fbfafa6bf5f9	134	SonarMeasures[1] = kalman.SonarMeasures[1];
narshu	0:fbfafa6bf5f9	135	SonarMeasures[2] = kalman.SonarMeasures[2];
narshu	0:fbfafa6bf5f9	136	IRMeasures[0] = kalman.IRMeasures[0];
narshu	0:fbfafa6bf5f9	137	IRMeasures[1] = kalman.IRMeasures[1];
narshu	0:fbfafa6bf5f9	138	IRMeasures[2] = kalman.IRMeasures[2];
narshu	0:fbfafa6bf5f9	139	kalman.statelock.unlock();
narshu	0:fbfafa6bf5f9	140	pc.printf("\r\n");
narshu	0:fbfafa6bf5f9	141	pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]);
narshu	0:fbfafa6bf5f9	142	pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]);
narshu	0:fbfafa6bf5f9	143	pc.printf("IR : %0.4f %0.4f %0.4f \r\n",IRMeasures[0]180/PI,IRMeasures[1]180/PI,IRMeasures[2]*180/PI);
narshu	0:fbfafa6bf5f9	144	pc.printf("Angle_Offset: %0.4f \r\n",angleOffset*180/PI);
narshu	0:fbfafa6bf5f9	145	Thread::wait(100);
narshu	0:fbfafa6bf5f9	146	}
narshu	0:fbfafa6bf5f9	147	}
narshu	0:fbfafa6bf5f9	148
narshu	0:fbfafa6bf5f9	149
narshu	0:fbfafa6bf5f9	150	// AI thread ------------------------------------
narshu	0:fbfafa6bf5f9	151	void ai_thread (void const *argument) {
narshu	0:fbfafa6bf5f9	152	// goes to the mid
narshu	0:fbfafa6bf5f9	153	Thread::signal_wait(0x01);
narshu	0:fbfafa6bf5f9	154	targetlock.lock();
narshu	0:fbfafa6bf5f9	155	targetX = 1500;
narshu	0:fbfafa6bf5f9	156	targetY = 1000;
narshu	0:fbfafa6bf5f9	157	targetTheta = PI/2;
narshu	0:fbfafa6bf5f9	158	targetlock.unlock();
narshu	0:fbfafa6bf5f9	159
narshu	0:fbfafa6bf5f9	160	// left roll
narshu	0:fbfafa6bf5f9	161	Thread::signal_wait(0x01);
narshu	0:fbfafa6bf5f9	162	targetlock.lock();
narshu	0:fbfafa6bf5f9	163	targetX = 500;
narshu	0:fbfafa6bf5f9	164	targetY = 1700;
narshu	0:fbfafa6bf5f9	165	targetTheta = PI/2;
narshu	0:fbfafa6bf5f9	166	targetlock.unlock();
narshu	0:fbfafa6bf5f9	167
narshu	0:fbfafa6bf5f9	168	// mid
narshu	0:fbfafa6bf5f9	169	Thread::signal_wait(0x01);
narshu	0:fbfafa6bf5f9	170	targetlock.lock();
narshu	0:fbfafa6bf5f9	171	targetX = 1500;
narshu	0:fbfafa6bf5f9	172	targetY = 1000;
narshu	0:fbfafa6bf5f9	173	targetTheta = PI/2;
narshu	0:fbfafa6bf5f9	174	targetlock.unlock();
narshu	0:fbfafa6bf5f9	175
narshu	0:fbfafa6bf5f9	176	// map
narshu	0:fbfafa6bf5f9	177	Thread::signal_wait(0x01);
narshu	0:fbfafa6bf5f9	178	targetlock.lock();
narshu	0:fbfafa6bf5f9	179	targetX = 1500;
narshu	0:fbfafa6bf5f9	180	targetY = 1700;
narshu	0:fbfafa6bf5f9	181	targetTheta = PI/2;
narshu	0:fbfafa6bf5f9	182	targetlock.unlock();
narshu	0:fbfafa6bf5f9	183
narshu	0:fbfafa6bf5f9	184	// mid
narshu	0:fbfafa6bf5f9	185	Thread::signal_wait(0x01);
narshu	0:fbfafa6bf5f9	186	targetlock.lock();
narshu	0:fbfafa6bf5f9	187	targetX = 1500;
narshu	0:fbfafa6bf5f9	188	targetY = 1000;
narshu	0:fbfafa6bf5f9	189	targetTheta = -PI/2;
narshu	0:fbfafa6bf5f9	190	targetlock.unlock();
narshu	0:fbfafa6bf5f9	191
narshu	0:fbfafa6bf5f9	192	// home
narshu	0:fbfafa6bf5f9	193	Thread::signal_wait(0x01);
narshu	0:fbfafa6bf5f9	194	targetlock.lock();
narshu	0:fbfafa6bf5f9	195	targetX = 500;
narshu	0:fbfafa6bf5f9	196	targetY = 500;
narshu	0:fbfafa6bf5f9	197	targetTheta = 0;
narshu	0:fbfafa6bf5f9	198	targetlock.unlock();
narshu	0:fbfafa6bf5f9	199
narshu	0:fbfafa6bf5f9	200	Thread::signal_wait(0x01);
narshu	0:fbfafa6bf5f9	201	flag_terminate = true;
narshu	0:fbfafa6bf5f9	202	//OLED3 = true;
narshu	0:fbfafa6bf5f9	203
narshu	0:fbfafa6bf5f9	204	while (true) {
narshu	0:fbfafa6bf5f9	205	Thread::wait(osWaitForever);
narshu	0:fbfafa6bf5f9	206	}
narshu	0:fbfafa6bf5f9	207	}
narshu	0:fbfafa6bf5f9	208
narshu	0:fbfafa6bf5f9	209	// motion control thread ------------------------
narshu	0:fbfafa6bf5f9	210	void motion_thread(void const *argument) {
narshu	0:fbfafa6bf5f9	211	motors.resetEncoders();
narshu	0:fbfafa6bf5f9	212	motors.setSpeed(MOVE_SPEED/2,MOVE_SPEED/2);
narshu	0:fbfafa6bf5f9	213	Thread::wait(1000);
narshu	0:fbfafa6bf5f9	214	motors.stop();
narshu	0:fbfafa6bf5f9	215	(*AI_Thread_Ptr).signal_set(0x01);
narshu	0:fbfafa6bf5f9	216
narshu	0:fbfafa6bf5f9	217
narshu	0:fbfafa6bf5f9	218
narshu	0:fbfafa6bf5f9	219	float currX, currY,currTheta;
narshu	0:fbfafa6bf5f9	220	float speedL,speedR;
narshu	0:fbfafa6bf5f9	221	float diffDir,diffSpeed;
narshu	0:fbfafa6bf5f9	222	float lastdiffSpeed = 0;
narshu	0:fbfafa6bf5f9	223
narshu	0:fbfafa6bf5f9	224	while (1) {
narshu	0:fbfafa6bf5f9	225	if (flag_terminate) {
narshu	0:fbfafa6bf5f9	226	terminate();
narshu	0:fbfafa6bf5f9	227	}
narshu	0:fbfafa6bf5f9	228
narshu	0:fbfafa6bf5f9	229	// get kalman localization estimate ------------------------
narshu	0:fbfafa6bf5f9	230	kalman.statelock.lock();
narshu	0:fbfafa6bf5f9	231	currX = kalman.X(0)*1000.0f;
narshu	0:fbfafa6bf5f9	232	currY = kalman.X(1)*1000.0f;
narshu	0:fbfafa6bf5f9	233	currTheta = kalman.X(2);
narshu	0:fbfafa6bf5f9	234	kalman.statelock.unlock();
narshu	0:fbfafa6bf5f9	235
narshu	0:fbfafa6bf5f9	236
narshu	0:fbfafa6bf5f9	237	// check if target reached ----------------------------------
narshu	0:fbfafa6bf5f9	238	if ( ( abs(currX - targetX) < POSITION_TOR )
narshu	0:fbfafa6bf5f9	239	&&( abs(currY - targetY) < POSITION_TOR )
narshu	0:fbfafa6bf5f9	240	) {
narshu	0:fbfafa6bf5f9	241
narshu	0:fbfafa6bf5f9	242	diffDir = rectifyAng(currTheta - targetTheta);
narshu	0:fbfafa6bf5f9	243	diffSpeed = diffDir / PI;
narshu	0:fbfafa6bf5f9	244
narshu	0:fbfafa6bf5f9	245	if (abs(diffDir) > ANGLE_TOR) {
narshu	0:fbfafa6bf5f9	246	if (abs(diffSpeed) < 0.5) {
narshu	0:fbfafa6bf5f9	247	diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
narshu	0:fbfafa6bf5f9	248	}
narshu	0:fbfafa6bf5f9	249	motors.setSpeed( int(diffSpeedMOVE_SPEED), -int(diffSpeedMOVE_SPEED));
narshu	0:fbfafa6bf5f9	250
narshu	0:fbfafa6bf5f9	251
narshu	0:fbfafa6bf5f9	252	} else {
narshu	0:fbfafa6bf5f9	253	motors.stop();
narshu	0:fbfafa6bf5f9	254	Thread::wait(4000);
narshu	0:fbfafa6bf5f9	255	(*AI_Thread_Ptr).signal_set(0x01);
narshu	0:fbfafa6bf5f9	256	}
narshu	0:fbfafa6bf5f9	257	}
narshu	0:fbfafa6bf5f9	258
narshu	0:fbfafa6bf5f9	259	// adjust motion to reach target ----------------------------
narshu	0:fbfafa6bf5f9	260	else {
narshu	0:fbfafa6bf5f9	261
narshu	0:fbfafa6bf5f9	262	// calc direction to target
narshu	0:fbfafa6bf5f9	263	float targetDir = atan2(targetY - currY, targetX - currX);
narshu	0:fbfafa6bf5f9	264
narshu	0:fbfafa6bf5f9	265
narshu	0:fbfafa6bf5f9	266	diffDir = rectifyAng(currTheta - targetDir);
narshu	0:fbfafa6bf5f9	267	diffSpeed = diffDir / PI;
narshu	0:fbfafa6bf5f9	268
narshu	0:fbfafa6bf5f9	269	if (abs(diffDir) > ANGLE_TOR*2) {
narshu	0:fbfafa6bf5f9	270	if (abs(diffSpeed) < 0.5) {
narshu	0:fbfafa6bf5f9	271	diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
narshu	0:fbfafa6bf5f9	272	}
narshu	0:fbfafa6bf5f9	273	motors.setSpeed( int(diffSpeedMOVE_SPEED), -int(diffSpeedMOVE_SPEED));
narshu	0:fbfafa6bf5f9	274	} else {
narshu	0:fbfafa6bf5f9	275
narshu	0:fbfafa6bf5f9	276
narshu	0:fbfafa6bf5f9	277	if (abs(diffSpeed-lastdiffSpeed) > MAX_STEP_RATIO ) {
narshu	0:fbfafa6bf5f9	278	if (diffSpeed-lastdiffSpeed >= 0) {
narshu	0:fbfafa6bf5f9	279	diffSpeed = lastdiffSpeed + MAX_STEP_RATIO;
narshu	0:fbfafa6bf5f9	280	} else {
narshu	0:fbfafa6bf5f9	281	diffSpeed = lastdiffSpeed - MAX_STEP_RATIO;
narshu	0:fbfafa6bf5f9	282	}
narshu	0:fbfafa6bf5f9	283	}
narshu	0:fbfafa6bf5f9	284	lastdiffSpeed = diffSpeed;
narshu	0:fbfafa6bf5f9	285
narshu	0:fbfafa6bf5f9	286	// calculte the motor speeds
narshu	0:fbfafa6bf5f9	287	if (diffSpeed <= 0) {
narshu	0:fbfafa6bf5f9	288	speedL = (1-2abs(diffSpeed))MOVE_SPEED;
narshu	0:fbfafa6bf5f9	289	speedR = MOVE_SPEED;
narshu	0:fbfafa6bf5f9	290
narshu	0:fbfafa6bf5f9	291	} else {
narshu	0:fbfafa6bf5f9	292	speedR = (1-2abs(diffSpeed))MOVE_SPEED;
narshu	0:fbfafa6bf5f9	293	speedL = MOVE_SPEED;
narshu	0:fbfafa6bf5f9	294	}
narshu	0:fbfafa6bf5f9	295
narshu	0:fbfafa6bf5f9	296	motors.setSpeed( int(speedL), int(speedR));
narshu	0:fbfafa6bf5f9	297	}
narshu	0:fbfafa6bf5f9	298	}
narshu	0:fbfafa6bf5f9	299
narshu	0:fbfafa6bf5f9	300	// wait
narshu	0:fbfafa6bf5f9	301	Thread::wait(MOTION_UPDATE_PERIOD);
narshu	0:fbfafa6bf5f9	302	}
narshu	0:fbfafa6bf5f9	303	}
narshu	0:fbfafa6bf5f9	304
narshu	0:fbfafa6bf5f9	305	void vIRValueISR (void) {
narshu	0:fbfafa6bf5f9	306
narshu	0:fbfafa6bf5f9	307	OLED3 = !OLED3;
narshu	0:fbfafa6bf5f9	308	// A workaround for mbed UART ISR bug
narshu	0:fbfafa6bf5f9	309	// Clear the RBR flag to make sure the interrupt doesn't loop
narshu	0:fbfafa6bf5f9	310	// UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14.
narshu	0:fbfafa6bf5f9	311	// UART0 for USB UART
narshu	0:fbfafa6bf5f9	312	unsigned char RBR = LPC_UART1->RBR;
narshu	0:fbfafa6bf5f9	313
narshu	0:fbfafa6bf5f9	314
narshu	0:fbfafa6bf5f9	315	if (!data_flag) { // look for alignment bytes
narshu	0:fbfafa6bf5f9	316	if (RBR == Alignment_char[Alignment_ptr]) {
narshu	0:fbfafa6bf5f9	317	Alignment_ptr ++;
narshu	0:fbfafa6bf5f9	318	}
narshu	0:fbfafa6bf5f9	319	if (Alignment_ptr >= 4) {
narshu	0:fbfafa6bf5f9	320	Alignment_ptr = 0;
narshu	0:fbfafa6bf5f9	321	data_flag = true; // set the dataflag
narshu	0:fbfafa6bf5f9	322	}
narshu	0:fbfafa6bf5f9	323	} else { // fetch data bytes
narshu	0:fbfafa6bf5f9	324	IRValues.IR_chars[buff_pointer] = RBR;
narshu	0:fbfafa6bf5f9	325	buff_pointer ++;
narshu	0:fbfafa6bf5f9	326	if (buff_pointer >= 12) {
narshu	0:fbfafa6bf5f9	327	buff_pointer = 0;
narshu	0:fbfafa6bf5f9	328	data_flag = false; // dessert the dataflag
narshu	0:fbfafa6bf5f9	329	if (angleInit) {
narshu	0:fbfafa6bf5f9	330	kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),rectifyAng(IRValues.IR_floats[1]+angleOffset),IRValues.IR_floats[2]);
narshu	0:fbfafa6bf5f9	331	} else {
narshu	0:fbfafa6bf5f9	332	kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRValues.IR_floats[2]);
narshu	0:fbfafa6bf5f9	333	}
narshu	0:fbfafa6bf5f9	334	}
narshu	0:fbfafa6bf5f9	335
narshu	0:fbfafa6bf5f9	336	}
narshu	0:fbfafa6bf5f9	337	}
narshu	0:fbfafa6bf5f9	338
narshu	0:fbfafa6bf5f9	339	void vKalmanInit(void) {
narshu	0:fbfafa6bf5f9	340	float SonarMeasures[3];
narshu	0:fbfafa6bf5f9	341	float IRMeasures[3];
narshu	0:fbfafa6bf5f9	342	int beacon_cnt = 0;
narshu	0:fbfafa6bf5f9	343	wait(1);
narshu	0:fbfafa6bf5f9	344	IRturret.attach(NULL,Serial::RxIrq);
narshu	0:fbfafa6bf5f9	345	kalman.statelock.lock();
narshu	0:fbfafa6bf5f9	346	SonarMeasures[0] = kalman.SonarMeasures[0]*1000.0f;
narshu	0:fbfafa6bf5f9	347	SonarMeasures[1] = kalman.SonarMeasures[1]*1000.0f;
narshu	0:fbfafa6bf5f9	348	SonarMeasures[2] = kalman.SonarMeasures[2]*1000.0f;
narshu	0:fbfafa6bf5f9	349	IRMeasures[0] = kalman.IRMeasures[0];
narshu	0:fbfafa6bf5f9	350	IRMeasures[1] = kalman.IRMeasures[1];
narshu	0:fbfafa6bf5f9	351	IRMeasures[2] = kalman.IRMeasures[2];
narshu	0:fbfafa6bf5f9	352	kalman.statelock.unlock();
narshu	0:fbfafa6bf5f9	353	//printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasures[0]180/PI, IRMeasures[1]180/PI, IRMeasures[2]*180/PI);
narshu	0:fbfafa6bf5f9	354	float d = beaconpos[2].y - beaconpos[1].y;
narshu	0:fbfafa6bf5f9	355	float i = beaconpos[0].y - beaconpos[1].y;
narshu	0:fbfafa6bf5f9	356	float j = beaconpos[0].x - beaconpos[1].x;
narshu	0:fbfafa6bf5f9	357	float y_coor = (SonarMeasures[1]SonarMeasures[1]- SonarMeasures[2]SonarMeasures[2] + dd) / (2d);
narshu	0:fbfafa6bf5f9	358	float x_coor = (SonarMeasures[1]SonarMeasures[1] - SonarMeasures[0]SonarMeasures[0] + ii + jj)/(2j) - iy_coor/j;
narshu	0:fbfafa6bf5f9	359	angleOffset = 0;
narshu	0:fbfafa6bf5f9	360	for (int i = 0; i < 3; i++) {
narshu	0:fbfafa6bf5f9	361	float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
narshu	0:fbfafa6bf5f9	362	if (IRMeasures[i] != 0){
narshu	0:fbfafa6bf5f9	363	beacon_cnt ++;
narshu	0:fbfafa6bf5f9	364	float angle_temp = angle_est - IRMeasures[i];
narshu	0:fbfafa6bf5f9	365	angle_temp -= (floor(angle_temp/(2PI)))2*PI;
narshu	0:fbfafa6bf5f9	366	angleOffset += angle_temp;
narshu	0:fbfafa6bf5f9	367	}
narshu	0:fbfafa6bf5f9	368	}
narshu	0:fbfafa6bf5f9	369	angleOffset = angleOffset/float(beacon_cnt);
narshu	0:fbfafa6bf5f9	370	//printf("\n\r");
narshu	0:fbfafa6bf5f9	371	angleInit = true;
narshu	0:fbfafa6bf5f9	372	kalman.statelock.lock();
narshu	0:fbfafa6bf5f9	373	kalman.X(0) = x_coor/1000.0f;
narshu	0:fbfafa6bf5f9	374	kalman.X(1) = y_coor/1000.0f;
narshu	0:fbfafa6bf5f9	375	kalman.X(2) = 0;
narshu	0:fbfafa6bf5f9	376	kalman.statelock.unlock();
narshu	0:fbfafa6bf5f9	377	//printf("x: %0.4f, y: %0.4f, offset: %0.4f \n\r", x_coor, y_coor, angleOffset*180/PI);
narshu	0:fbfafa6bf5f9	378	IRturret.attach(&vIRValueISR,Serial::RxIrq);
narshu	0:fbfafa6bf5f9	379	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	20 Apr 2012
Imports:	2
Forks:	1
Commits:	1
Dependents:	0
Dependencies:	2
Followers:	1

main.cpp@0:fbfafa6bf5f9, 2012-04-20 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning