RP6 Ultraschallsensor SRF08

[Marian Otte]

so binde ich den if satz ein:

#include "RP6ControlLib.h" 		
#include "RP6I2CmasterTWI.h"
#include "RP6Control_I2CMasterLib.h"  

#define SRF_ADR  0xE2
#define SRF_ADRS1 0xE4
#define SRF_ADRS2 0xE6


void changeSRFAddr(uint8_t old_addr, uint8_t new_addr)
{
	I2CTWI_transmit2Bytes(old_addr, 0, 0xA0);
	I2CTWI_transmit2Bytes(old_addr, 0, 0xAA);
	I2CTWI_transmit2Bytes(old_addr, 0, 0xA5);
	I2CTWI_transmit2Bytes(old_addr, 0, new_addr);
}


#define MEASURE_US_LOW 	0
#define MEASURE_US_HIGH 1

uint16_t distance;

void I2C_requestedDataReady(uint8_t dataRequestID)
{
	uint8_t messageBuf[8];
	static uint8_t dist_tmp;
	switch(dataRequestID)
	{
		case MEASURE_US_HIGH: 
			I2CTWI_getReceivedData(messageBuf, 2);
			dist_tmp = (messageBuf[0]);
			I2CTWI_transmitByte(SRF_ADR, 3);
			I2CTWI_requestDataFromDevice(SRF_ADR, MEASURE_US_LOW, 1);
		break;
		case MEASURE_US_LOW:
			I2CTWI_getReceivedData(messageBuf, 2);
			distance = messageBuf[0] + (dist_tmp << 8);
			writeStringLCD("Distance: ");
			writeIntegerLCD(distance, DEC);
			writeStringLCD(" cm | ");
			
			writeCharLCD('\n');
			setCursorPosLCD(0,0);
			
		break;
	}
}

void I2C_transmissionError(uint8_t errorState)
{
	writeString_P("\nI2C ERROR --> TWI STATE IS: 0x");
	writeInteger(errorState, HEX);
	writeChar('\n');
}

void task_SRF(void)
{
	static uint8_t measureInProgress = false;
	if(!measureInProgress) 
	{
		if(TWI_operation == I2CTWI_NO_OPERATION)
		{
			I2CTWI_transmit2Bytes(SRF_ADR, 0, 81);
			measureInProgress = true;
			setStopwatch1(0);
		}
	}
	else if(getStopwatch1() > 70)
	{
		I2CTWI_transmitByte(SRF_ADR, 2);
		I2CTWI_requestDataFromDevice(SRF_ADR, MEASURE_US_HIGH, 1);
		measureInProgress = false;
		setStopwatch1(0);
	}
	
	
	else if(getStopwatch1() > 70)
	{
		I2CTWI_transmitByte(SRF_ADR, 2);
		I2CTWI_requestDataFromDevice(SRF_ADRS1, MEASURE_US_HIGH, 1);
		measureInProgress = false;
		setStopwatch1(0);
	}
}

int main(void)
{
    initRP6Control(); 
	initLCD();
    I2CTWI_initMaster(100);
	I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);
	I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);

	setLEDs(0b111111);
	mSleep(500);	   
	setLEDs(0b000000);
	
	startStopwatch1();
        
	
	while(true)  
	{
		task_SRF();
		task_I2CTWI();
		if(bumper_left || bumper_right) 
	{
		moveAtSpeed(0,0);
        rotate(50, LEFT, 90, true);
        moveAtSpeed(80,80);
		setLEDs(0b010000);
		startStopwatch1();
	}
	}
	return 0;
}

[Dirk]

Hi Marian,

1. Dieser rotate-Befehl blockiert die ganze Hauptschleife, die aber möglichst schnell abgearbeitet werden muss (ohne Pausen!).
2. Für die Bewegungsbefehle muss immer die task_motionControl() in der Hauptschleife aufgerufen werden.
3. Du solltest hier im Forum deine Programme in (Code)...Programm...(/Code) setzen, das bleibt dann übersichtlicher (Die runden Klammern nur durch eckige ersetzen!) und sieht dann so aus:

...Programm...

[Marian Otte]

das problem ist aber, das das Atmega 32 task moution control nicht kennt

