Bumper über M32

[roy-sam]

Hallo,
ich habe meinen RP6 gerade mit ein paar Wärmesensoren ausgestattet.
Diese werden über die M32 ausgewertet.
Soweit läuft alles, aber jetzt wollte ich die Bumper mit ins Programm einfügen.
Leider reagieren die nicht!
Was mache ich falsch.](*,) ](*,) ](*,)
Hier der Quellcode.

// Includes:

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

/************************************************** ***************************/
#define I2C_RP6_BASE_ADR 10

// I2C Requests:

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

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

void bumpersStateChanged(void)
{
// Bumper status changed, output current state and play sounds:
writeString_P("Bumpers changed: ");
if(bumper_right && bumper_left)
{
writeString_P("MIDDLE!");
sound(200,100,0);
}
else
{
if(bumper_left)
{
writeString_P("LEFT!");
sound(200,50,10);
sound(150,20,0);
}
else if(bumper_right)
{
writeString_P("RIGHT!");
sound(200,50,10);
sound(150,20,0);
}
else
{
writeString_P("FREE!");
}
}
writeChar('\n');
}


int z; //Globale Variable

void scan (int ds)
{
writeString_P("\nDS\n");
writeInteger(ds, DEC );
int max;
do
{
sound(100,10,20);
sound(245,10,30);
sound(215,60,10);

int16_t adc2r = readADC(ADC_2);
int16_t adc3r = readADC(ADC_3);

max=adc2r-(adc3r+5); //Wert der Umgebung wird genommen

writeString_P("\nrechts SCAN\n");
writeInteger(adc2r, DEC );
writeString_P("\nlinks SCAN\n");
writeInteger(adc3r, DEC );
moveAtSpeed(50,0); //Drehe
setLEDs(0b1111);
}
while(!(max<(ds-15))); //Bis neu gemessener Wert stark von dem Übergebenen Anfangswert abweicht
}

void Snake (int dif)
{
sound(225,40,0);
sound(215,60,0);
sound(205,80,0);
sound(215,60,0);

int16_t adc2 = readADC(ADC_2);
int16_t sSnake = adc2;
int16_t adc3 = readADC(ADC_3);
int16_t sSnake2 = adc3;


int a,b;

a=(sSnake-sSnake2); //Neuer Wert wird genommen
b=0;

if (a>dif+10)
{
writeString_P("\na ist grosser dif\n");
writeInteger(a, DEC );
setLEDs(0b1100);

moveAtSpeed(50,0); //rechts drehen
b=1;
}

if(a<dif-5)
{
writeString_P("\na ist kleiner dif\n");
writeInteger(a, DEC );
setLEDs(0b0011);
moveAtSpeed(0,50); //links drehen
b=1;
}

if (b==0)
{
writeString_P("\na ist gleich dif\n");
writeInteger(a, DEC );
setLEDs(0b0110);
moveAtSpeed(40,40); //gerade fahren Richtung Feuer
z=a;
}

if((sSnake>420||sSnake2>420))
{
writeString_P("\nFeuer erreicht Löschen\n");
writeString_P("\nFeuer erreicht links\n");
writeInteger(sSnake, DEC );
writeString_P("\nFeuer erreicht rechts\n");
writeInteger(sSnake2, DEC );
moveAtSpeed(0,0);

DDRC |= IO_PC7; //PC7 als Ausgang Lüfter 5-Volt
PORTC |= IO_PC7; // Lüfter an
mSleep(500); // 0,5s
PORTC &= ~IO_PC7; // Lüfter aus

move(60,BWD, DIST_MM(200), BLOCKING);
rotate(40,RIGHT,65,BLOCKING);

mSleep(4000);

changeDirection(FWD);

adc2 = readADC(ADC_2);
sSnake = adc2;
adc3 = readADC(ADC_3);
sSnake2 = adc3;

a=(sSnake-sSnake2);
z=a;
scan(z);
}
}

// 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");

