RP6 Ultraschallsensor SRF08

[Marian Otte]

Hallo,

ich wollte gerne wissen wo und wie ich 3 SRF08 Ultraschallsensoren am RP6 oder an seinem Erweiterungsboard m32 anschließen kann.
Außerdem habe ich noch keine Idee wie ich den Ultraschallsensor abfragen kann. Kann mir jemand vieleicht ein Codebeispiel senden.



Danke
Marian Otte

[Dirk]

Anschluß: An den I2C-Bus (SCL/SDA).
Programmbeispiel: RP6Base_I2CMaster_03 (Beispiel wird mit dem RP6 mitgeliefert).

[Marian Otte]

vielen dank

Marian Otte

[Marian Otte]

ach ja und außerdem möchte ich gerne noch wissen wie das programm aussieht wenn ich die Ultrschallsensoren an das Atmega 32 anschließen will?
Der Rp6 läuft im Slave modus.
Ich habe das beispielpgrogramm von der base für das Atmega 32 umgeschrieben aber irgenwie funktioniert das nicht.

Danke Marian Otte

Das sähe dann bei mir so aus:



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

#define SRF_ADR 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);

writeString_P("Distance: ");
writeInteger(distance, DEC);
writeString_P(" cm | ");
uint16_t dist_count;
for(dist_count = distance; dist_count > 4; dist_count-=2)
writeChar('#');
writeChar('\n');
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);
}
}

int main(void)
{


I2CTWI_initMaster(100);
I2CTWI_setRequestedDataReadyHandler(I2C_requestedD ataReady);
I2CTWI_setTransmissionErrorHandler(I2C_transmissio nError);

setLEDs(0b111111);
mSleep(500);
setLEDs(0b000000);



startStopwatch1();

while(true)
{
task_SRF();
task_I2CTWI();

}
return 0;
}

ich bekomme das mit dem Code einfügen einfach nicht hin

[Dirk]

Dein Code dürfte auch nicht ohne Fehler kompilieren.

Versuch mal, nicht den Code "einzufügen", sondern so wie er ist an die M32 anzupassen:
- ControlLib anstelle der BaseLib einbinden
- Im Main:
-- ControlLib initialisieren
-- setLEDs auf 4 Einsen/Nullen in der Klammer ändern: setLEDs(0b1111)
-- powerON() entfernen
-- task_RP6System() entfernen

[Marian Otte]

habe ich doch und ich meinte den code in Das forum einfügen und nicht in den Roboter :)
also sollte das programm geändert auch auf dem m32 funktionieren oder nicht?

Danke Marian Otte

[Dirk]

Ja, das Programm sollte mit den genannten Änderungen auf der M32 laufen.
Bei deinem Code sehe ich aber z.B. nicht die Initialisierung der ControlLib im Main: initRP6Control();

[Marian Otte]

jo vielen danke du hast mir mal wieder sehr gut geholfen. Werde es gleich mal durchprobieren.

[Marian Otte]

ich habe jetzt alles angeschossen und mit diesem programm kann ich mir auch die werte eines sensors ausgeben lassen.



#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_requestedD ataReady);
I2CTWI_setTransmissionErrorHandler(I2C_transmissio nError);

setLEDs(0b111111);
mSleep(500);
setLEDs(0b000000);

startStopwatch1();


while(true)
{
task_SRF();
task_I2CTWI();
}
return 0;
}


Aber wenn ich eine if funktion z.B. If (distance < 10)
{
rotate(50, Left, 90, true);
moveAtSpeed(80,80);
}
einbinde fährt der Roboter immer nur die Rotate funktion.
Kann mir jemand bitte helfen was ich jetzt noch umschreiben muss damit meine funktion klagpp.
Außerdem möchte ich noch wissen wie ich mehere sensoren in einem Programm auslesen kann und die angabe dann in cm ausgegeben wird.

Würde mich über eine Antwort sehr freuen.

Marian Otte

- - - Aktualisiert - - -

Außerdem habe ich noch das prolem mit den bumpern, ich bekomme es nicht hin die vom atmega 32 aus abzufragen.
Kann mir jemand helfen.

Marian Otte

[Dirk]

Zeig mal, wie du den if-Teil genau einbindest!

[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_requestedD ataReady);
I2CTWI_setTransmissionErrorHandler(I2C_transmissio nError);

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_requestedD ataReady);
I2CTWI_setTransmissionErrorHandler(I2C_transmissio nError);


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

[Marian Otte]

oder gibt es vielleicht ein beispielprogramm was ich benutzen kann ?

Danke Marian Otte