- - - Aktualisiert - - -

außerdem bekomme ich das mit den bumpern nicht hin, das Atmega reagiert nicht auf die bumper die leds auf der robot base blinken aber

[Marian Otte]

so das mit den bumpern wäre geschafft, da bleibt nur noch das mit den ultraschallsenoren kann mir eventuell jemand ein code beispiel schicken mit dem ich 10cm vor der wand stehen bleiben kann, dann um 90° drehen und dann weiter fahren kann.

Danke Marian

[Dirk]

Marian:

problem ist aber, das das Atmega 32 task moution control nicht kennt

Sorry! Du arbeitest ja mit der M32 und nicht direkt mit der Base. Hatte ich wieder übersehen.

Hast du es denn schon geschafft, das Auslesen der US-Sensoren in das I2C-Master-Programm auf der M32 einzubauen?
Bau das Programm erstmal so auf, dass dir alle 3 Sekunden ein Entfernungswert angezeigt wird.
Die Move-Befehle kommen danach.

Welches M32-Demoprogramm hast du denn als Grundlage für dein Programm genommen?

[Marian Otte]

ja ich habe es schon geschafft ich arbeite mit dem I2C-Master 03.
Wie die move befehle kommen danach?

[Marian Otte]

außerdem habe ich noch nicht ganz verstanden wie ich von den low und hight byte zu cm komme

[Dirk]

Das I2C-Master 03 Programm ist ja das für den SRF08.
Was du brauchst, um den RP6 mit der M32 über I2C anzusprechen, ist z.B. das Progamm Example_09_Move aus den RP6CONTROL_EXAMPLES.
Da siehst du, wie der RP6 bewegt wird. In dieses Programm müßtest du den SRF08-Kram einfügen.

[Marian Otte]

ja danke mach ich ma sofort

[Marian Otte]

Hallo

Ein problem habe ich noch und zwa in diesem code soll der roboter nachdem der vordere Sensor und 10 cm ist sich um 90° drehen. Aber das problem ist wenn ich diese bedingung in die while schleife der Rampenvoid einbaue misst der Roboter keinen ultraschall sensor mehr aus. Warum?
Der Roboter überspring quasi das ganze rampenprogramm und geht sofort in secund rein.

#include "RP6ControlLib.h" 		 
								// Always needs to be included!

#include "RP6I2CmasterTWI.h"	// I2C Master Library


/*****************************************************************************/
/*****************************************************************************/
// Include our new "RP6 Control I2C Master library":

#include "RP6Control_I2CMasterLib.h"

#define SRF_LEFT_ADR  0xE4
#define SRF_RIGHT_ADR 0xE6
#define SRF_FRONT_ADR 0xE2

#define MEASURE_US_LEFT_LOW 	0
#define MEASURE_US_LEFT_HIGH 	1
#define MEASURE_US_RIGHT_LOW 	2
#define MEASURE_US_RIGHT_HIGH 	3
#define MEASURE_US_FRONT_LOW 	4
#define MEASURE_US_FRONT_HIGH 	5

void second(void)
{
rotate(50, LEFT, 90, true);
}

void task_SRF(void)


{  
	static uint8_t measureInProgress = false;
	static uint8_t channel = 0;
	if(!measureInProgress)
	{
		if(TWI_operation == I2CTWI_NO_OPERATION)
		{
			if(channel == 0)
				I2CTWI_transmit2Bytes(SRF_LEFT_ADR, 0, 81);
			else if(channel == 1)
				I2CTWI_transmit2Bytes(SRF_RIGHT_ADR, 0, 81);
			else if(channel == 2)
			    I2CTWI_transmit2Bytes(SRF_FRONT_ADR, 0, 81);
				measureInProgress = true;
			setStopwatch1(0);
		}
	}
	else if(getStopwatch1() > 70)
	{
		if(channel == 0)
		{
			I2CTWI_transmitByte(SRF_LEFT_ADR, 2);
			I2CTWI_requestDataFromDevice(SRF_LEFT_ADR, MEASURE_US_LEFT_HIGH, 1);
			channel = 1;
		}
		else if(channel == 1)
		{
			I2CTWI_transmitByte(SRF_RIGHT_ADR, 2);
			I2CTWI_requestDataFromDevice(SRF_RIGHT_ADR, MEASURE_US_RIGHT_HIGH, 1);
			channel = 2;
		}
		else if(channel == 2)
		{  
		I2CTWI_transmitByte(SRF_FRONT_ADR, 2);
			I2CTWI_requestDataFromDevice(SRF_FRONT_ADR, MEASURE_US_FRONT_HIGH, 1);
			channel = 0;
		}
		
		measureInProgress = false;
		setStopwatch1(0);
		
	}

}

   
uint16_t distance_left = 0;
uint16_t distance_right = 0;
uint16_t distance_front = 0;