DDRA &= ~ADC3;
DDRA &= ~ADC2;
// ---------------------------------------
WDT_setRequestHandler(watchDogRequest);
BUMPERS_setStateChangedHandler(bumpersStateChanged );
// ---------------------------------------
// Init TWI Interface:
I2CTWI_initMaster(100);
I2CTWI_setRequestedDataReadyHandler(I2C_requestedD ataReady);
I2CTWI_setTransmissionErrorHandler(I2C_transmissio nError);


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

setLEDs(0b1001);

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

I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_ACS_POWER, ACS_PWR_MED);
I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT, true);
I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT_RQ, true);

int16_t adc2 = readADC(ADC_2);
int16_t sSnake = adc2;
writeString_P("\nadc2 MAIN\n");
writeInteger(sSnake, DEC );

int16_t adc3 = readADC(ADC_3);
int16_t sSnake2 = adc3;
writeString_P("\nadc3 MAIN\n");
writeInteger(sSnake2, DEC );
int dd;

dd=(sSnake-sSnake2); //Startwert
scan(dd);
sound(245,10,0);//Abeichung zum Startwert erkannt
sound(235,20,0);
sound(225,40,0);
sound(215,60,0);
sound(205,80,0);
sound(215,60,0);
sound(225,40,0);
sound(235,20,0);
sound(245,10,0);
setLEDs(0b0001);

sSnake=readADC(ADC_2);
writeString_P("\nneu rechts ADC2\n");
writeInteger(sSnake, DEC );
sSnake2=readADC(ADC_3);
writeString_P("\nneu links ADC3\n");
writeInteger(sSnake2, DEC );


dd=(sSnake-sSnake2); //Feuer gefunden Neuer Wert
writeString_P("\ndd zu dif\n");
writeInteger(dd, DEC );
z=dd;


while (true)
{
Snake(dd);
task_checkINT0();
task_I2CTWI();
dd=z;
}
return 0;
}


Danke für eure Hilfe
roy-sam

[roy-sam]

Hat keiner eine Idee???

[fabqu]

Hi Roy-Sam,
sorry, dass ich dir jetzt gar nicht weiterhelfen kann, ich hab auch nur eine Frage: Welche Wärmesensoren benutzt du? Die Snakevision vom Azuro?
Dein Programm gefällt mir nämlich :D Klappt das ansonsten?
Gruß,
Fabian

[roy-sam]

Hallo Fabian,
schade warte schon seit gestern auf eine Antwort, komme einfach nicht weiter.
Ja es ist das Snakevision vom Azuro.
Es funktioniert soweit, außer halt die Bumper!

Also hat jemand eine Idee, was ich mit den Bumpern falsch mache?

Gruß
roy-sam

[Xenergy]

Hi,
was klappt den bei dir genau nicht?
Laut deinem Code soll es piepsen und auf die Schnittstelle die Daten übergeben.
Soll der RP6 auch dabei fahren?

Übrigens, auf dem Mainboard muss Slaveprogramm geladen sein.

[roy-sam]

Hi Xenergy,

also in der Base ist das Slaveprog. geladen.

Außerhalb der Bumperabfrage habe ich ja ein Programm laufen das Ständig nach Wärmequellen sucht (void scan...), wenn es eine gefunden hat soll der RP6 zur Wärmequelle fahren (void snake...).
Danach wieder mit scannen beginnen usw. usw.

Das funktioniert soweit, jetzt wollte ich halt die Bumperabfrage mit einbinden, um zu testen ob es geht piepst er halt nur und gibt eine Meldung über die Schnittstelle aus.

Später soll er, wenn ein Bumper betätigt wird z.B.: rückwärts fahren und sich ein Stück drehen, danach soll der Normale Scanvorgang weitergehen.

Danke
roy-sam

[SlyD]

Funktioniert es denn mit den Beispielprogrammen und funktionieren die Bumper beim Selftest?

[roy-sam]