[Max Web]

Hallo,


Aber das problem ist wenn ich diese bedingung in die while schleife der Rampenvoid einbaue misst der Roboter keinen ultraschall sensor mehr aus. Warum?
Die Ursache dafür ist der blockierende Aufruf der Fahrbefehle. Der RP6 blockiert Dein Hauptprogramm so lange bis die Bewegung abgeschlossen ist. Was Du benötigst ist ein nichtblockierender Aufruf,
was jedoch mit mehr Aufwand verbunden ist. Das Stichwort hier lautet: Zustandsmaschine.
Schau Dir dazu doch mal eines der verhaltensgesteuerten Move-Programme an - die bieten eigentlich genau das was Du benötigst (und sogar noch mehr)!
Du könntest den Ultraschallsensor z.B. mit einem der beiden Bumper/dem ACS koppeln (d.h. einfach in die passenden if-Abfragen mit einbauen) oder auch ein neues Behavior speziell für den SRF schreiben.
Dazu könntest Du das avoid/escape Behavior als Vorlage benutzen.


außerdem habe ich noch nicht ganz verstanden wie ich von den low und hight byte zu cm komme
Der SRF misst die Distanz bereits in cm, da Du ihm mit
I2CTWI_transmit2Bytes(SRF_LEFT_ADR, 0, 81); den Befehl dazu gibst.
Allerdings ist der gemessene Entfernungswert 16 Bit (2 Register im SRF) breit, mit der I2C-Lib kannst Du allerdings nur Register zu 8 Bit auslesen. Daher werden die 16 Bit in zwei 8 Bit-Zahlen aufgesplittet, übertragen und danach wieder zusammengesetzt (dann hast Du die Distanz in cm). Das High Byte enthält dabei die "oberen" 8 Bit, d.h. diejenigen, bei denen das Most Significant Bit enthalten ist. Das Low Byte enthält die verbleibenden 8 Bit. Ich hoffe, dass das die Frage beantwortet und einigermaßen verständlich ist ;)

[Marian Otte]

vielen danke ich werde mir das beispielprogramm noch ma angucken und auch für mich änder.

Wenn ich noch Fragen habe poste ich diese einfache.

Danke Marian Otte

[Marian Otte]

Hallo,

jetzt habe ich versucht das linienprogramm mit einzubinden und zwa in den cruise state aber es klapp nicht.
Der roboter fährt die linie jetzt sehr lansam ab er stock immer wieder in gleichen abständen.



/*
* ************************************************** **************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M32 Examples
* ************************************************** **************************
* Example: I2C Master 10 - Behaviour based Robot
* Author(s): Dominik S. Herwald
* ************************************************** **************************
* Description:
* This is a rahter large example. We ported one of the behaviour based Robot
* Examples for the RP6 CONTROL M32 and added some new Behaviours.
* Now there is a Behaviour that lets the Robot wait until you clapped your
* hands three times or made some other noises.
* There is also a behaviour that checks if the Battery voltage is too low.
* If this is the case, the robot is stopped.
*
* Of course the Robot still drives around and tries to avoid collisions.
* The only difference is that it is controlled by the RP6 CONTROL M32.
*
* ################################################## ##########################
* #+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+ #+#+#+#+#+#+#+#+#+#+#+#+#+
*
* ATTENTION: THE ROBOT MOVES AROUND IN THIS EXAMPLE! PLEASE PROVIDE ABOUT
* 2m x 2m OR MORE FREE SPACE FOR THE ROBOT!
*
* >>> DO NOT FORGET TO REMOVE THE FLAT CABLE CONNECTION TO THE USB INTERFACE
* BEFORE YOU START THIS PROGRAM BY PRESSING THE START BUTTON ON THE ROBOT!
*
* #+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+#+ #+#+#+#+#+#+#+#+#+#+#+#+#+
* ################################################## ##########################
* ************************************************** **************************
*/

/************************************************** ***************************/
// Includes:

#include "RP6ControlLib.h" // The RP6 Control Library.
// Always needs to be included!

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


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

#include "RP6Control_I2CMasterLib.h"

/************************************************** ***************************/


/************************************************** ***************************/
// Behaviour command type:

#define IDLE 0

// The behaviour command data type:
typedef struct {
uint8_t speed_left; // left speed (is used for rotation and
// move distance commands - if these commands are
// active, speed_right is ignored!)
uint8_t speed_right; // right speed
unsigned dir:2; // direction (FWD, BWD, LEFT, RIGHT)
unsigned move:1; // move flag
unsigned rotate:1; // rotate flag
uint16_t move_value; // move value is used for distance and angle values
uint8_t state; // state of the behaviour
} behaviour_command_t;