void I2C_requestedDataReady(uint8_t dataRequestID)
{
	uint8_t messageBuf[8];
	static uint8_t dist_tmp;
	switch(dataRequestID)
	{
		case MEASURE_US_LEFT_HIGH:
			I2CTWI_getReceivedData( messageBuf, 2 );
			dist_tmp = messageBuf[0];
			I2CTWI_transmitByte(SRF_LEFT_ADR, 3);
			I2CTWI_requestDataFromDevice(SRF_LEFT_ADR, MEASURE_US_LEFT_LOW, 1);
		break;
		case MEASURE_US_LEFT_LOW:
			I2CTWI_getReceivedData( messageBuf, 2 );
			distance_left = messageBuf[0] + (dist_tmp << 8);
			writeString_P("DistanceL: ");
			writeInteger(distance_left, DEC);
			writeString_P(" cm ");
		break;
		case MEASURE_US_RIGHT_HIGH:
			I2CTWI_getReceivedData( messageBuf, 2 );
			dist_tmp = messageBuf[0];
			I2CTWI_transmitByte(SRF_RIGHT_ADR, 3);
			I2CTWI_requestDataFromDevice(SRF_RIGHT_ADR, MEASURE_US_RIGHT_LOW, 1);
		break;
		case MEASURE_US_RIGHT_LOW:
			I2CTWI_getReceivedData( messageBuf, 2 );
			distance_right = messageBuf[0] + (dist_tmp << 8);
			writeString_P("\t\tDistanceR: ");
			writeInteger(distance_right, DEC);
			writeString_P(" cm\n");
		break;
		case MEASURE_US_FRONT_HIGH:
			I2CTWI_getReceivedData( messageBuf, 2 );
			dist_tmp = messageBuf[0];
			I2CTWI_transmitByte(SRF_FRONT_ADR, 3);
			I2CTWI_requestDataFromDevice(SRF_FRONT_ADR, MEASURE_US_FRONT_LOW, 1);
		break;
		case MEASURE_US_FRONT_LOW:
			I2CTWI_getReceivedData( messageBuf, 2 );
			distance_front = messageBuf[0] + (dist_tmp << 8);
			writeString_P("\t\tDistanceF: ");
			writeInteger(distance_front, DEC);
			writeString_P(" cm\n");
		break;
	}
}



void rampe(void)
{
if (readADC(ADC_6) > 1000)
	{
	    showScreenLCD("Rampe","erkannt");
		changeDirection(FWD);
		while(distance_front < 10)
		{
		task_SRF();
		moveAtSpeed(30,30);
		if (distance_front < 10)
		{
		second();
		}
		if (distance_right < 10.5)
		{
		moveAtSpeed(80,20);
		}
		if (distance_left < 10)
		moveAtSpeed(20,80);
		}
		return 0;
		
	}

}

