int8_t i2c_send_and_receive(int8_t slave_addr, int8_t Cmd){
int8_t i2c_answer=0;
uint8_t timeout=0;

	I2CHW_1_bWriteBytes(slave_addr,&Cmd,1,I2CHW_1_CompleteXfer);		

			while (!(I2CHW_1_bReadI2CStatus() & I2CHW_WR_COMPLETE) && timeout<I2C_TIMEOUT){
			timeout++;
				if(timeout>I2C_TIMEOUT){
					UART_CPutString("i2c> write timed out!\r\n");
					return 99;
				}
			}

			timeout=0;
			I2CHW_1_ClrWrStatus(); 
			I2CHW_1_fReadBytes(slave_addr,&i2c_answer,1,I2CHW_1_CompleteXfer );
			
			while (!(I2CHW_1_bReadI2CStatus() & I2CHW_RD_COMPLETE) && timeout<I2C_TIMEOUT){
			timeout++;
			}
				if(timeout>I2C_TIMEOUT){
					UART_CPutString("i2c> read timed out!\r\n");
					return 99;
				}
			
			I2CHW_1_ClrRdStatus();


	return i2c_answer;				
}
Exemple #2
0
// Test reading and writing the 23K256 status register:
BYTE SPIRAM_StatusRegisterTest(void)
{
	// NOTE: SPIRAM_SEQUENTIAL_MODE|SPIRAM_PAGE_MODE is "Reserved", don't use it
	BYTE mode[6] = { SPIRAM_BYTE_MODE,
					 SPIRAM_BYTE_MODE|SPIRAM_DISABLE_HOLD,
					 SPIRAM_SEQUENTIAL_MODE,
					 SPIRAM_SEQUENTIAL_MODE|SPIRAM_DISABLE_HOLD,
					 SPIRAM_PAGE_MODE,
					 SPIRAM_PAGE_MODE|SPIRAM_DISABLE_HOLD };
	BYTE status;
	BYTE b;

	UART_CPutString("Status Register W/R Test: 0x  ");
	for (b=0; b<6 ; b++) {
		UART_PutChar(0x08);
		UART_PutChar(0x08);
		UART_PutSHexByte(mode[b]);
		if (SPIRAM_WriteStatusRegister(mode[b])) {
			UART_CPutString("\r\nWrite of invalid Status Register value. System halted.\r\n");
			M8C_Stop;
		}
		status = SPIRAM_ReadStatusRegister();
		if (status != mode[b]) {
			UART_CPutString(" FAIL\r\n");
			return(1);
		}
	}
	UART_CPutString("\b\b\b\b\b PASS\r\n");
	// Place the SRAM back in Byte Mode
	SPIRAM_WriteStatusRegister(SPIRAM_BYTE_MODE|SPIRAM_DISABLE_HOLD);
	return(0);
}
Exemple #3
0
void main(void)
{
	char opt;
	
	// Make sure nCS is high before doing anything
	nCS_HIGH;
	
	// Enable user module interrupts
	SleepTimer_EnableInt();

	// Enable global interrutps
	M8C_EnableGInt;

	// Start the user modules
	UART_Start(UART_PARITY_NONE);
	UART_PutCRLF();
	SPIM_Start(SPIM_SPIM_MODE_0 | SPIM_SPIM_MSB_FIRST);
	SleepTimer_Start();
	DAC8_Start(DAC8_FULLPOWER);
	UART_CPutString("Synthesiszing waveforms\r\n");
	WriteBlock(0);
	WriteBlock(1);
	WriteBlock(2);
	WriteBlock(3);
	while(1) 
	{
		UART_CPutString("Synthetic wave output is on Port0[4]\r\nCowabunga Dude! Time to catch some waves.\r\n\r\n0. Play block 0\r\n1. Play block 1\r\n2. Play block 2\r\n3. Play block 3\r\n4. Test status register\r\n5. Test byte mode\r\n6. Test sequential mode\r\n");
		
		opt = GetNumber(0, 6);
		switch (opt)
		{
			case 4:
				while(SPIRAM_StatusRegisterTest()) 
					SleepTimer_SyncWait(4, SleepTimer_WAIT_RELOAD);
				break;
			
			case 5:
				while(SPIRAM_ByteModeTest()) 
					SleepTimer_SyncWait(4, SleepTimer_WAIT_RELOAD);	
				break;
			
			case 6:
				while(SPIRAM_SequentialModeTest()) 
					SleepTimer_SyncWait(4, SleepTimer_WAIT_RELOAD);
				break;
			
			default:
				PlayBlock(opt);
				break;	
		}
	}
}
void init_robot(struct robot *bot){

	rs_init(&bot->rs);
   
    cs_init(&bot->cs_a);
    cs_init(&bot->cs_d);

    quadramp_init(&bot->qr_a);
    quadramp_init(&bot->qr_d);

    pid_init(&bot->pid_a);
    pid_init(&bot->pid_d);

   position_init(&bot->posr); //nous
   
//	bot->EVENT_DO_CS =1;
	bot->events = EVENT_DO_CS | EVENT_DO_POS;
	
    quadramp_init(&bot->qr_a);
    quadramp_init(&bot->qr_d);
	
	#ifdef UART_VERBOSE
	UART_CPutString("\r\n init robot structures : [OK]");
	#endif
}
Exemple #5
0
signed char fReadByteLoop(unsigned int blknum)
{
	bTargetAddress = 0;
	bTargetDataPtr = 0;

	while(bTargetDataPtr < TARGET_DATABUFF_LEN)
	{
		//Send Read Byte vector and then get a byte from Target
		SendVector(read_byte_v, 4);
		// Set the drive here because SendByte() does not
		SetSDATAStrong();
		SendByte(bTargetAddress,7);

		RunClock(2);       // Run two SCLK cycles between writing and reading
		SetSDATAHiZ();     // Set to HiZ so Target can drive SDATA
		bTargetDataIN = bReceiveByte();

		RunClock(1);
		SendVector(read_byte_v + 1, 1);     // Send the ReadByte Vector End

		// Test the Byte that was read from the Target against the original
		// value (already in the 128-Byte array "abTargetDataOUT[]"). If it
		// matches, then bump the address & pointer,loop-back and continue.
		// If it does NOT match abort the loop and return and error.

		//if (bTargetDataIN != abTargetDataOUT[bTargetDataPtr])
		if (bTargetDataIN != pSocData[blknum*TARGET_DATABUFF_LEN + bTargetDataPtr])
		{
#ifdef TX_ON
			UART_PutCRLF();
			UART_CPutString("bTargetDataIN : ");
			UART_PutHexByte(bTargetDataIN);
			UART_CPutString(" abTargetDataOUT : ");
			UART_PutHexByte(pSocData[blknum*TARGET_DATABUFF_LEN + bTargetDataPtr]);
#endif
			return(BLOCK_ERROR);
		}

		bTargetDataPtr++;
		// Increment the address by 2 to accomodate 7-Bit addressing
		// (puts the 7-bit address into MSBit locations for "SendByte()").
		bTargetAddress += 2;

	}

	return(PASS);
}
void i2c_send_only(int8_t slave_addr,int8_t Cmd){
uint8_t timeout=0;
	I2CHW_1_bWriteBytes(slave_addr,&Cmd,1,I2CHW_1_CompleteXfer);		

			while (!(I2CHW_1_bReadI2CStatus() & I2CHW_WR_COMPLETE) && timeout<I2C_TIMEOUT){
			timeout++;
			}	
			if(timeout == I2C_TIMEOUT)
				UART_CPutString("i2c> operation timed out :( !\r\n");
			I2CHW_1_ClrWrStatus(); 
				
}
void init_uart(void){
	///UART////      

    UART_CmdReset(); 
    UART_IntCntl(UART_ENABLE_RX_INT);       
    UART_Start(UART_PARITY_NONE);            
    UART_PutChar(12);
	
	#ifdef UART_VERBOSE
	UART_CPutString("\r\n\r\n\r\n\r\n\r\n\r\n waking up......................................."); 
	#endif
}
Exemple #8
0
// This function plays a block of data where opt is the data block 0-3.
void PlayBlock(char id)
{	
	WORD startAddr = id * 0x2000; // Where the address starts for block
	WORD endAddr = startAddr + 0x2000; // Where the address ends for block
	WORD addr = startAddr;
	
	SPIRAM_WriteStatusRegister(SPIRAM_BYTE_MODE | SPIRAM_DISABLE_HOLD);
	UART_CPutString("Press any key to abort\r\n");
	while (!UART_cReadChar())
	{
		DAC8_WriteStall(SPIRAM_ReadByte(addr));
		if (++addr > endAddr) addr = startAddr;
	}
}
Exemple #9
0
// Test reading and writing the 23K256 in Sequential Mode:
WORD SPIRAM_SequentialModeTest(void)
{
	BYTE status;
	BYTE a;
	BYTE b;
	BYTE in;
	WORD addr;

	SPIRAM_WriteStatusRegister(SPIRAM_SEQUENTIAL_MODE|SPIRAM_DISABLE_HOLD);
	UART_CPutString("Sequential Mode W/R Test: Addr 0x    ");

	for (addr=0; addr<0x8000 ; addr+=ARRAY_SIZE) {
		UART_CPutString("\b\b\b\b");
		UART_PutSHexInt(addr);
		b = 0;
		do {
			for (a=0 ; a<ARRAY_SIZE ; a++) {
				DataOut[a] = b;
			}
			SPIRAM_WriteArray(addr, DataOut, ARRAY_SIZE);
			SPIRAM_ReadArray(addr, DataIn, ARRAY_SIZE);
			for (a=0 ; a<ARRAY_SIZE ; a++) {
				if (DataIn[a] != b) {
					UART_CPutString("\b\b\b\b");
					UART_PutSHexInt(addr+a);
					UART_CPutString(" FAIL\r\n");
					return(1);
				}
			}
			if (!b)
				b = 0x01;
			else
				b = b << 1;
		} while(b);
		if (UART_cReadChar()) {
			UART_CPutString("\b\b\b\b\b\b\b\b\b\b\b\b ABORTED    \r\n");
			return(0);
		}
	}
	UART_CPutString("\b\b\b\b\b\b\b\b\b\b\b\b PASS       \r\n");
	return(0);
}
Exemple #10
0
// Test reading and writing the 23K256 in Byte Mode:
WORD SPIRAM_ByteModeTest(void)
{
	BYTE status;
	BYTE b;
	BYTE in;
	WORD addr;

	SPIRAM_WriteStatusRegister(SPIRAM_BYTE_MODE|SPIRAM_DISABLE_HOLD);
	UART_CPutString("      Byte Mode W/R Test: Addr 0x    ");

	for (addr=0; addr<0x8000 ; addr++) {
		if (((addr-1) & 0x000f) == 0x000f) {
			UART_CPutString("\b\b\b\b");
			UART_PutSHexInt(addr);
		}
		b = 0;
		do {
			SPIRAM_WriteByte(addr, b);
			in = SPIRAM_ReadByte(addr);
			if (in != b) {
				UART_CPutString("\b\b\b\b");
				UART_PutSHexInt(addr);
				UART_CPutString(" FAIL\r\n");
				return(1);
			}
			if (!b)
				b = 0x01;
			else
				b = b << 1;
		} while(b);
		if (UART_cReadChar()) {
			UART_CPutString("\b\b\b\b\b\b\b\b\b\b\b\b ABORTED    \r\n");
			return(0);
		}
	}
	UART_CPutString("\b\b\b\b\b\b\b\b\b\b\b\b PASS       \r\n");
	return(0);
}
Exemple #11
0
void main(void)
{	
	M8C_EnableGInt ; // Uncomment this line to enable Global Interrupts
	
	// Start the UART(with no parity), and Counter16
	UART_Start(UART_PARITY_NONE);
	// clock for moving serial
	Counter16_Start();
	
	// Start I2CHW
	I2CHW_Start();
	I2CHW_EnableMstr();
	I2CHW_EnableInt();
	
	// This is the command usage string
	UART_CPutString("########################## I2C External SRAM ########################\r\n\
#	W # XX T [Data]\r\n\
#		W    - Write command\r\n\
#		#    - Group Address (0 - 7)\r\n\
#		XX   - Memory Location in hex (00 - FF)\r\n\
#		T    - Data Type, either A for ASCII or H for Hexadecimal\r\n\
#		Data - Either ASCII string or Hexadecimal separates by spaces\r\n\
#\t\t\tA - Mary had a little lamb\r\n\
#\t\t\tH - 01 FF A0 0F D8 C3\r\n\
#\r\n\
#	R # XX T NN\r\n\
#		R    - Read command\r\n\
#		#    - Group Address (0 - 7)\r\n\
#		XX   - Memory Location in hex (00 - FF)\r\n\
#		T    - Data Type, either A for ASCII or H for Hexadecimal\r\n\
#		NN	 - Number of bytes to read in hexadecimal\r\n\
#####################################################################\r\n");
	while (1)
	{
		char *cmd;
		char *params;
		char slaveAddress = 0x50;		// 010100000 R/W shifted to front
		
		GetLine(buf, 79); // Retrieves a line with a maximum length of 70 characters and put it in buf.
		
		memset(data, 0x00, 256);	// Initialize all the set {data} to NULL bytes
		cmd = Lowercase(cstrtok(buf, " ")); // Get the first word from the entered string and lowercase it.
		if (strlen(cmd) == 1 && cmd[0] == 'w') // If the command is one letter and it is w, then write command
		{	
			int groupAddress; // only 1 and 2 actually go to SRAM
			int memLoc;
			char dataType;
			
			int len;

			params = cstrtok(0x00, " ");  // 0x00 indicates it will continue from last cstrtok command and get next word. This gets the next parameter
			
			// csscanf if used to parse the string into values such as hexadecimal or integers
			// It returns the number of parameters it parsed which should be one
			// If the length of the params is not right or it does not parse the right amount, it returns an error
			// %d gets an integer, this is the groupAddress
			if (strlen(params) != 1 || csscanf(params, "%d", &groupAddress) != 1) goto error;
			
			// %x gets a hexadecimal value, this can read capital or lowercase letters, this is the memory location
			params = cstrtok(0x00, " ");
			if (strlen(params) != 2 || csscanf(params, "%x", &memLoc) != 1) goto error;
			
			// %c gets a character, the data type character
			params = cstrtok(0x00, " ");
			if (strlen(params) != 1 || csscanf(params, "%c", &dataType) != 1) goto error;
			
			// This reads the rest of the string and stores it in params. 
			// If the length is zero or if cstrtok returns 0, this means that there was no valid string/hex entered
			params = cstrtok(0x00, "\0");
			if (strlen(params) == 0 || params == 0x00) goto error;	// They did all the params but didn't write anything
			
			dataType = tolower(dataType); // Lowercase the data type
			if (groupAddress < 0 || groupAddress > 7)
				goto error; // groupAddress was not in range
			
			data[0] = memLoc;	// First byte needs to be the memory location according to PCF8570 datasheet
			slaveAddress |= groupAddress;	// ORs the group 2 address to the group 1 address to get slaveAddress
			
			if (dataType == 'a') // If the data type is ASCII
			{
				strcpy((data + 1), params); // Copy the string from params and put it right after the data[0] byte
				len = strlen((data + 1)) + 1; // len is the number of bytes to write, it is the length of the string and then +1 because of the memLoc byte
				// Cant just do strlen(data) because data[0] could be 0x00 and it would return 0 as the string length
			}
			else if (dataType == 'h') // If the data type is hex
			{
				// Take ASCII encoded hex data params and put it after data[0], returns number of bytes converted
				if ((len = HexConversion(params, (data + 1))) == -1)
					goto error;
				len++; // Add one to the length because of the memLoc byte at data[0]
			}
			else
				goto error;
			
			I2CHW_bWriteBytes(slaveAddress, data, len, I2CHW_CompleteXfer); // Write len bytes from data 
			while (!(I2CHW_bReadI2CStatus() & I2CHW_WR_COMPLETE)); // Wait while it is writing
			I2CHW_ClrWrStatus(); // Clear the write bit

			csprintf(data, "%x bytes were written", len); // csprintf takes the string and substitutes %x for len, puts into data str
			UART_PutString(data); // Print the string to UART
			UART_PutCRLF();
		}
		else if (strlen(cmd) == 1 && cmd[0] == 'r') // If the command is one letter and it is r, then read command
		{
			int groupAddress;
			int memLoc;
			char dataType;
			int numBytes;
			
			char hexStr[4];
			int i;

			// csscanf if used to parse the string into values such as hexadecimal or integers
			// It returns the number of parameters it parsed which should be one
			// If the length of the params is not right or it does not parse the right amount, it returns an error
			// %d gets an integer, this is the groupAddress
			params = cstrtok(0x00, " ");
			if (strlen(params) != 1 || csscanf(params, "%d", &groupAddress) != 1) goto error;
	
			// %x gets a hexadecimal value, this can read capital or lowercase letters, this is the memory location
			params = cstrtok(0x00, " ");
			if (strlen(params) != 2 || csscanf(params, "%x", &memLoc) != 1) goto error;
			
			// %c gets a character, the data type character
			params = cstrtok(0x00, " ");
			if (strlen(params) != 1 || csscanf(params, "%c", &dataType) != 1) goto error;
			
			// %x gets a hexadecimal value, number of bytes to read
			params = cstrtok(0x00, " ");
			if (strlen(params) != 2 || csscanf(params, "%x", &numBytes) != 1) goto error;
			
			// If there is any data after the number of bytes, then the format is invalid and it should return an error
			if (cstrtok(0x00, " ") != 0x00) goto error;
			
			dataType = tolower(dataType); // Lowercase the data type
			if (groupAddress < 0 || groupAddress > 7)
				goto error; // groupAddress was not in range
			
			data[0] = memLoc;	// First byte needs to be the memory location according to PCF8570 datasheet
			slaveAddress |= groupAddress;	// ORs the group 2 address to the group 1 address to get slaveAddress
			
			I2CHW_bWriteBytes(slaveAddress, data, 1, I2CHW_NoStop); // Write one byte to the RAM, the slaveAddress so it knows who were talking to
			while (!(I2CHW_bReadI2CStatus() & I2CHW_WR_COMPLETE)); // Wait while it is writing
			I2CHW_ClrWrStatus(); // Clear the write bit
			
			I2CHW_fReadBytes(slaveAddress, data, numBytes, I2CHW_CompleteXfer); // Read numBytes from the RAM, put it in data
			while(!(I2CHW_bReadI2CStatus() & I2CHW_RD_COMPLETE)); // Wait while it is reading
			I2CHW_ClrRdStatus(); // Clear the read bit
			
			if (dataType == 'a') // If the data type is ASCII
			{
				for (i = 0; i < numBytes; ++i) // Loop through each byte
					UART_PutChar(data[i]); // Put the character in PuTTy
				UART_PutCRLF();
			}
			else if (dataType == 'h') // If the data type is Hex
			{
				for (i = 0; i < numBytes; ++i) // Loop through each byte
				{
					csprintf(hexStr, "%X ", data[i]); // csprintf prints into hexStr a hexadecimal with a space
					UART_PutString(hexStr); // Print hexStr
				}
				UART_PutCRLF();
			}
			else
				goto error;
		}
		else 
			goto error;
		
		continue; // This is so that the error is skipped when everything goes right
		error: // This outputs an invalid format message and continues on to read another line
			UART_CPutString("Invalid format entered. Valid formats are:\r\n\tW [GroupAddress] [MemoryLocation] [h|a] Hex/ASCII\r\n\tR [GroupAddress] [MemoryLocation] [h|a] [NumBytes]\r\n");
	}
}
void strat_go_get_elts_on_table(struct strategy * strat){
static uint8_t elts_to_store;
static uint24_t starttime; 
static uint8_t storage_lock;

	switch (Strat_Action) {
		case 0:
		/*Go p1*/
			i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_BOUFFE);
			if(beudabot.color==COLOR_GREEN)
				trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X,ON_TABLE_ELT_G1_Y);
			else
				trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X,ON_TABLE_ELT_R1_Y);
			Strat_Action++;
		break;
		
		case 1:
		/*Go p2*/	
			if(beudabot.color==COLOR_GREEN)
				trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X+60,ON_TABLE_ELT_G1_Y);
			else
				trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X+60,ON_TABLE_ELT_R1_Y);
			Strat_Action++;
		break;
		
		case 2:
			Strat_Action=0;
			Strat_State=STRAT_EMERGENCY_BEHAVIOUR;
		break;
		/*
		case 2:
			//before continuing the seq, we check how many elts we got
			elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN);
			Strat_Action++;
		break;*/
		
		case 3:
			if(elts_to_store==2){
				if(strat_store_elt(elts_to_store)){
					Strat_Action++;	
					setBit(strat_flags,TWO_ELTS_LOADED);
					i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_BOUFFE); //after storage, it stops bouffing ??
			
				}
			}
			else //if we don't have 2elts we just continu the seq.
				Strat_Action++;
		break;
		
		case 4:
		/*here, if we got 1 or 2 elements, they're stored, and the pince is well placed*/
			quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_SLOW, QR_SPEED_D_NEG_SLOW); // Vitesse 
			if(beudabot.color==COLOR_GREEN)
				trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X+60,ON_TABLE_ELT_G1_Y+25);
			else	
				trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X+60,ON_TABLE_ELT_R1_Y-25);
			Strat_Action++;	
		break;
		
		case 5:
			elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN);
			Strat_Action++;	
		break;
		
		case 6:
			if(elts_to_store==2){
				if(strat_store_elt(elts_to_store)){
				/*Warning: no timeout here, we trust the tapis-roulo at this time...*/
					if(strat_flags & TWO_ELTS_LOADED){
					//Yes !! we got 4 elts, lets build a real temple!
						i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_PROTECT); //we don't want more for the moment
						Strat_Action = 0;
						Strat_State++; 
						setBit(strat_flags,I_CAN_BUILD_A_TEMPLE); //for lint deposit operation...
						clearBit(strat_flags,TWO_ELTS_LOADED);
					}
					
				}
			}
			else 
				Strat_Action++;
			
		break;
		
		case 7:
		/*
		here the situation is:
		- we got 2 elts stored, and we are looking for 2 others...
		- we have nothing.. (check strat_flags & TWO_ELTS_LOADED the know the answer...)
		*/
			if(beudabot.color==COLOR_GREEN)		
				trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X,ON_TABLE_ELT_G1_Y+25);
			else
				trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X,ON_TABLE_ELT_R1_Y-25);
			Strat_Action++;
		break;

		case 8:
			elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN);
			Strat_Action++;	
		break;
		
		case 9:
			if(elts_to_store==2){ //owkey, we have now 2 elts 
				if(strat_store_elt(elts_to_store)){
					if(strat_flags & TWO_ELTS_LOADED){ //AWESOME, we got 2elts inside, and 2elts stored...
						i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_PROTECT); //we don't want more for the moment
						Strat_Action = 0;
						Strat_State++; //let's build !!
						clearBit(strat_flags,TWO_ELTS_LOADED);
						setBit(strat_flags,I_CAN_BUILD_A_TEMPLE); //for lint deposit operation lock...
					}
				}
			}
			else 
				Strat_Action++;
		break;
		case 10:
			if(beudabot.color==COLOR_GREEN)
				trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X,ON_TABLE_ELT_G1_Y+50);
			else
				trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X,ON_TABLE_ELT_R1_Y-50);
			Strat_Action++;	
		break;
		
		case 11:
			elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN);
			Strat_Action++;	
		break;
		
		case 12:
			if(elts_to_store==2){
				if(strat_store_elt(elts_to_store)){
					if(strat_flags & TWO_ELTS_LOADED){  //AWESOME, we got 2elts inside, and 2elts stored...
						Strat_Action = 0;
						i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_PROTECT); //we don't want more for the moment
						Strat_State++; //next op is building...
						setBit(strat_flags,I_CAN_BUILD_A_TEMPLE); //for lint deposit operation...
						clearBit(strat_flags,TWO_ELTS_LOADED);
					}	
				}
			}
			else 
				Strat_Action++;
		break;
			
		case 13:
			if(beudabot.color==COLOR_GREEN)
				trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X+60,ON_TABLE_ELT_G1_Y+50);	
			else
				trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X+60,ON_TABLE_ELT_R1_Y-50);
			Strat_Action++;	
		break;
		
		case 14:	
			/*I still don't have 4 elts :(, let's build with what we have*/
			UART_CPutString("Going to build with less than 4elts :( go back and try again ???\r\n");
			Strat_Action=0;
			Strat_State++;	
			clearBit(strat_flags,TWO_ELTS_LOADED);
		break;
	}
}
void strat_manage(struct strategy * strat){
	static uint24_t starttime;
	
	if( beudatraj.state==READY){
	
			switch(Strat_State){
				case STRAT_GET_ELT_ON_TABLE:
				#ifdef DEBUG_STRAT
				UART_CPutString("strat> get elt on table\r\n");
				#endif
					strat_go_get_elts_on_table(strat);
				break;
			
					 
				case STRAT_CONSTRUCT_TEMPLE_3:
				#ifdef DEBUG_STRAT
				UART_CPutString("strat> hav to build a temple 3\r\n");
				#endif
					strat_construct_temple(strat,3,0); //0 it depends on our color
				break;
				
				case STRAT_GET_LINT_1:
				#ifdef DEBUG_STRAT
				UART_CPutString("strat> get lint area 1\r\n");
				#endif
					strat_go_get_lint(strat,LINT_DISP_L1);
				break;
				
				case STRAT_GET_LINT_2:
				#ifdef DEBUG_STRAT
				UART_CPutString("strat> get lint area 2\r\n");
				#endif
					strat_go_get_lint(strat,LINT_DISP_L2);
				break;
				
				case STRAT_GET_ELT_ON_FIXED_DISP:
				#ifdef DEBUG_STRAT
				UART_CPutString("strat> get elt on fixed disp\r\n");
				#endif
					strat_go_get_elts_disp(strat,0);
				break;
				
				
				case STRAT_GET_ELT_ON_RAND_DISP_1:
				#ifdef DEBUG_STRAT
				UART_CPutString("strat> get elt on rand 1\r\n");
				#endif
					strat_go_get_elts_disp(strat,1);
				break;
				
				case STRAT_GET_ELT_ON_RAND_DISP_2:
				#ifdef DEBUG_STRAT
				UART_CPutString("strat> get elt on disp 2\r\n");
				#endif
					strat_go_get_elts_disp(strat,2);
				break;
				 
				case STRAT_CONSTRUCT_TEMPLE_2_1:
				#ifdef DEBUG_STRAT
				UART_CPutString("strat> constr temp 2 1\r\n");
				#endif
					strat_construct_temple(strat,2,1);

				break;
				
				case STRAT_CONSTRUCT_TEMPLE_2_2:
				#ifdef DEBUG_STRAT
				UART_CPutString("strat> constr temp 2 2\r\n");
				#endif
					strat_construct_temple(strat,2,2);
				break;
			
				case STRAT_CONSTRUCT_TEMPLE_1_1:
				#ifdef DEBUG_STRAT
				UART_CPutString("strat> constr temp 1 1\r\n");
				#endif
					strat_construct_temple(strat,1,1);
				break;
				
				case STRAT_CONSTRUCT_TEMPLE_1_2:
				#ifdef DEBUG_STRAT
				UART_CPutString("strat> constr temp 1 2\r\n");
				#endif
					strat_construct_temple(strat,1,2);
				break;
				
				case STRAT_CONSTRUCT_TEMPLE_1_3:
				#ifdef DEBUG_STRAT
				UART_CPutString("strat> constr temp 1 3\r\n");
				#endif
					strat_construct_temple(strat,1,3);
				break;
				
				case STRAT_CONSTRUCT_TEMPLE_1_4:
				#ifdef DEBUG_STRAT
				UART_CPutString("strat> constr temp 1 4\r\n");
				#endif
					strat_construct_temple(strat,1,4);
				break;
				
				case STRAT_EMERGENCY_BEHAVIOUR:
				#ifdef DEBUG_STRAT
				UART_CPutString("strat> emergency behaviour on\r\n");
				#endif
					strat_do_emergency_behaviour();
					PRT2DR ^=0x80;
				break;
				
				case STRAT_EXIT:
			
					#ifdef DEBUG_STRAT
					UART_CPutString("strat> exit\r\n");
					#endif
					pwm_left(0);
					pwm_right(0);
					
					//ordre i2c pour sortir et ouvrir la pince
					i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_EJECT);
					i2c_send_only(I2C_LINTO_ADDR,I2C_LINT_DROP);
					i2c_send_only(I2C_PINCE_ADDR,I2C_PINCE_DROP);
					
					starttime = gettime();
					while(gettime() < (starttime + DELAY_TAPIS_EJECT));
						i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_STOP);
						clearBit(beudabot.events,EVENT_DO_STRAT);
					while(1);
				break;
			}
	}
}
void strat_go_get_lint(struct strategy * strat,uint8_t position){
static uint24_t starttime;

	if(beudatraj.state==READY){
		switch(Strat_Action){
		
			case 0:
				quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_A_POS_SLOW, QR_SPEED_A_NEG_SLOW); // Vitesse 
					if(position==1){
						if(beudabot.color==COLOR_GREEN)
							oa_set_target(&beudaoa, G_LINT_DISP_1_X-20,G_LINT_DISP_1_Y);
						else
							oa_set_target(&beudaoa, R_LINT_DISP_1_X-20,R_LINT_DISP_1_Y);
					}
					else if(position==2){
						if(beudabot.color==COLOR_GREEN)
							oa_set_target(&beudaoa, G_LINT_DISP_2_X-20,G_LINT_DISP_2_Y);
						else
							oa_set_target(&beudaoa, R_LINT_DISP_2_X-20,R_LINT_DISP_2_Y);
					}
				Strat_Action++;
			break;
			
			case 1:
				if(beudaoa.state==OA_IN_TARGET){
					i2c_send_only(I2C_LINTO_ADDR,I2C_LINT_GET_DOWN);
					Strat_Action++;
				}
			break;
			
			case 2:
				quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_VERY_SLOW, QR_SPEED_D_NEG_VERY_SLOW); // Vitesse 
				trajectory_d_a_rel(&beudatraj, 20, 0);	
				Strat_Action++;
			break;
			
			case 3:
				//i2c lint remont
				i2c_send_only(I2C_LINTO_ADDR,I2C_LINT_GET_UP);
				starttime = gettime();
				Strat_Action++;
			break;
			
			case 4:
				if(gettime() > starttime + DELAY_LINT_UP){
					if(i2c_send_and_receive(I2C_LINTO_ADDR, I2C_GOT_SMTG)==1){
					//go get another one, somewhere else...
						Strat_Action = 0;
						Strat_State = STRAT_GET_LINT_2;
					}
					else{ //2
						#ifdef DEBUG_STRAT
						UART_CPutString("strat> we got a lint !! \r\n");
						#endif
						Strat_Action = 0;
						Strat_State = STRAT_GET_ELT_ON_FIXED_DISP;
					}
				
				}
			break;
			
		}
	}	

}
void strat_construct_temple(struct strategy * strat,uint8_t build_area,uint8_t half){
static uint24_t starttime;

	if(beudatraj.state==READY){
	
		switch(Strat_Action){	
		
			case 0:
				if(build_area==3){
					if(beudabot.color==COLOR_GREEN)
						oa_set_target(&beudaoa, BUILD_AREA_3b1_X,BUILD_AREA_3b1_Y);
					else
						oa_set_target(&beudaoa, BUILD_AREA_3b2_X,BUILD_AREA_3b2_Y);
				}
				else if(build_area==2){ 
					if(half == 1)
						oa_set_target(&beudaoa, BUILD_AREA_2b1_X,BUILD_AREA_2b1_Y);
					else
						oa_set_target(&beudaoa, BUILD_AREA_2b2_X,BUILD_AREA_2b2_Y);
				}
				else if(build_area==1){
					/*here we should check our pos and determinate the closest one*/
					if(half == 1)
						oa_set_target(&beudaoa, BUILD_AREA_1b1_X,BUILD_AREA_1b1_Y);
					else if(half == 2)
						oa_set_target(&beudaoa, BUILD_AREA_1b2_X,BUILD_AREA_1b2_Y);
					
					else if(half == 3)
						oa_set_target(&beudaoa, BUILD_AREA_1b3_X,BUILD_AREA_1b3_Y);
					
					else if(half == 4)
						oa_set_target(&beudaoa, BUILD_AREA_1b4_X,BUILD_AREA_1b4_Y);
					
				}
				Strat_Action++;
			break;
		
			case 1:
				if(beudaoa.state==OA_IN_TARGET){
					//trajectory_turnto_xy_behind(&beudatraj, 105, 150);
					if(build_area==3){
						if(beudabot.color==COLOR_GREEN)
							trajectory_a_abs(&beudatraj,-90);
						else
							trajectory_a_abs(&beudatraj,90);
					}
					else if(build_area==2 || build_area==1){
						trajectory_a_abs(&beudatraj,180);
					}
				}
				Strat_Action++;
			break;
		
			case 2:
			//here whe sould give the back to the build area x
				quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_VERY_SLOW, QR_SPEED_D_NEG_VERY_SLOW); // Vitesse 
				trajectory_d_a_rel(&beudatraj, -20, 0);
				Strat_Action++;
			break;
		
			case 3:
				//envoi d'ordres i2c à la pinc pour construire
				i2c_send_only(I2C_PINCE_ADDR,I2C_PINCE_CONSTRUCT);
				starttime = gettime();
				Strat_Action++;
			break;

			case 4:
				
				if((gettime() > starttime + DELAY_PINCE_CONSTRUCTION)){

					if(i2c_send_and_receive(I2C_PINCE_ADDR,I2C_PINCE_DID_YOU_BUILD_SMTG)==0){
						//we wait more
						if(gettime() > (starttime + 2*DELAY_PINCE_CONSTRUCTION)){
							//there is a problem here... 
							//drop the shit and let's continue the sequ...
							//pince is maybe just f****d up ...
							/*A CHANGER !!!!!*/
							Strat_State = STRAT_EMERGENCY_BEHAVIOUR;
							
							
							#ifdef DEBUG_STRAT
								UART_CPutString("Pince is down... !\r\n");
							#endif
								setBit(strat_flags,PINCE_IS_SHITTY);
						}
							
					}
					else if (i2c_send_and_receive(I2C_PINCE_ADDR,I2C_PINCE_DID_YOU_BUILD_SMTG)==2){
						//Damn it! there is already a construction here... let's move and try somewhere else!..
							Strat_Action = 0;
						//if(Strat_State <STRAT_CONSTRUCT_TEMPLE_1_2)
							strat_go_next_build_area(); //this function will change strat state to build somewhere else...
							current_building_area++; //for next call of strat_go_next_build_area();
							#ifdef DEBUG_STRAT
								UART_CPutString("Playing against RCVA || Microb ???!\r\n");
							#endif			 
					}
					else if(i2c_send_and_receive(I2C_PINCE_ADDR,I2C_PINCE_DID_YOU_BUILD_SMTG)==1){
						//finish building, let's put the lint if flag I_CAN_BUILD_A_TEMPLE is set
						Strat_Action++;
					}
				}
			break;
			
			case 5:
				if(strat_flags & I_CAN_BUILD_A_TEMPLE){
					quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_VERY_SLOW, QR_SPEED_D_NEG_VERY_SLOW); // Vitesse 
					trajectory_d_a_rel(&beudatraj, -20, 0);
					
				}
				Strat_Action++;
			break;
			
			case 6:
				if(strat_flags & I_CAN_BUILD_A_TEMPLE){
					trajectory_d_a_rel(&beudatraj, 0, 180);
				}
				Strat_Action++;
			break;
			
			case 7:
				if(strat_flags & I_CAN_BUILD_A_TEMPLE){
					trajectory_d_a_rel(&beudatraj, 20, 0);
				}
				Strat_Action++;
			break;
			
			case 8:
				if(strat_flags & I_CAN_BUILD_A_TEMPLE){
					i2c_send_only(I2C_LINTO_ADDR, I2C_LINT_DEPOSIT_DOWN);
					starttime = gettime();
				}
				Strat_Action++;
				
			break;
			
			case 9:
				if(strat_flags & I_CAN_BUILD_A_TEMPLE){
					if(gettime() > starttime + DELAY_LINT_DOWN){
						Strat_State=STRAT_GET_LINT_1;
					}
				}
				else
					Strat_State=STRAT_GET_LINT_1;
				 //get a lint, then elts, to build another one...
			break;

		}
	}
 }