behaviour_command_t STOP = {0, 0, FWD, false, false, 0, IDLE};

/************************************************** ***************************/
// Cruise Behaviour:

#define CRUISE_SPEED_FWD 80 // Default speed

#define MOVE_FORWARDS 1
behaviour_command_t cruise = {0, 0, FWD, false, false, 0, IDLE};

/**
* Cruise Behaviour
*/
void behaviour_cruise(void)
{
while(true)
{
{

{ 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(10,10);
}
}
return 0 ;
}
}

/************************************************** ***************************/
// Escape Behaviour:

#define ESCAPE_SPEED_BWD 80
#define ESCAPE_SPEED_ROTATE 60

#define ESCAPE_FRONT 1
#define ESCAPE_FRONT_WAIT 2
#define ESCAPE_LEFT 3
#define ESCAPE_LEFT_WAIT 4
#define ESCAPE_RIGHT 5
#define ESCAPE_RIGHT_WAIT 6
#define ESCAPE_WAIT_END 7
behaviour_command_t escape = {0, 0, FWD, false, false, 0, IDLE};

/**
* This is the Escape behaviour for the Bumpers.
*/
void behaviour_escape(void)
{
switch(escape.state)
{
case ESCAPE_FRONT:
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);
clearLCD();
escape.state = IDLE;
break;
}

}
/**
* Bumpers Event handler
*/
void bumpersStateChanged(void)
{
if(bumper_left || bumper_right)
{
sound(200,100,0);
escape.state = ESCAPE_FRONT;
}

}

/************************************************** ***************************/
// Avoid Behaviour:

#define AVOID_SPEED_L_ARC_LEFT 20
#define AVOID_SPEED_L_ARC_RIGHT 80
#define AVOID_SPEED_R_ARC_LEFT 80
#define AVOID_SPEED_R_ARC_RIGHT 20

#define AVOID_SPEED_ROTATE 60

#define AVOID_OBSTACLE_RIGHT 1
#define AVOID_OBSTACLE_LEFT 2
#define AVOID_OBSTACLE_MIDDLE 3
#define AVOID_OBSTACLE_MIDDLE_WAIT 4
#define AVOID_END 5
behaviour_command_t avoid = {0, 0, FWD, false, false, 0, IDLE};

/**
* Avoid behaviour with ACS IR Sensors.
*/
void behaviour_avoid(void)
{
}



void acsStateChanged(void)
{




}

/************************************************** ***************************/
// Behaviour waitForStart:

#define PREPARE 1
#define WAIT 2
behaviour_command_t waitForStart = {0, 0, FWD,
false, false, 0, PREPARE};

/**
* Wait for start Behaviour.
* You need to clap your hands (or make other noise) three times in order
* to start the Robot!
*/
void behaviour_waitForStart(void)
{
static uint8_t peak_count = 3;
if(waitForStart.state == PREPARE)
{
if(getStopwatch2() > 250)
{
setCursorPosLCD(1, 6);
writeIntegerLengthLCD( peak_count, DEC, 1);
dischargePeakDetector();
waitForStart.state = WAIT;
setStopwatch2(0);
}
}
else if(waitForStart.state == WAIT)
{
uint8_t key = checkReleasedKeyEvent();
if(key)
waitForStart.state = IDLE;
if(getStopwatch2() > 50)
{
uint16_t tmp = getMicrophonePeak();
if(tmp > 4)
{
externalPort.LEDS = 0;
uint16_t i;
uint8_t j;
for(i = 0, j = 2; i < tmp; i+= 40)
{
if(i < 40)
{
externalPort.LEDS++;
}
else
{
externalPort.LEDS <<=1;
externalPort.LEDS++;
}
}
outputExt();
if(tmp > 120)
{
waitForStart.state = PREPARE;
peak_count--;
}
if(peak_count == 0)
waitForStart.state = IDLE;
}
else
setLEDs(0b0000);
setStopwatch2(0);
}
}
}



/**
* This function processes the movement commands that the behaviours generate.
* Depending on the values in the behaviour_command_t struct, it sets motor
* speed, moves a given distance or rotates.
*/
void moveCommand(behaviour_command_t * cmd)
{
if(cmd->move_value > 0) // move or rotate?
{
if(cmd->rotate)
rotate(cmd->speed_left, cmd->dir, cmd->move_value, false);
else if(cmd->move)
move(cmd->speed_left, cmd->dir, DIST_MM(cmd->move_value), false);
cmd->move_value = 0; // clear move value - the move commands are only
// given once and then runs in background.
}
else if(!(cmd->move || cmd->rotate)) // just move at speed?
{
changeDirection(cmd->dir);
moveAtSpeed(cmd->speed_left,cmd->speed_right);
}
else if(isMovementComplete()) // movement complete? --> clear flags!
{
cmd->rotate = false;
cmd->move = false;
}
}