Funktioniert es denn mit den Beispielprogrammen und funktionieren die Bumper beim Selftest?

Ja die Bumper funktionieren. in den Beispielprogrammen und beim Selftest.

[SlyD]

Was wird denn im Terminal ausgegeben wenn das Programm läuft und Du die Bumper drückst?
Mach evtl. noch mehr Debug ausgaben rein - auch in die Master Lib aus den Beispielprogrammen (die übrigens erst im letzen Beispiel komplett ist also diese verwenden! )

Auf den ersten (1 Minute) Blick seh ich da auch keinen Fehler habe keine Zeit das genauer durchzugehen da musst Du wohl oder übel etwas "Debuggen" (also der normalzustand bei Softwareentwicklung :) ).

MfG,
SlyD

[roy-sam]

Hi,
also die Bumper funktionieren doch, nur leider sind sie nicht "übergeordnet" d.h. wenn ich die Bumper drücke und der RP6 gerade im "Void scan" Unterprogramm ist, reagiert er nicht auf die Bumper.
Wenn er im "void Snake" UP ist dann reagiert er ab und zu mal.
An der stelle wo ich ein Stück rückwärts fahre und mich drehe, reagiert er auf alle Bumperänderungen.
Kann ich irgendwie die Bumperabfrage als oberste Priorität festlegen, so das er immer auf die Bumper reagiert?

Danke für eure Hilfe.
roy-sam

[SlyD]

Das liegt daran das Du die
task_checkINT0();
task_I2CTWI();

dort nicht regelmäßig aufrufst. Dort wird geprüft ob sich was verändert hat.

Ist genauso wie auf der RP6Base - da muss nur stattdessen die task_RP6System(); ständig aufgerufen werden.

Das muss in jede für längere Zeit blockierende while Schleife o.ä. rein.

MfG,
SlyD

[roy-sam]