void oa_manage(struct obstacle_avoidance *oa){
   	
	xy_position my_pos_xy;
	my_pos_xy.x = position_get_x_s16(oa->traj->position);
	my_pos_xy.y = position_get_y_s16(oa->traj->position);
	uint8_t my_quad = oa_get_quad(my_pos_xy);
	
   	if(oa->state==OA_TARGET_SET && oa->traj->state==READY){ //new target where to go !
   		
		uint8_t opp_quad = oa_get_quad(oa->opponent_xy);
		uint8_t target_quad = oa_get_quad(oa->target_xy);
	
		//oa->state==OA_PROCESSING;
		uint8_t tmpquad;
		#ifdef DEBUG_OA
    	UART_CPutString("oa> processing new target\r\n");
		#endif
		
		if(oa_get_com_dt(my_pos_xy,oa->target_xy)!=0){  //!=0
			#ifdef DEBUG_OA
			UART_CPutString("oa> same dt than target\r\n");
			#endif
		
			if(oa_get_com_dt(my_pos_xy,oa->opponent_xy)!=0){
				#ifdef DEBUG_OA
				UART_CPutString("oa> same dt than opp-> sixt\r\n");
				#endif
				oa_sixt(oa);
			}
			else{
				#ifdef DEBUG_OA
				UART_CPutString("oa> not same dt than opp ->xy!\r\n");
				#endif
				trajectory_goto_xy_abs(oa->traj,oa->target_xy.x,oa->target_xy.y);
				oa->state=OA_IN_TARGET;
			}
		}
		
		else{ //==0
			#ifdef DEBUG_OA
			UART_CPutString("oa> not in same dt than target\r\n");
			#endif
	
			#ifdef DEBUG_OA
			sprintf(uart_out_buffer,"oa> quads: me %d opp %d targ %d\r\n",my_quad,opp_quad,target_quad);
			UART_PutString(&uart_out_buffer);
			#endif
			
			if(oa_is_adj(my_quad,opp_quad)){ //me & opp are adj
				#ifdef DEBUG_OA
				UART_CPutString("oa> I'm adj to opponent\r\n");
				#endif
				
				if(my_quad==target_quad){ //my_quad==target_quad
					#ifdef DEBUG_OA
					UART_CPutString("oa> my_qd=tg_qd->my_qd center\r\n");
					#endif
					oa_goto_quad(oa,my_quad);
				}
				else{ //my_quad!=target_quad
					#ifdef DEBUG_OA
					UART_CPutString("oa> my_qd!=tg_qd\r\n");
					#endif
					
					if(target_quad==opp_quad){ //target_quad==opp_quad
						#ifdef DEBUG_OA
						UART_CPutString("oa> tg_qd=op_qd->!opp half\r\n");
						#endif
						
						tmpquad = oa_get_no_half_quad(oa->opponent_xy,target_quad,my_quad);
						oa_goto_quad(oa,tmpquad); 
						
						#ifdef DEBUG_OA
						sprintf(uart_out_buffer,"oa> !opp half is qd %d\r\n",tmpquad);
						UART_PutString(&uart_out_buffer);
						#endif
					}
					else{//target_quad!=opp_quad
						#ifdef DEBUG_OA
						UART_CPutString("oa> tg_qd!=op_qd->sym opp quad (but first, my quad)\r\n");
						#endif
						//   
		/*last modified stuff: before: oa_goto_quad(oa,oa_get_sym_quad(opp_quad)); only..*/				
						if(!(oa->flags & OA_FLAG_GOSYMQD_PART1_DONE)){
							setBit(oa->flags,OA_FLAG_GOSYMQD_PART1_DONE);
							#ifdef DEBUG_OA
							UART_CPutString("oa> my quad...\r\n");
							#endif
							oa_goto_quad(oa,my_quad); 
							}
						else {
							clearBit(oa->flags,OA_FLAG_GOSYMQD_PART1_DONE);
							#ifdef DEBUG_OA
							UART_CPutString("oa> opp sym quad...\r\n");
							#endif
							oa_goto_quad(oa,oa_get_sym_quad(opp_quad));
						}
						
					}
				}
					
			}
			else{  //me & opp aren't adj
				#ifdef DEBUG_OA
				UART_CPutString("oa> I'm not adj to opponent\r\n");
				#endif
				
				if(my_quad==opp_quad){ //my_quad==opp_quad
					#ifdef DEBUG_OA
					UART_CPutString("oa> my_qd=opp_qd->sixt\r\n");
					#endif
					oa_sixt(oa); //apply the celebre "Sixt()" algorithm..
				}
				else{ // !my_quad==opp_quad
					#ifdef DEBUG_OA
					UART_CPutString("oa> my_qd!=opp_qd\r\n");
					#endif
					
					if(target_quad==opp_quad){ //target_quad==opp_quad
						#ifdef DEBUG_OA
						UART_CPutString("oa> tg_qd=opp_qd->!opp half\r\n");
						#endif 
						//tmpquad = (my_quad-1+oa_get_no_half_quad(oa->opponent_xy,target_quad))%4+1;
						tmpquad = oa_get_no_half_quad(oa->opponent_xy,target_quad,my_quad);
						
						//oa_goto_quad(oa,tmpquad); 
						if(!(oa->flags & OA_FLAG_GOOPPQD_PART1_DONE)){
							setBit(oa->flags,OA_FLAG_GOOPPQD_PART1_DONE);
							#ifdef DEBUG_OA
							UART_CPutString("oa> my quad...\r\n");
							#endif
							oa_goto_quad(oa,my_quad); 
							}
						else {
							clearBit(oa->flags,OA_FLAG_GOOPPQD_PART1_DONE);
							#ifdef DEBUG_OA
							UART_CPutString("oa> !opp quad...\r\n");
							#endif
							oa_goto_quad(oa,tmpquad);
						}
						
						//-1 because we don't want to go to the same half quad than the opponent
						#ifdef DEBUG_OA
						sprintf(uart_out_buffer,"oa> !opp half is qd %d\r\n",tmpquad);
						UART_PutString(&uart_out_buffer);
						#endif
					}
					else{ // !target_quad==opp_quad
						#ifdef DEBUG_OA
						UART_CPutString("oa> tg_qd!=opp_qd->tg qd\r\n");
						#endif
						oa_goto_quad(oa,target_quad); //0 middle, +1 upper half, -1 lower half
					}
				}
			}
		
		}
	}
	
	/*
	//we calc the dist between me and the opp.
	oa->curr_dist_me_opp =(int16_t)(abs(my_pos_xy.x - oa->opponent_xy.x)*abs(my_pos_xy.x - oa->opponent_xy.x)+abs(my_pos_xy.y - oa->opponent_xy.y)*abs(my_pos_xy.y - oa->opponent_xy.y));
	
	//if we are getting closer, and we are tooo close, we just stop, wait, 
	if((oa->curr_dist_me_opp < oa->prev_dist_me_opp) && (oa->curr_dist_me_opp < MIN_COLLISION_DIST)){
		#ifdef DEBUG_OA
		sprintf(uart_out_buffer,"oa> opp @ %d(^1/2) cm !!!! \r\n",oa->curr_dist_me_opp);
		UART_PutString(&uart_out_buffer);
	#endif
			
	
		trajectory_hardstop(oa->traj);
		oa->state=OA_COLLISION_AVOIDED;
		oa->timeout=gettime();
		
	}
	
	if(oa->state==OA_COLLISION_AVOIDED && (gettime()-oa->timeout)>COLLISION_AVOIDED_TIMEOUT ){
	//hope nothing's broken...
	#ifdef DEBUG_OA
	UART_CPutString("oa> collision avoided ?\r\n");
	#endif
	
	oa->state=OA_TARGET_SET;
	oa->traj->state=READY;
	oa->timeout = 0;
	}
*/
	oa->prev_dist_me_opp = oa->curr_dist_me_opp;
	oa->prev_quad = my_quad;
}
void oa_sixt(struct obstacle_avoidance * oa){
/*we check different possible "next_area" that are in fact the adj_areas
and try to find the shortest, and the most safe one*/

    //we start by computing the possibles next areas
    int16_t my_pos_x,my_pos_y;
	play_area my_area,next_area,target_area,opp_area;
	uint8_t i,j;
    int16_t d_nxt_area_opp[3],d_nxt_area_target[3];
	float tmp;
	uint8_t index0,index1,index2;
	xy_position my_pos_xy;
	
	next_area.x=0;
	next_area.y=0;
	
	my_pos_x=position_get_x_s16(oa->traj->position);
    my_pos_y=position_get_y_s16(oa->traj->position);
    
	my_pos_xy.x = my_pos_x;
	my_pos_xy.y = my_pos_y;
	
	my_area = oa_xy_to_area(my_pos_x,my_pos_y);
	target_area = oa_xy_to_area(oa->target_xy.x,oa->target_xy.y);
	opp_area = oa_xy_to_area(oa->opponent_xy.x,oa->opponent_xy.y);
	
	//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
	char is_opp_adj=oa_is_in_v4(&my_area,&opp_area);
	//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
	
	#ifdef DEBUG_OA
	sprintf(uart_out_buffer,"oa> my area {%d,%d}\r\n",my_area.x,my_area.y);
	UART_PutString(&uart_out_buffer);	
	sprintf(uart_out_buffer,"oa> target area {%d,%d}\r\n",target_area.x,target_area.y);
	UART_PutString(&uart_out_buffer);	
	sprintf(uart_out_buffer,"oa> opp area {%d,%d}\r\n",opp_area.x,opp_area.y);
	UART_PutString(&uart_out_buffer);
	#endif
	/*let's calc adj areas now..*/
	
	oa_find_adj_areas_to_target(oa,&my_area,&target_area);
    
   
    
    /*we check if one or more, of the computed adj areas isn't/aren't one of the forbidden areas
    or just out of the game_area*/
 
    for(i=0;i<3;i++){
        for(j=0;j<NB_FORBIDDEN_AREAS;j++){
		xy_position pos1 =  oa_area_to_xy(oa->adj_area_tab[i]);
	//	xy_position pos2 =  oa_area_to_xy(target_area);
		
            if( ( (oa->adj_area_tab[i].x == oa->forbidden_area[j].x) 
			&& (oa->adj_area_tab[i].y == oa->forbidden_area[j].y)) 
			|| oa_is_out_of_table(oa->adj_area_tab[i]) 
			|| !( oa_get_com_dt(pos1,oa->target_xy)) 
			|| !( oa_get_com_dt(my_pos_xy,pos1))
			|| ((oa->adj_area_tab[i].x ==opp_area.x)&&(oa->adj_area_tab[i].y==opp_area.y))
			){
				/*if adj area is target, just forget about the forbidden areas...*/
					
					
					if((oa->adj_area_tab[i].x == target_area.x) && (oa->adj_area_tab[i].y == target_area.y)){
					//not forbidden areas when it's out target!
						d_nxt_area_opp[i] = 0;
						d_nxt_area_target[i] = 0;
					}
					else {
						//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
						if (is_opp_adj)
						{
							if (oa_is_in_v4(&oa->adj_area_tab[i],&opp_area))
							{
								//on a une case en diagonale, on veut la remplacer
								if( oa->adj_area_tab[i].x==opp_area.x)
									//on remplace par une case du voisinage de my_area qui n'est pas l'adacent
									oa->adj_area_tab[i].x=my_area.x;
								else
									//oa->adj_area_tab[i].y==opp_area.y
									oa->adj_area_tab[i].y=my_area.y;
							}
							else
							{
								d_nxt_area_opp[i] = MARKED_FORBIDDEN;
								d_nxt_area_target[i] = MARKED_FORBIDDEN;
							}
								
						}
						else
						{
							d_nxt_area_opp[i] = MARKED_FORBIDDEN;
							d_nxt_area_target[i] = MARKED_FORBIDDEN;
						}
						//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
					}
					//if((oa->adj_area_tab[i].x != target_area.x) && (oa->adj_area_tab[i].y != target_area.y)){}
					#ifdef DEBUG_OA
					sprintf(uart_out_buffer,"oa> area %d is forb. or out of table!\r\n",i);
					UART_PutString(&uart_out_buffer);
					#endif
				//}
			}
        }     	
    }
   
    /*we calc the dist from opponent and from target for authorized next areas
	we'll choose the shortest one from the target AND the most far from the opponent
	-(NO YET)distance from target has a coeff of 1/2
	-distance from opponent has a coeff 1
	we'll choose the smallest result[]
	*/
	
	uint16_t dist_target_opp = ABS(target_area.x - opp_area.x)*ABS(target_area.x - opp_area.x) + ABS(target_area.y - opp_area.y)*ABS(target_area.y - opp_area.y);
	uint16_t dist_me_opp = ABS(opp_area.x - my_area.x)*ABS(opp_area.x - my_area.x) + ABS(opp_area.y - my_area.y)*ABS(opp_area.y - my_area.y);
	
	if((dist_me_opp > MIN_DIST_BEFORE_IGNORING_OPP) ){ //&& (dist_me_opp > MIN_DIST_BEFORE_IGNORING_OPP)
		/*if we, and the target are too far from the opponent, we ignore the opponent for the next_area choice*/
		#ifdef DEBUG_OA
		sprintf(uart_out_buffer,"oa> opp too far!  d:%d(^1/2) check. d aj<->targ.!\r\n",dist_target_opp);
		UART_PutString(&uart_out_buffer);
		#endif
		
		for(i=0;i<3;i++){
			if( d_nxt_area_target[i] != MARKED_FORBIDDEN){
				d_nxt_area_target[i] = (int16_t)((oa->adj_area_tab[i].x - target_area.x)*(oa->adj_area_tab[i].x - target_area.x) + (oa->adj_area_tab[i].y - target_area.y)*(oa->adj_area_tab[i].y - target_area.y));
			}
		}
		
			/*sorting d_my_area_target ascending (0 shortest, 3 longest)*/
		uint8_t sorted_d_nxt_area_target[3];

		oa_sort_tab3(d_nxt_area_target,sorted_d_nxt_area_target);

		
		index0=sorted_d_nxt_area_target[0];
		index1=sorted_d_nxt_area_target[1];
		index2=sorted_d_nxt_area_target[2];
		
		#ifdef DEBUG_OA
		sprintf(uart_out_buffer,"oa> sorted areas {%d,%d}{%d,%d}{%d,%d}\r\n",
		oa->adj_area_tab[index0].x,oa->adj_area_tab[index0].y,
		oa->adj_area_tab[index1].x,oa->adj_area_tab[index1].y,
		oa->adj_area_tab[index2].x,oa->adj_area_tab[index2].y);
		UART_PutString(&uart_out_buffer);
		#endif
				
		/*electing next area (we have to choose the shortest)*/
		for(i=0;i<3;i++){
		j = sorted_d_nxt_area_target[2-i]; //var reuse
			if(d_nxt_area_target[j]!=MARKED_FORBIDDEN){
				
				next_area = oa->adj_area_tab[j];
			}
		}
	}
	else{
		if(dist_target_opp==0){ //really close
			if(oa->wait<TIMEOUT_WAIT_IN_PLACE){
				#ifdef DEBUG_OA
				sprintf(uart_out_buffer,"oa> opp in same place than target! waiting %d \r\n",oa->wait);
				UART_PutString(&uart_out_buffer);
				#endif
				
				next_area.x = 0;
				next_area.y = 0;
				oa->wait++;
			}
			else{
				/**/
				#ifdef DEBUG_OA
				UART_CPutString("oa> waiting for soooo long... going back to prev quad\r\n");
				#endif
				oa_goto_quad(oa,oa->prev_quad);
			}
		}
		else{
			#ifdef DEBUG_OA
			sprintf(uart_out_buffer,"oa> opp too close d:%d(^1/2) checking dist op<->adj\r\n",dist_me_opp);
			UART_PutString(&uart_out_buffer);
			#endif
			
			for(i=0;i<3;i++){
				if( d_nxt_area_opp[i] != MARKED_FORBIDDEN)
					d_nxt_area_opp[i] = (int16_t)((oa->adj_area_tab[i].x - opp_area.x)*(oa->adj_area_tab[i].x - opp_area.x) + (oa->adj_area_tab[i].y - opp_area.y)*(oa->adj_area_tab[i].y - opp_area.y));
			}
			
			uint8_t sorted_d_nxt_area_opp[3];
	
			oa_sort_tab3(d_nxt_area_opp,sorted_d_nxt_area_opp);
			
			index0= sorted_d_nxt_area_opp[0];
			index1= sorted_d_nxt_area_opp[1];
			index2= sorted_d_nxt_area_opp[2];
			
			#ifdef DEBUG_OA
			sprintf(uart_out_buffer,"oa> sorted areas {%d,%d}{%d,%d}{%d,%d}\r\n",
			oa->adj_area_tab[index0].x,oa->adj_area_tab[index0].y,
			oa->adj_area_tab[index1].x,oa->adj_area_tab[index1].y,
			oa->adj_area_tab[index2].x,oa->adj_area_tab[index2].y);
			
			UART_PutString(&uart_out_buffer);
			#endif
			
			//electing next area (we have to choose the longest)
			for(i=0;i<3;i++){
			j = sorted_d_nxt_area_opp[2-i];
				if(d_nxt_area_opp[j]!=MARKED_FORBIDDEN){
					
					next_area = oa->adj_area_tab[j];
				}
			}
	
		}
	}
	
	/*
	We have to take the farest from the opp, and the shortest from target
	if opp is too far(), we have to ignore it
	*/
	

	//UART_CPutString("oa>election done..\r\n");
	if((next_area.x == target_area.x) && (next_area.y == target_area.y)){
		trajectory_goto_xy_abs(oa->traj,(double)oa->target_xy.x,(double)oa->target_xy.y);	
		oa->state=OA_IN_TARGET;
	}
	else{
		if(next_area.x==0 && next_area.y==0){
			//no one has been elected, stay in place for the moment
			#ifdef DEBUG_OA
			UART_CPutString("oa> sixt: nowhere to go, going back \r\n");
			#endif
			
			next_area.x= oa->prev_area.x;
			next_area.y= oa->prev_area.y;
			
		}
		else{
			#ifdef DEBUG_OA
			sprintf(uart_out_buffer,"oa> sixt: going to area {%d,%d}\r\n",next_area.x,next_area.y);
			UART_PutString(&uart_out_buffer);
			#endif
		}
		xy_position pos;
		pos = oa_area_to_xy(next_area);
		
		/*we save our previous area, is next time we have nowhere to go..*/
		
		trajectory_goto_xy_abs(oa->traj,(double)pos.x,(double)pos.y);
	}
	oa->prev_area.x = next_area.x;
	oa->prev_area.y = next_area.y;
}
int cypress_update( int HW_ver )
{
	// -- This example section of commands show the high-level calls to -------
	// -- perform Target Initialization, SilcionID Test, Bulk-Erase, Target ---
	// -- RAM Load, FLASH-Block Program, and Target Checksum Verification. ----
	unsigned char fIsError = 0;
	//	unsigned char bTry=0;

	// >>>> ISSP Programming Starts Here <<<<

	//    ioctl(touch_fd, DEV_CTRL_TOUCH_INT_DISABLE,NULL);
	//    ioctl(touch_fd, DEV_CTRL_TOUCH_SET_FLAG,NULL);
	printk(KERN_INFO "[TSP] %s, %d, HW ver=%d\n", __func__, __LINE__,HW_ver);
#if defined(CONFIG_MACH_GIO)
	if( HW_ver == 1)
		pSocData = Firmware_Data_HW1;
	else if ( HW_ver == 2 )
		pSocData = Firmware_Data_HW2;
	else if ( HW_ver == 3 || HW_ver == 0 )
		pSocData = Firmware_Data_HW3;
	else if ( HW_ver == 17 )
		pSocData = Firmware_Data_HW11;
	else if ( HW_ver == 33 )
		pSocData = Firmware_Data_HW21;
	else if ( HW_ver == 34)
		pSocData = Firmware_Data_HW22;
	else if ( HW_ver == 35)
		pSocData = Firmware_Data_HW23;	
#elif defined(CONFIG_MACH_COOPER) 
	if( HW_ver == 4 || HW_ver == 3 || HW_ver == 0 )
		pSocData = Firmware_Data_HW3;
#else
	if( HW_ver == 1 )
		pSocData = Firmware_Data_HW1;
	else if( HW_ver == 2 )
		pSocData = Firmware_Data_HW2;
	else if( HW_ver == 3 )
		pSocData = Firmware_Data_HW3;
	else if( HW_ver == 4 )
		pSocData = Firmware_Data_HW4;
#endif
	else
	{
		printk(KERN_INFO "[TSP] %s, %d, HW ver is wrong!!\n", __func__, __LINE__);
		goto update_err;
	}
#ifdef TX_ON
	UART_Start();
	UART_CPutString("Start HSSP - Ovation");
	UART_PutCRLF();
#endif
	// >>>> ISSP Programming Starts Here <<<<

	// Acquire the device through reset or power cycle
#ifdef RESET_MODE
	UART_CPutString("Reset Mode activated");
#else
	//    UART_CPutString("Power Cycle Mode activated");
	// Initialize the Host & Target for ISSP operations
	fIsError = fPowerCycleInitializeTargetForISSP();
	if (fIsError )
	{
		printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
		ErrorTrap(fIsError);
		goto update_err;
	}
#endif

//	printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
#ifdef TX_ON
	UART_PutCRLF();
	UART_CPutString("Verify SiliconID");
#endif

//	printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);

	// Run the SiliconID Verification, and proceed according to result.
#if !defined(CONFIG_MACH_TASS) && !defined(CONFIG_MACH_TASSDT) && !defined(CONFIG_MACH_GIO)
	fIsError = fVerifySiliconID();
#endif
	if (fIsError )
	{
		printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
		ErrorTrap(fIsError);
		goto update_err;
	}

#ifdef TX_ON
	UART_PutCRLF();
	UART_CPutString("End VerifySiliconID");
#endif

//	printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);

#if 1
	// Bulk-Erase the Device.
	fIsError = fEraseTarget();
	if (fIsError )
	{
		printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
		ErrorTrap(fIsError);
		goto update_err;
	}

//	printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
#ifdef TX_ON
	UART_PutCRLF();
	UART_CPutString("End EraseTarget");
	UART_PutCRLF();
	UART_CPutString("Program Flash Blocks Start");
	UART_PutCRLF();
#endif

#endif

//	printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
#if 1   // program flash block
	//LCD_Char_Position(1, 0);
	//LCD_Char_PrintString("Program Flash Blocks Start");

	//==============================================================//
	// Program Flash blocks with predetermined data. In the final application
	// this data should come from the HEX output of PSoC Designer.

	iChecksumData = 0;     // Calculte the device checksum as you go
	for (iBlockCounter=0; iBlockCounter<BLOCKS_PER_BANK; iBlockCounter++)
	{
		fIsError = fReadWriteSetup();
		if (fIsError )
		{
			printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
			ErrorTrap(fIsError);
			goto update_err;
		}

		//LoadProgramData(bBankCounter, (unsigned char)iBlockCounter);
		iChecksumData += iLoadTarget(iBlockCounter);

		fIsError = fProgramTargetBlock(bBankCounter,(unsigned char)iBlockCounter);
		if (fIsError )
		{
			printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
			ErrorTrap(fIsError);
			goto update_err;
		}

		fIsError = fReadStatus();
		if (fIsError)
		{
			printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
			ErrorTrap(fIsError);
			goto update_err;
		}

#ifdef TX_ON
		UART_PutChar('#');
#endif

	}

#ifdef TX_ON
	UART_PutCRLF();
	UART_CPutString("Program Flash Blocks End");
#endif

#endif


#if 1  // verify
#ifdef TX_ON
	UART_PutCRLF();
	UART_CPutString("Verify Start");
	UART_PutCRLF();
#endif

	//=======================================================//
	//PTJ: Doing Verify
	//PTJ: this code isnt needed in the program flow because we use PROGRAM-AND-VERIFY (ProgramAndVerify SROM Func)
	//PTJ: which has Verify built into it.
	// Verify included for completeness in case host desires to do a stand-alone verify at a later date.

	for (iBlockCounter=0; iBlockCounter<BLOCKS_PER_BANK; iBlockCounter++)
	{
		//LoadProgramData(bBankCounter, (unsigned char) iBlockCounter);

		fIsError = fReadWriteSetup();
		if (fIsError )
		{
			printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
			ErrorTrap(fIsError);
			goto update_err;
		}

		fIsError = fVerifySetup(bBankCounter,(unsigned char)iBlockCounter);
		if (fIsError)
		{
			printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
			ErrorTrap(fIsError);
			goto update_err;
		}

		fIsError = fReadStatus();
		if (fIsError ) {
			printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
			ErrorTrap(fIsError);
			goto update_err;
		}

		fIsError = fReadWriteSetup();
		if (fIsError)  {
			printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
			ErrorTrap(fIsError);
			goto update_err;
		}

		fIsError = fReadByteLoop(iBlockCounter);
		if (fIsError ) {
			printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
			ErrorTrap(fIsError);
			goto update_err;
		}

#ifdef TX_ON
		UART_PutChar('.');
#endif

	}

#ifdef TX_ON
	UART_PutCRLF();
	UART_CPutString("Verify End");
#endif

#endif // end verify

#if 1
#ifdef TX_ON
	UART_PutCRLF();
	UART_CPutString("Security Start");
#endif


	//=======================================================//
	// Program security data into target PSoC. In the final application this
	// data should come from the HEX output of PSoC Designer.
	for (bBankCounter=0; (bBankCounter<NUM_BANKS) ; bBankCounter++)
	{
		//PTJ: READ-WRITE-SETUP used here to select SRAM Bank 1

		fIsError = fReadWriteSetup();
		if (fIsError )
		{
			printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
			ErrorTrap(fIsError);
			goto update_err;
		}
		// Load one bank of security data from hex file into buffer
   		fIsError = fLoadSecurityData(bBankCounter);
		if (fIsError )
		{
			printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
			ErrorTrap(fIsError);
			goto update_err;
		}
		// Secure one bank of the target flash
		fIsError = fSecureTargetFlash();
		if (fIsError )
		{
			printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
			ErrorTrap(fIsError);
			goto update_err;
		}
	}

#ifdef TX_ON
	UART_PutCRLF();
	UART_CPutString("End Security data");
#endif

#endif


#if 1   // checksum
#ifdef TX_ON
	UART_PutCRLF();
	UART_CPutString("CheckSum Start");
#endif

//	printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
	//PTJ: Doing Checksum
	iChecksumTarget = 0;
	fIsError = fAccTargetBankChecksum(&iChecksumTarget);
	if (fIsError )
	{
		printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
		ErrorTrap(fIsError);
		goto update_err;
	}

//	printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);
#ifdef TX_ON
	UART_PutCRLF();
	UART_CPutString("Checksum : iChecksumTarget (0x");
	UART_PutHexWord(iChecksumTarget);
	UART_CPutString("), iChecksumData (0x");
	UART_PutHexWord(iChecksumData);
	UART_CPutString(")");
#endif

	iChecksumTarget = iChecksumTarget & 0xFFFF;
	iChecksumData = iChecksumData & 0xFFFF;

	if (iChecksumTarget != iChecksumData)
	{
		printk(KERN_INFO "[TSP] %s, %d, iChecksumTarget=%d, iChecksumData=%d\n", __func__, __LINE__,iChecksumTarget, iChecksumData );
		ErrorTrap(CHECKSUM_ERROR);
		goto update_err;
	}

#ifdef TX_ON
	UART_PutCRLF();
	UART_CPutString("End Checksum");
#endif

#endif

	// *** SUCCESS ***
	// At this point, the Target has been successfully Initialize, ID-Checked,
	// Bulk-Erased, Block-Loaded, Block-Programmed, Block-Verified, and Device-
	// Checksum Verified.

	// You may want to restart Your Target PSoC Here.
	ReStartTarget();
//	printk(KERN_INFO "[TSP] %s, %d\n", __func__, __LINE__);

	return 1;

update_err:
	return 0;
}
Exemple #19
0
// ============================================================================
// fVerifySiliconID()
// Returns:
//     0 if successful
//     Si_ID_ERROR if timed out on handshake to the device.
// ============================================================================
signed char fVerifySiliconID(void)
{
	SendVector(id_setup_2, num_bits_id_setup_2);

	fIsError = fDetectHiLoTransition();
	if (fIsError)
	{
#ifdef TX_ON
		UART_PutCRLF();
		UART_CPutString("fDetectHiLoTransition Error");
#endif
		return(SiID_ERROR);
	}
	SendVector(wait_and_poll_end, num_bits_wait_and_poll_end);

	SendVector(tsync_enable, num_bits_tsync_enable);

	//Send Read ID vector and get Target ID
	SendVector(read_id_v, 11);      // Read-MSB Vector is the first 11-Bits
	RunClock(2);                    // Two SCLK cycles between write & read
	bTargetID[0] = bReceiveByte();
	RunClock(1);
	SendVector(read_id_v+2, 12);    // 1+11 bits starting from the 3rd byte

	RunClock(2);                    // Read-LSB Command
	bTargetID[1] = bReceiveByte();

	RunClock(1);
	SendVector(read_id_v+4, 1);     // 1 bit starting from the 5th byte

	//read Revision ID from Accumulator A and Accumulator X
	//SendVector(read_id_v+5, 11);	//11 bits starting from the 6th byte
	//RunClock(2);
	//bTargetID[2] = bReceiveByte();	//Read from Acc.X
	//RunClock(1);
	//SendVector(read_id_v+7, 12);    //1+11 bits starting from the 8th byte
	//
	//RunClock(2);
	//bTargetID[3] = bReceiveByte();	//Read from Acc.A
	//
	//RunClock(1);
	//SendVector(read_id_v+4, 1);     //1 bit starting from the 5th byte,

	SendVector(tsync_disable, num_bits_tsync_disable);


#ifdef TX_ON
	// Print READ-ID
	UART_PutCRLF();
	UART_CPutString("Silicon-ID : ");
	UART_PutChar(' ');
	UART_PutHexByte(bTargetID[0]);
	UART_PutChar(' ');
	UART_PutHexByte(bTargetID[1]);
	UART_PutChar(' ');
#endif

#ifdef LCD_ON
	LCD_Char_Position(1, 0);
	LCD_Char_PrintString("ID : ");
	LCD_Char_PrintInt8(bTargetID[0]);
	LCD_Char_PutChar(' ');
	LCD_Char_PrintInt8(bTargetID[1]);
	LCD_Char_PutChar(' ');
#endif

	if (bTargetID[0] == target_id_v[0] && bTargetID[1] == target_id_v[1])
	{
		return(PASS);
	}
	else if (bTargetID[0] == target_id_v2[0] && bTargetID[1] == target_id_v2[1])	
	{
		return(PASS);
	}
	else
	{
		printk("%x %x \n", bTargetID[0], bTargetID[1]);
		return(SiID_ERROR);
	}
}
Exemple #20
0
void serial_print(unsigned char *str) {
	UART_PutString(str);
	UART_CPutString("\r\n");
}
Exemple #21
0
void serial_cprint(const unsigned char *str)
{
	UART_CPutString(str);
	UART_CPutString("\r\n");
}