void displayBehaviour(uint8_t behave)
{
static uint8_t compare = 0;
if(compare != behave)
{
compare = behave;
clearPosLCD(1, 0, 13);
setCursorPosLCD(1, 0);
switch(behave)
{
case 6: writeStringLCD_P("LOW BATTERY!"); setLEDs(0b0000); break;
case 5: writeStringLCD_P("WAIT"); setLEDs(0b0000); break;
case 4: writeStringLCD_P("ESCAPE"); setLEDs(0b0110); break;
case 3: writeStringLCD_P("AVOID"); setLEDs(0b1001); break;
case 2: writeStringLCD_P("CRUISE"); setLEDs(0b0000); break;
case 1: writeStringLCD_P("STOP"); setLEDs(0b0000); break;
}
}
if(behave == 2) // If Cruise behaviour is active, show a running light...
{
static uint8_t runLEDs = 1;
static uint8_t dir = 0;
if(getStopwatch2() > 100)
{
setLEDs(runLEDs);
if(dir == 0)
runLEDs <<= 1;
else
runLEDs >>= 1;
if(runLEDs > 7 )
dir = 1;
else if (runLEDs < 2 )
dir = 0;
setStopwatch2(0);
}
}
}



/**
* The behaviourController task controls the subsumption architechture.
* It implements the priority levels of the different behaviours.
*
* Here we also show which behaviour is active on the LC-Display!
*
*/
void behaviourController(void)
{
// Call all the behaviour tasks:


behaviour_cruise();
behaviour_avoid();
behaviour_escape();




if(escape.state != IDLE) // Priority - 4
{
displayBehaviour(4);

}
else if(avoid.state != IDLE) // Priority - 3
{
displayBehaviour(3);
moveCommand(&avoid);
}
else if(cruise.state != IDLE) // Priority - 1
{
displayBehaviour(2);
moveCommand(&cruise);
}
else // Lowest priority - 0
{
displayBehaviour(1);
moveCommand(&STOP); // Default command - do nothing!
// In the current implementation this never
// happens.
}
}


/************************************************** ***************************/

/**
* 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');
}


/**
* Heartbeat function
*/
void task_LCDHeartbeat(void)
{
if(getStopwatch1() > 500)
{
static uint8_t heartbeat = false;
if(heartbeat)
{
clearPosLCD(1, 15, 1);
heartbeat = false;
}
else
{
setCursorPosLCD(1, 15);
writeStringLCD_P("*");
heartbeat = true;
printAllSensorValues();
}
setStopwatch1(0);
}
}


/**
* Timed Watchdog display - the other heartbeat function
* does not work in this example as we use blocked moving functions here.
*/
void watchDogRequest(void)
{
static uint8_t heartbeat2 = false;
if(heartbeat2)
{
clearPosLCD(1, 14, 1);
heartbeat2 = false;
}
else
{
setCursorPosLCD(1, 14);
writeStringLCD_P("#");
heartbeat2 = true;
}
}

/************************************************** ***************************/
// I2C Requests:

/**
* The I2C_requestedDataReady Event Handler
*/
void I2C_requestedDataReady(uint8_t dataRequestID)
{
checkRP6Status(dataRequestID);
}

/************************************************** ***************************/
// 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();

writeString_P("\n\nRP6 CONTROL M32 I2C Master Example Program!\n");
writeString_P("\nMoving...\n");

// ---------------------------------------
WDT_setRequestHandler(watchDogRequest);
BUMPERS_setStateChangedHandler(bumpersStateChanged );
ACS_setStateChangedHandler(acsStateChanged);

// ---------------------------------------
I2CTWI_initMaster(100);
I2CTWI_setRequestedDataReadyHandler(I2C_requestedD ataReady);
I2CTWI_setTransmissionErrorHandler(I2C_transmissio nError);

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

setLEDs(0b1111);

showScreenLCD("################", "################");
mSleep(500);
showScreenLCD("I2C-Master", "Behaviours");
mSleep(1000);
setLEDs(0b0000);

// ---------------------------------------

startStopwatch1();
startStopwatch2();

showScreenLCD("Active Behaviour", "");

while(true)
{

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