void linienverfolgung(void)
{
while(readADC(ADC_6) < 1000)
{ 
	{	
	
	    {   uint16_t adc_2 = readADC(ADC_2);
			setCursorPosLCD(0, 13); 
			writeIntegerLengthLCD(adc_2, DEC, 3);
			
			
		  uint16_t adc_3 = readADC(ADC_3);
			setCursorPosLCD(1, 13); 
			writeIntegerLengthLCD(adc_3, DEC, 3);
		}
		
		
       
	    
			
	    writeInteger(readADC(ADC_2), 10);
		writeString_P(" - ");
		writeInteger(readADC(ADC_3), 10);
		writeString_P("\n\r");
		writeInteger(readADC(ADC_4), 10);
		writeString_P(" - ");
		writeInteger(readADC(ADC_5), 10);
		writeString_P("\n\r");
		writeInteger(readADC(ADC_6), 10);
		writeString_P(" - ");
		
		
	
	if (readADC(ADC_4) < 400)
		{
		
		moveAtSpeed(10,80);
		
		}
		
		if (readADC(ADC_5) < 400)
		{
		
		moveAtSpeed(80,10);
		
		}
	
	if (readADC(ADC_2) < 400)
		{
		changeDirection(LEFT);
		moveAtSpeed(40,70);
		
		}
		
		if (readADC(ADC_3) < 400)
		{
		changeDirection(RIGHT);
		moveAtSpeed(70,40);
		
		}
		
		if (readADC(ADC_2) + readADC(ADC_3) > 1025)
        {
		changeDirection(FWD);
		moveAtSpeed(60,60);
		}
		
		if (readADC(ADC_6) > 1000)
		{
		moveAtSpeed(0,0);
		mSleep(1000);
		rampe();
		}
	}
	
}


	
    if(bumper_left || bumper_right)
	{
	clearLCD();
	showScreenLCD("Gegenstand","erkannt");
	move(60, BWD, DIST_MM(55), true);
	rotate(50, RIGHT, 50, true);
	move(60, FWD, DIST_MM(200), true);
	rotate(50, LEFT, 50, true);
	move(60, FWD, DIST_MM(150), true);
	rotate(50, LEFT, 60, true);
	
	}
}




/**
 * Prints all Sensor Values on the Serial Interface.
 */
void printAllSensorValues(void)
{
	getAllSensors();		
	writeString_P("\nRead Sensor Values:\n");
	writeString_P("PL:");writeIntegerLength(mleft_power,DEC,3);
	writeString_P(" | PR:");writeIntegerLength(mright_power,DEC,3);
	writeString_P(" | VL:");writeIntegerLength(mleft_speed,DEC,3);
	writeString_P(" | VR:");writeIntegerLength(mright_speed,DEC,3);
	writeString_P(" | DL:");writeIntegerLength(mleft_des_speed,DEC,3);
	writeString_P(" | DR:");writeIntegerLength(mright_des_speed,DEC,3);
	writeChar('\n');
	writeString_P("DSTL:");writeIntegerLength(mleft_dist,DEC,5);
	writeString_P(" | DSTR:");writeIntegerLength(mright_dist,DEC,5);
	writeChar('\n');
	writeString_P("LSL:");writeIntegerLength(adcLSL,DEC,4);
	writeString_P(" | LSR:");writeIntegerLength(adcLSR,DEC,4);
	writeString_P(" | MCL:");writeIntegerLength(adcMotorCurrentLeft,DEC,4);
	writeString_P(" | MCR:");writeIntegerLength(adcMotorCurrentRight,DEC,4);
	writeString_P(" | BAT:");writeIntegerLength(adcBat,DEC,4);
	writeString_P(" | AD0:");writeIntegerLength(adc0,DEC,4);
	writeString_P(" | AD1:");writeIntegerLength(adc1,DEC,4);
	writeChar('\n');
}






/*****************************************************************************/
// I2C Error handler

/**
 * This function gets called automatically if there was an I2C Error like
 * the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
 */
void I2C_transmissionError(uint8_t errorState)
{
	writeString_P("\nI2C ERROR - TWI STATE: 0x");
	writeInteger(errorState, HEX);
	writeChar('\n');
}

/*****************************************************************************/
// Main function - The program starts here:

int main(void)
{
	initRP6Control();  
	initLCD();
    
	// ---------------------------------------
	
	
	

	// ---------------------------------------
	I2CTWI_initMaster(100);  
	I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);
	I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);


	sound(180,80,25);
	sound(220,80,25);

	setLEDs(0b1111);

	showScreenLCD("", "");
	mSleep(500);
	showScreenLCD("", "");
	mSleep(1000);
	setLEDs(0b0000);
	
  

	startStopwatch1();
	startStopwatch2();
	linienverfolgung();


	while(true) 
	{ 

		task_checkINT0();
	    task_I2CTWI();
	}
	return 0;
}

Marian Otte