Hi SlyD,
danke für die schnelle Antwort.
Im "void snake" funktioniert es jetzt, aber im "void scan" noch nicht so richtig.
Ich habe die Abfrage direkt eine Zeile unter do{ in der do While schleife eingfügt, aber es wird nur so jeder 2te oder 3te Bumperdruck registriert, in der "void snake" werden alle erkannt?!?!?!?
Danke

[SlyD]

Das könnte an den ganzen Verzögerungen liegen die da drin sind.
Wenn Du den Bumper LANGE drückst wird es immer klappen denke ich.
"sound" ist blockierend und fügt eine Pause ein du kannst stattdessen versuchen es mit beep (s. Kommentare in der Lib und Doku) und einer Stopwatch zu lösen wenn da unbedingt die drei Töne reinmüssen - aber das ist halt etwas aufwändiger.

Wenn viel "gleichzeitig" passieren soll darf man keine festen Pausen ins Programm einbauen (von ein paar wenigen Millisekunden mal abgesehen).

MfG,
SlyD

[roy-sam]

Hi,
so hab die sounds verkürzt, jetzt gehts besser, aber wenn ich einen Bumper drücke fährt er ein Stück zurück und dreht sich ein paar Grad.
So habe ich es programmiert, funktioniert also.
Aber er bleibt dann stehen und fährt mit dem Programm nicht fort.
Wie komme ich aus dem UP Bumper wieder raus?

[roy-sam]

Keiner einen Vorschlag? [-o< [-o< [-o<

Hier das UP-Bumper

void bumpersStateChanged(void)
{
// Bumper status changed
writeString_P("Bumper betätigt: ");
if(bumper_right && bumper_left)
{
writeString_P("Beide!");
move(50,BWD, DIST_MM(100), BLOCKING);
rotate(35,RIGHT,30,BLOCKING);
changeDirection(FWD);
}

else if(bumper_left)
{
writeString_P("Links!");
move(50,BWD, DIST_MM(100), BLOCKING);

rotate(35,RIGHT,30,BLOCKING);
changeDirection(FWD);
}

else if(bumper_right)
{
writeString_P("Rechts!");
move(50,BWD, DIST_MM(100), BLOCKING);
rotate(35,RIGHT,30,BLOCKING);
changeDirection(FWD);
}

else
{
writeString_P("Frei!");
}

writeChar('\n');

}

Mir ist gerade aufgefallen das er nur zurück fährt, die Drehung macht er schon garnicht mehr. :-(
Wenn ich das Rückwärtsfahren und die Drehung weg lösche, geht er im Programm weiter?!?!?!?!?!?

[SlyD]

> BLOCKING

Ist in Event Handlern nicht sinnvoll nutzbar genauso wie jegliche längere Pausen.
Das musst Du anders lösen.

(am einfachsten wird es für Dich sein einfach die Bumper selbst in der Hauptschleife abzufragen - sauber wird es wie in den Subsumptions-Beispielprogrammen mit Zustandsautomaten)

MfG,
SlyD

[roy-sam]

Puh,
das klingt aber gerade kompliziert.
Kannst du mir vielleicht ein bissle genauer sagen was du meinst :-)
Oder eine kleine Befehlszeile als Beispiel posten.
Damit ich weis nach was ich suchen muss.

Hier mal mein kompletter Code:

// 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"

/************************************************** ***************************/
#define I2C_RP6_BASE_ADR 10
//#define adc2 ;

/*
* Timed Watchdog display only - 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(0, 14, 1);
heartbeat2 = false;
}
else
{
setCursorPosLCD(0, 14);
writeStringLCD_P("#");
heartbeat2 = true;
}
}

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

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

void bumpersStateChanged(void)
{
// Bumper status changed
writeString_P("Bumper betätigt: ");
if(bumper_right && bumper_left)
{
writeString_P("Beide!");
move(50,BWD, DIST_MM(100), BLOCKING);
rotate(35,RIGHT,30,BLOCKING);
changeDirection(FWD);
}

else if(bumper_left)
{
writeString_P("Links!");
move(50,BWD, DIST_MM(100), BLOCKING);
rotate(35,RIGHT,30,BLOCKING);
changeDirection(FWD);
}

else if(bumper_right)
{
writeString_P("Rechts!");
move(50,BWD, DIST_MM(100), BLOCKING);
rotate(35,RIGHT,30,BLOCKING);
changeDirection(FWD);
}

else
{
writeString_P("Frei!");
}

writeChar('\n');

}


int z; //Globale Variable

void scan (int ds)
{
writeString_P("\nDS\n");
writeInteger(ds, DEC );
int max;
do
{
task_checkINT0();
task_I2CTWI();

sound(20,50,1);//Tonhöhe,Zeit,Pause

int16_t adc2r = readADC(ADC_2);
int16_t adc3r = readADC(ADC_3);

max=adc2r-adc3r;

writeString_P("\nrechts SCAN\n");
writeInteger(adc2r, DEC );
writeString_P("\nlinks SCAN\n");
writeInteger(adc3r, DEC );

moveAtSpeed(50,0); //Drehe
setLEDs(0b1111);

}
while(!(max<(ds-15))); //Bis neu gemessener Wert stark von dem Übergebenen Anfangswert abweicht

moveAtSpeed(0,0);

uint8_t i;
uint8_t j;
for(i = 1; i < 4; i++) //Sirene--> Abeichung zum Startwert erkannt
{
for(j = 230; j > 46; j-=2)
{
setBeeperPitch(j);
mSleep(5);
}
for(; j < 230; j+=2)
{
setBeeperPitch(j);
mSleep(5);
}
// Beeper aus:
setBeeperPitch(0);
}

rotate(40,LEFT,10,BLOCKING); //Ein kleines Stück zurückdrehen
mSleep(2000);
changeDirection(FWD);

}

void Snake (int dif)
{
task_checkINT0();
task_I2CTWI();


sound(20,40,0);
sound(215,60,0);

int16_t adc2 = readADC(ADC_2);
int16_t sSnake = adc2;
int16_t adc3 = readADC(ADC_3);
int16_t sSnake2 = adc3;

int a,b;

a=(sSnake-sSnake2); //Neuer Wert wird genommen

b=0;

if (a>dif+12)
{

writeString_P("\na ist grosser dif\n");
writeInteger(a, DEC );
setLEDs(0b1100);

moveAtSpeed(50,0); //rechts drehen
b=1;
}

else if(a<dif-12)
{

writeString_P("\na ist kleiner dif\n");
writeInteger(a, DEC );
setLEDs(0b0011);

moveAtSpeed(0,50); //links drehen
b=1;
}

else if (b==0)
{

writeString_P("\na ist gleich dif\n");
writeInteger(a, DEC );
setLEDs(0b0110);

moveAtSpeed(40,40); //gerade fahren Richtung Feuer
z=a;
}

else if((sSnake>420||sSnake2>420))
{

writeString_P("\nFeuer erreicht Löschen\n");
writeString_P("\nFeuer erreicht links\n");
writeInteger(sSnake, DEC );
writeString_P("\nFeuer erreicht rechts\n");
writeInteger(sSnake2, DEC );
moveAtSpeed(0,0);

DDRC |= IO_PC7; //PC7 als Ausgang Lüfter 5-Volt
PORTC |= IO_PC7; // Lüfter an
mSleep(500); // 0,5s
PORTC &= ~IO_PC7; // Lüfter aus

move(60,BWD, DIST_MM(200), BLOCKING);
rotate(40,RIGHT,65,BLOCKING);
mSleep(4000);
changeDirection(FWD);

adc2 = readADC(ADC_2);
sSnake = adc2;
adc3 = readADC(ADC_3);
sSnake2 = adc3;

a=(sSnake-sSnake2);
z=a;

scan(z);
}
}

// 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");

DDRA &= ~ADC3;
DDRA &= ~ADC2;
// ---------------------------------------
WDT_setRequestHandler(watchDogRequest);
BUMPERS_setStateChangedHandler(bumpersStateChanged );
// ---------------------------------------
// Init TWI Interface:
I2CTWI_initMaster(100);
I2CTWI_setRequestedDataReadyHandler(I2C_requestedD ataReady);
I2CTWI_setTransmissionErrorHandler(I2C_transmissio nError);


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

setLEDs(0b1001);

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

I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_ACS_POWER, ACS_PWR_MED);
I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT, true);
I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT_RQ, true);


int16_t adc2 = readADC(ADC_2);
int16_t sSnake = adc2;
writeString_P("\nadc2 MAIN\n");
writeInteger(sSnake, DEC );

int16_t adc3 = readADC(ADC_3);
int16_t sSnake2 = adc3;
writeString_P("\nadc3 MAIN\n");
writeInteger(sSnake2, DEC );

int dd;

dd=(sSnake-sSnake2); //Startwert

scan(dd);

//Nach dem ersten Scannen geht es hier weiter
setLEDs(0b0001);

sSnake=readADC(ADC_2);
writeString_P("\nneu rechts ADC2\n");
writeInteger(sSnake, DEC );
sSnake2=readADC(ADC_3);
writeString_P("\nneu links ADC3\n");
writeInteger(sSnake2, DEC );


dd=(sSnake-sSnake2); //Feuer gefunden Neuer Wert
writeString_P("\ndd zu dif\n");
writeInteger(dd, DEC );

z=dd;

while (true)
{
task_checkINT0();
task_I2CTWI();
Snake(dd);
dd=z;
}
return 0;
}




Danke

[SlyD]

Na alles damit: move(50,BWD, DIST_MM(100), --->>> BLOCKING <<<------ )
Darfst Du im Event Handler NICHT verwenden.

Wie Du es richtig machst steht in der RP6 Anleitung bei Zustandsautomaten / State Machines und in den zugehörigen Beispielen.
(ich hab keine Zeit das hier genauer zu erklären sorry)

MfG,
SlyD