State-Maschine auf Slave funktioniert nicht (GELÖST)

[Adleo]

Hallo,
ich habe zu Weihnachten einen RP6 bekommen und bis jetzt bin ich dank des Forums und der tollen Bedienungsanleitung immer weitergekommen (alle vor mir hatten die Probleme auch schon). Nun muss ich selbst eine Frage stellen:

Ich will meine M32 als Master und die Base als Slave verwenden, allerdings soll der Master nur ab und zu eingreifen, sonst soll die Base eine State Machine ausführen! Ich habe es probiert, aber es hat nicht geklappt.

Mein Master-Code:

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

#define status_start 0
#define status_fahren 1 // Vorwärtsfahrt mit Überprüfung...
#define status_BWD 2
#define status_bemerkt_links 3
#define status_bemerkt_rechts 4
#define status_bemerkt_beide 5
#define status_KurveR 6
#define status_KurveL 7
#define status_STOP 8

#define state_master 2

void BaseMotorStart(void)
{

if (!I2CTWI_isBusy())
{
I2CTWI_transmit2Bytes(10, 1, 0); //status_start in 1 an 10
I2CTWI_transmit2Bytes(10, 0, 2); //state_master in 0 an 10
}

}

void BaseMotorStop(void)
{
if (!I2CTWI_isBusy())
{
I2CTWI_transmit2Bytes(10, 1, status_STOP);
I2CTWI_transmit2Bytes(10, 0, 2);
}
}

int main(void)
{
initRP6Control();
initLCD();
I2CTWI_initMaster(100); // TWI initialisieren und Frequenz 100 einstellen

uint8_t anzeige=1;
uint8_t Batterie;
uint8_t key_abfrage=0;
uint8_t movestate;
uint8_t AusA;

while(true){
if(anzeige){
clearLCD();
showScreenLCD("Drucke einen","Taster!!!!!!!!!!");
anzeige=0;
}
key_abfrage=0;
key_abfrage=getPressedKeyNumber();

if (key_abfrage != 0){

anzeige=1;

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

switch(key_abfrage){

case 1:
if(!I2CTWI_isBusy()){
I2CTWI_transmitByte(10,2);
movestate=I2CTWI_readByte(10);
}

clearLCD;
showScreenLCD("move_state:","");
setCursorPosLCD(1,1);
writeIntegerLengthLCD(movestate, DEC, 1);
mSleep(2000);
break;

case 2:
if(!I2CTWI_isBusy()){
I2CTWI_transmitByte(10,3);
AusA=I2CTWI_readByte(10);
}

clearLCD;
showScreenLCD("Aus:","");
setCursorPosLCD(1,1);
writeIntegerLengthLCD(AusA, DEC, 1);
mSleep(2000);
break;

case 3:
clearLCD();
showScreenLCD("Batterie: ","ist das OK?");

setCursorPosLCD(0, 10);

if (!I2CTWI_isBusy())
{
I2CTWI_transmitByte(10, 1);
Batterie = I2CTWI_readByte(10); //Batterie auslesen
}

writeIntegerLengthLCD(Batterie, DEC, 4);
mSleep(2000);
break;

case 4:
clearLCD();
showScreenLCD("Warnung! Der","Motor ist an!");

BaseMotorStart(); //Motor starten

clearLCD();
showScreenLCD("Motor lauft!","!!!!!!!!!!!!!!!!");
mSleep(2000);
break;

case 5:
BaseMotorStop();

clearLCD();
showScreenLCD("Motor wird","angehalten!!!!!!");
mSleep(2000);
break;
}

} }
return 0;
}

Mein Slave:


#include "RP6RobotBaseLib.h"
#include "RP6I2CslaveTWI.h"

#define status_start 0
#define status_fahren 1 // Vorwärtsfahrt mit Überprüfung...
#define status_BWD 2
#define status_bemerkt_links 3
#define status_bemerkt_rechts 4
#define status_bemerkt_beide 5
#define status_KurveR 6
#define status_KurveL 7
#define status_STOP 8
#define status_master 9


#define RC5_links 21 //Fernbeninungssteuerung...
#define RC5_rechts 22
#define RC5_FWD 32
#define RC5_BWD 33
#define RC5_STOP 13
#define RC5_speed_plus 14
#define RC5_speed_minus 35
#define RC5_winkel_plus 16
#define RC5_winkel_minus 17
#define RC5_90grad 55
#define RC5_180grad 54
#define RC5_150speed 50
#define RC5_80speed 52
#define RC5_Aus 12
#define RC5_ACS_Aus 63
#define RC5_ACS_An 56
#define RC5_korrigieren_R 18
#define RC5_korrigieren_L 48
#define RC5_drehenR 34
#define RC5_drehenL 0

#define master_status 2

uint8_t move_state; // Speicher für die aktuelle Fahrsituation
uint8_t RC5_state; //Speicher für RC5Kommandos
uint16_t Spann;
uint8_t speed=150;
uint8_t winkel=90;
uint8_t Aus=1;
uint8_t AC_System=0;

uint8_t param1;

void masterabfrage(void)
{
task_RP6System();

if(I2CTWI_writeRegisters[0] && !I2CTWI_writeBusy){
uint8_t cmd = I2CTWI_writeRegisters[0];
I2CTWI_writeRegisters[0] = 0;

switch(cmd){

case master_status:
param1=I2CTWI_writeRegisters[1];
move_state=param1;
break;

}
}

I2CTWI_readRegisters[1]=readADC(ADC_BAT);
I2CTWI_readRegisters[2]=move_state;
I2CTWI_readRegisters[3]=Aus;
}

void receiveRC5Data(RC5data_t rc5data)
{
switch (rc5data.key_code)
{
case RC5_rechts:
setLEDs(0b000111);
move_state=status_bemerkt_rechts;
break;

case RC5_links:
setLEDs(0b111000);
move_state=status_bemerkt_links;
break;

case RC5_BWD:
setLEDs(0b100001);
move_state=status_BWD;
break;

case RC5_FWD:
setMotorDir(FWD, FWD);
moveAtSpeed(speed,speed);
move_state=status_fahren;
break;

case RC5_STOP:
setLEDs(0b111111);
moveAtSpeed(0,0);
break;

case RC5_speed_plus:
setLEDs(0b100000);
speed=speed+10;
break;

case RC5_speed_minus:
setLEDs(0b000001);
speed=speed-10;
break;

case RC5_winkel_plus:
setLEDs(0b010000);
winkel=winkel+10;
break;

case RC5_winkel_minus:
setLEDs(0b000010);
winkel=winkel-10;
break;

case RC5_90grad:
winkel=90;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_180grad:
winkel=180;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_150speed:
speed=150;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_80speed:
speed=80;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_Aus: //AN/AUS gedrück!
if (Aus) {moveAtSpeed(0,0);Aus=0;} else {Aus=1;}
break;

case RC5_ACS_Aus:
AC_System=0;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_ACS_An:
AC_System=1;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_korrigieren_R:

move_state=status_KurveL;
break;

case RC5_korrigieren_L:
move_state=status_KurveR;
break;

case RC5_drehenR:
setMotorDir(FWD,BWD);
move_state=status_fahren;
break;

case RC5_drehenL:
setMotorDir(BWD,FWD);
move_state=status_fahren;
break;
}
}


void acsStateChanged(void)
{
if (AC_System) {
if(obstacle_left)
move_state=status_bemerkt_rechts;

if(obstacle_right)
move_state=status_bemerkt_links;

if(obstacle_right && obstacle_left)
move_state=status_bemerkt_beide;


// -----------------------------------------------------------
// Set LEDs

if(obstacle_left && obstacle_right) // Obstacle in the middle?
statusLEDs.byte = 0b100100;
else
statusLEDs.byte = 0b000000;
statusLEDs.LED5 = obstacle_left;
statusLEDs.LED4 = (!obstacle_left); // Inverted LED5!
statusLEDs.LED2 = obstacle_right;
statusLEDs.LED1 = (!obstacle_right); // Inverted LED2!

// Update the LED values:
updateStatusLEDs(); // The LEDs are turned on/off here! __Not__ in the lines above!
}
}

void bumpersStateChanged(void)
{
if(bumper_left)
move_state=status_bemerkt_rechts;

if(bumper_right)
move_state=status_bemerkt_links;

if(bumper_left && bumper_right)
move_state=status_bemerkt_beide;
}

/*void blink(void)
{
while(true) {
setLEDs(0b111111);
mSleep(500);
setLEDs(0);
mSleep(500);
}
}*/

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

int main(void)
{
initRobotBase(); //roboter einbinden
I2CTWI_initSlave(10); //als Slave10 registrieren!

setLEDs(0b111111);
mSleep(1000);
setLEDs(0b001001);

move_state=status_master;

powerON(); //Verletzungsgefahr!

speed=150;

// Register Event Handlers:
ACS_setStateChangedHandler(acsStateChanged); //ACS HAndler
BUMPERS_setStateChangedHandler(bumpersStateChanged ); //Bumper HAndler
IRCOMM_setRC5DataReadyHandler(receiveRC5Data); //RC5 Handler



setACSPwrMed(); // Aktivieren des ACS in Medium

Spann=readADC(ADC_BAT);



while(true) //Wiederholen bis Akku leer
{
task_motionControl();

switch (move_state) // ist move_state...
{
case status_start:
setLEDs(0b011011);
if(Aus){setMotorDir(FWD, FWD); moveAtSpeed(speed,speed);}
move_state=status_fahren;
break;

case status_fahren:
task_motionControl();
setLEDs(0b001001);
Spann=readADC(ADC_BAT);
if (Spann<6.5) {
moveAtSpeed(0,0);
//blink();
}
do
masterabfrage();


//Nach ACS und Bumpern und RC5 suchen... (in Master-Abfrage)

while (move_state==status_fahren);// ...bis sich etwas geändert hat! (das gleiche wie: solange status_fahren)
break;

case status_bemerkt_links:

mSleep(500);
setLEDs(0b100000);
if(Aus){rotate(speed,LEFT,winkel,1);} // ...nicht wenn AN/AUS gedrückt
move_state=status_start;
break;

case status_bemerkt_rechts:
setLEDs(0b000010);
if(Aus){rotate(speed,RIGHT,winkel,1);} // ...nicht wenn AN/AUS gedrückt
move_state=status_start;
break;

case status_bemerkt_beide:
setLEDs(0b010010);
if(Aus){rotate(speed,RIGHT,winkel,1);} // ...nicht wenn AN/AUS gedrückt
move_state=status_start;
break;

case status_BWD:
setLEDs(0b011011);
if(Aus){setMotorDir(BWD, BWD); moveAtSpeed(speed,speed);}
move_state=status_fahren;
break;

case status_KurveR:
setLEDs(0b000111);
moveAtSpeed(speed-50, speed);
do

//Nach ACS und Bumpern und RC5 suchen...
masterabfrage();
while (move_state==status_KurveR);
break;

case status_KurveL:
setLEDs(0b000111);
moveAtSpeed(speed, speed-50);
do

//Nach ACS und Bumpern und RC5 suchen...
masterabfrage();
while (move_state==status_KurveL);
break;

case status_STOP:
moveAtSpeed(0,0);
move_state=status_fahren;
break;

case status_master:
setLEDs(0b111000);
do
masterabfrage();
while(move_state==status_master);
break;
}
}
return 0;
}

Ich verzweifle! Wieso funktioniert das nicht! ](*,)

Das Auslesen der Register funktioniert prima. Aber wenn ich der State-Maschine auf dem Slave durch den Master den Befehl gebe ihren Status zu ändern, reagiert sie nicht.
Meine RC5-Fernbedienung funktioniert leider auch nicht mehr, sobald die Base als Slave initiiert wurde...

Ich hoffe mir kann jemand helfen.

Viele Grüsse an Alle

Adleo

[SlyD]

Hallo,

Du hast da an sehr vielen Stellen Endlosschleifen drin, z.B.:
while (move_state==status_fahren);

hier blockiert das Programm.
Entweder musst Du die State Machine etwas anders aufbauen
oder in die Endlosschleifen alle Abfragen und Aktualisierungen reinmachen.
Alle "task_" Funktionen müssen immer periodisch (mal hier und da 10 - 50ms Pause sind egal) aufgerufen werden (auch Deine masterabfrage() Funktion).


MfG,
SlyD

PS:
Übrigens besser nicht nur task_motionControl() verwenden sondern direkt task_RP6System()

[Adleo]

Hi,
ich habe jetzt den Code:



#include "RP6RobotBaseLib.h"
#include "RP6I2CslaveTWI.h"

#define status_start 0
#define status_fahren 1 // Vorwärtsfahrt mit Überprüfung...
#define status_BWD 2
#define status_bemerkt_links 3
#define status_bemerkt_rechts 4
#define status_bemerkt_beide 5
#define status_KurveR 6
#define status_KurveL 7
#define status_STOP 8
#define status_master 9


#define RC5_links 21 //Fernbeninungssteuerung...
#define RC5_rechts 22
#define RC5_FWD 32
#define RC5_BWD 33
#define RC5_STOP 13
#define RC5_speed_plus 14
#define RC5_speed_minus 35
#define RC5_winkel_plus 16
#define RC5_winkel_minus 17
#define RC5_90grad 55
#define RC5_180grad 54
#define RC5_150speed 50
#define RC5_80speed 52
#define RC5_Aus 12
#define RC5_ACS_Aus 63
#define RC5_ACS_An 56
#define RC5_korrigieren_R 18
#define RC5_korrigieren_L 48
#define RC5_drehenR 34
#define RC5_drehenL 0

#define master_status 2

uint8_t move_state; // Speicher für die aktuelle Fahrsituation
uint8_t RC5_state; //Speicher für RC5Kommandos
uint16_t Spann;
uint8_t speed=150;
uint8_t winkel=90;
uint8_t Aus=1;
uint8_t AC_System=0;

uint8_t param1;

void masterabfrage(void)
{
task_RP6System();

if(I2CTWI_writeRegisters[0] && !I2CTWI_writeBusy){
uint8_t cmd = I2CTWI_writeRegisters[0];
I2CTWI_writeRegisters[0] = 0;

switch(cmd){

case master_status:
param1=I2CTWI_writeRegisters[1];
move_state=param1;
break;

}
}

I2CTWI_readRegisters[1]=readADC(ADC_BAT);
I2CTWI_readRegisters[2]=move_state;
I2CTWI_readRegisters[3]=Aus;
}

void receiveRC5Data(RC5data_t rc5data)
{
switch (rc5data.key_code)
{
case RC5_rechts:
setLEDs(0b000111);
move_state=status_bemerkt_rechts;
break;

case RC5_links:
setLEDs(0b111000);
move_state=status_bemerkt_links;
break;

case RC5_BWD:
setLEDs(0b100001);
move_state=status_BWD;
break;

case RC5_FWD:
setMotorDir(FWD, FWD);
moveAtSpeed(speed,speed);
move_state=status_fahren;
break;

case RC5_STOP:
setLEDs(0b111111);
moveAtSpeed(0,0);
break;

case RC5_speed_plus:
setLEDs(0b100000);
speed=speed+10;
break;

case RC5_speed_minus:
setLEDs(0b000001);
speed=speed-10;
break;

case RC5_winkel_plus:
setLEDs(0b010000);
winkel=winkel+10;
break;

case RC5_winkel_minus:
setLEDs(0b000010);
winkel=winkel-10;
break;

case RC5_90grad:
winkel=90;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_180grad:
winkel=180;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_150speed:
speed=150;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_80speed:
speed=80;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_Aus: //AN/AUS gedrück!
if (Aus) {moveAtSpeed(0,0);Aus=0;} else {Aus=1;}
break;

case RC5_ACS_Aus:
AC_System=0;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_ACS_An:
AC_System=1;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_korrigieren_R:

move_state=status_KurveL;
break;

case RC5_korrigieren_L:
move_state=status_KurveR;
break;

case RC5_drehenR:
setMotorDir(FWD,BWD);
move_state=status_fahren;
break;

case RC5_drehenL:
setMotorDir(BWD,FWD);
move_state=status_fahren;
break;
}
}


void acsStateChanged(void)
{
if (AC_System) {
if(obstacle_left)
move_state=status_bemerkt_rechts;

if(obstacle_right)
move_state=status_bemerkt_links;

if(obstacle_right && obstacle_left)
move_state=status_bemerkt_beide;


// -----------------------------------------------------------
// Set LEDs

if(obstacle_left && obstacle_right) // Obstacle in the middle?
statusLEDs.byte = 0b100100;
else
statusLEDs.byte = 0b000000;
statusLEDs.LED5 = obstacle_left;
statusLEDs.LED4 = (!obstacle_left); // Inverted LED5!
statusLEDs.LED2 = obstacle_right;
statusLEDs.LED1 = (!obstacle_right); // Inverted LED2!

// Update the LED values:
updateStatusLEDs(); // The LEDs are turned on/off here! __Not__ in the lines above!
}
}

void bumpersStateChanged(void)
{
if(bumper_left)
move_state=status_bemerkt_rechts;

if(bumper_right)
move_state=status_bemerkt_links;

if(bumper_left && bumper_right)
move_state=status_bemerkt_beide;
}

void task_abfrage(void)
{
task_RP6System();
masterabfrage();
}

/*void blink(void)
{
while(true) {
setLEDs(0b111111);
mSleep(500);
setLEDs(0);
mSleep(500);
}
}*/

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

int main(void)
{
initRobotBase(); //roboter einbinden
I2CTWI_initSlave(10); //als Slave10 registrieren!

setLEDs(0b111111);
mSleep(1000);
setLEDs(0b001001);

move_state=status_master;

powerON(); //Verletzungsgefahr!

speed=150;

// Register Event Handlers:
ACS_setStateChangedHandler(acsStateChanged); //ACS HAndler
BUMPERS_setStateChangedHandler(bumpersStateChanged ); //Bumper HAndler
IRCOMM_setRC5DataReadyHandler(receiveRC5Data); //RC5 Handler



setACSPwrMed(); // Aktivieren des ACS in Medium

Spann=readADC(ADC_BAT);



while(true) //Wiederholen bis Akku leer
{
task_abfrage();

switch (move_state) // ist move_state...
{
case status_start:
setLEDs(0b011011);
if(Aus){setMotorDir(FWD, FWD); moveAtSpeed(speed,speed);}
move_state=status_fahren;
break;

case status_fahren:
task_motionControl();
setLEDs(0b001001);
Spann=readADC(ADC_BAT);
if (Spann<6.5) {
moveAtSpeed(0,0);
//blink();
}
do
task_abfrage();
//Nach ACS und Bumpern und RC5 suchen... (in Master-Abfrage)

while (move_state==status_fahren);// ...bis sich etwas geändert hat! (das gleiche wie: solange status_fahren)
break;

case status_bemerkt_links:

mSleep(500);
setLEDs(0b100000);
if(Aus){rotate(speed,LEFT,winkel,1);} // ...nicht wenn AN/AUS gedrückt
move_state=status_start;
break;

case status_bemerkt_rechts:
setLEDs(0b000010);
if(Aus){rotate(speed,RIGHT,winkel,1);} // ...nicht wenn AN/AUS gedrückt
move_state=status_start;
break;

case status_bemerkt_beide:
setLEDs(0b010010);
if(Aus){rotate(speed,RIGHT,winkel,1);} // ...nicht wenn AN/AUS gedrückt
move_state=status_start;
break;

case status_BWD:
setLEDs(0b011011);
if(Aus){setMotorDir(BWD, BWD); moveAtSpeed(speed,speed);}
move_state=status_fahren;
break;

case status_KurveR:
setLEDs(0b000111);
moveAtSpeed(speed-50, speed);
do

//Nach ACS und Bumpern und RC5 suchen...
task_abfrage();
while (move_state==status_KurveR);
break;

case status_KurveL:
setLEDs(0b000111);
moveAtSpeed(speed, speed-50);
do

//Nach ACS und Bumpern und RC5 suchen...
task_abfrage();
while (move_state==status_KurveL);
break;

case status_STOP:
moveAtSpeed(0,0);
move_state=status_fahren;
break;

case status_master:
setLEDs(0b111000);
do
task_abfrage();
while(move_state==status_master);
break;
}
}
return 0;
}





Aber die Probleme sind immer noch da:
Der RP6 Fährt nicht.
Er wechselt nur die Zustände! (Das kann ich mit Taste 2 auslesen)

Zwar reagiert er auf meine Fernbedienung und ändert dementsprechend die Zustände, aber: der Roboter fährt nicht.

Bin das Programm nochmals durchgegangen. Ich finde einfach keinen Fehler! :cry:

Viele Grüsse an alle
Adleo

[suicide]

Hi, ich hab mal ein paar Dinge im Code angemerkt. Vielleicht bringt das ja was.


while(true) //Wiederholen bis Akku leer
{
taskabfrage();


Die zwei Aufrufe aus der Methode taskabfrage() kannst du auch direkt in die while Schleife packen.



case status_fahren:
task_motionControl();
setLEDs(0b001001);
Spann=readADC(ADC_BAT);
if (Spann<6.5) {
moveAtSpeed(0,0);
//blink();
}
do
task_abfrage();
//Nach ACS und Bumpern und RC5 suchen... (in Master-Abfrage)

while (move_state==status_fahren);// ...bis sich etwas geändert hat! (das gleiche wie: solange status_fahren)
break;



Gibt readADC(ADC_BAT) einen Wert > 6.5 aus?
Der task_motionControl() müsste von dem task_RP6System() aufgerufen werden, welchen du schon am Anfang der while Schleife aufrufst.
Die do-while Schleife kannst du entfernen da du task_abfrage() schon vor der switch-case Schleife aufrufst. Dadurch bleibst du auch nicht in der do-while Schleife gefangen und die Akkuspannung wird weiter überprüft. So lange der move_state == status_fahren bleibt, springt die switch-case Schleife auch immer wieder in den status_fahren Block, es gibt also keinen Grund, den Ablauf in diesem Block festzuhalten so lange der Status status_fahren bleibt.

Grüße,
Jan

[Adleo]

Hallo,

Juuuuchuuuu! Es funktioniert!!!!!!!! \:D/ \:D/ \:D/ \:D/

Danke SlyD und suicide! Ihr habt mir sehr geholfen. Es
lag an den überfüllten Schleifen, sodass der Microcontroller
zuviel Rechenzeit gebraucht hat.

Ich habe jetzt folgenden Code für den Slave:



#include "RP6RobotBaseLib.h"
#include "RP6I2CslaveTWI.h"

#define status_start 0
#define status_fahren 1 // Vorwärtsfahrt mit Überprüfung...
#define status_BWD 2
#define status_bemerkt_links 3
#define status_bemerkt_rechts 4
#define status_bemerkt_beide 5
#define status_KurveR 6
#define status_KurveL 7
#define status_STOP 8
#define status_master 9


#define RC5_links 21 //Fernbeninungssteuerung...
#define RC5_rechts 22
#define RC5_FWD 32
#define RC5_BWD 33
#define RC5_STOP 13
#define RC5_speed_plus 14
#define RC5_speed_minus 35
#define RC5_winkel_plus 16
#define RC5_winkel_minus 17
#define RC5_90grad 55
#define RC5_180grad 54
#define RC5_150speed 50
#define RC5_80speed 52
#define RC5_Aus 12
#define RC5_ACS_Aus 63
#define RC5_ACS_An 56
#define RC5_korrigieren_R 18
#define RC5_korrigieren_L 48
#define RC5_drehenR 34
#define RC5_drehenL 0

#define master_status 2

uint8_t move_state; // Speicher für die aktuelle Fahrsituation
uint8_t RC5_state; //Speicher für RC5Kommandos
uint16_t Spann;
uint8_t speed=150;
uint8_t winkel=90;
uint8_t Aus=1;
uint8_t AC_System=0;

uint8_t param1;

void masterabfrage(void)
{
//task_RP6System();

if(I2CTWI_writeRegisters[0] && !I2CTWI_writeBusy){
uint8_t cmd = I2CTWI_writeRegisters[0];
I2CTWI_writeRegisters[0] = 0;

switch(cmd){

case master_status:
param1=I2CTWI_writeRegisters[1]; // Status Start
move_state=param1;
break;

}
}

//I2CTWI_readRegisters[1]=readADC(ADC_BAT);
//I2CTWI_readRegisters[2]=move_state;
//I2CTWI_readRegisters[3]=Aus;
}

void receiveRC5Data(RC5data_t rc5data)
{
switch (rc5data.key_code)
{
case RC5_rechts:
setLEDs(0b000111);
move_state=status_bemerkt_rechts;
break;

case RC5_links:
setLEDs(0b111000);
move_state=status_bemerkt_links;
break;

case RC5_BWD:
setLEDs(0b100001);
move_state=status_BWD;
break;

case RC5_FWD:
setMotorDir(FWD, FWD);
moveAtSpeed(speed,speed);
move_state=status_fahren;
break;

case RC5_STOP:
setLEDs(0b111111);
moveAtSpeed(0,0);
break;

case RC5_speed_plus:
setLEDs(0b100000);
speed=speed+10;
break;

case RC5_speed_minus:
setLEDs(0b000001);
speed=speed-10;
break;

case RC5_winkel_plus:
setLEDs(0b010000);
winkel=winkel+10;
break;

case RC5_winkel_minus:
setLEDs(0b000010);
winkel=winkel-10;
break;

case RC5_90grad:
winkel=90;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_180grad:
winkel=180;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_150speed:
speed=150;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_80speed:
speed=80;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_Aus: //AN/AUS gedrück!
if (Aus) {moveAtSpeed(0,0);Aus=0;} else {Aus=1;}
break;

case RC5_ACS_Aus:
AC_System=0;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_ACS_An:
AC_System=1;
setLEDs(0b111111);
mSleep(1000);
setLEDs(0);
break;

case RC5_korrigieren_R:

move_state=status_KurveL;
break;

case RC5_korrigieren_L:
move_state=status_KurveR;
break;

case RC5_drehenR:
//task_RP6System();
setMotorDir(FWD,BWD);
move_state=status_fahren;
break;

case RC5_drehenL:
setMotorDir(BWD,FWD);
move_state=status_fahren;
break;
}
}


void acsStateChanged(void)
{
if (AC_System) {
if(obstacle_left)
move_state=status_bemerkt_rechts;

if(obstacle_right)
move_state=status_bemerkt_links;

if(obstacle_right && obstacle_left)
move_state=status_bemerkt_beide;


// -----------------------------------------------------------
// Set LEDs

if(obstacle_left && obstacle_right) // Obstacle in the middle?
statusLEDs.byte = 0b100100;
else
statusLEDs.byte = 0b000000;
statusLEDs.LED5 = obstacle_left;
statusLEDs.LED4 = (!obstacle_left); // Inverted LED5!
statusLEDs.LED2 = obstacle_right;
statusLEDs.LED1 = (!obstacle_right); // Inverted LED2!

// Update the LED values:
updateStatusLEDs(); // The LEDs are turned on/off here! __Not__ in the lines above!
}
}

void bumpersStateChanged(void)
{
if(bumper_left)
move_state=status_bemerkt_rechts;

if(bumper_right)
move_state=status_bemerkt_links;

if(bumper_left && bumper_right)
move_state=status_bemerkt_beide;
}

void task_abfrage(void)
{
task_RP6System();
masterabfrage();
}

/*void blink(void)
{
while(true) {
setLEDs(0b111111);
mSleep(500);
setLEDs(0);
mSleep(500);
}
}*/

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

int main(void)
{
initRobotBase(); //roboter einbinden
I2CTWI_initSlave(10); //als Slave10 registrieren!

setLEDs(0b111111);
mSleep(1000);
setLEDs(0b001001);

move_state=status_start; //master;

powerON(); //Verletzungsgefahr!

speed=120;

// Register Event Handlers:
ACS_setStateChangedHandler(acsStateChanged); //ACS HAndler
BUMPERS_setStateChangedHandler(bumpersStateChanged ); //Bumper HAndler
IRCOMM_setRC5DataReadyHandler(receiveRC5Data); //RC5 Handler



setACSPwrMed(); // Aktivieren des ACS in Medium

Spann=readADC(ADC_BAT);



while(true) //Wiederholen bis Akku leer
{
masterabfrage();
task_RP6System(); //task_abfrage();

switch (move_state) // ist move_state...
{
case status_start:
setLEDs(0b011011);
if(Aus){setMotorDir(FWD, FWD); moveAtSpeed(speed,speed);}
move_state=status_fahren;
break;

case status_fahren:
//writeString("Hallo");
//task_abfrage();
setLEDs(0b001001);
//Spann=readADC(ADC_BAT);
//writeInteger(Spann,DEC);
/*if (Spann<6.5) {
moveAtSpeed(0,0);
//blink();
}*/
//do
//task_abfrage();
//Nach ACS und Bumpern und RC5 suchen... (in Master-Abfrage)

//while (move_state==status_fahren);// ...bis sich etwas geändert hat! (das gleiche wie: solange status_fahren)
break;

case status_bemerkt_links:

mSleep(500);
setLEDs(0b100000);
if(Aus){rotate(speed,LEFT,winkel,1);} // ...nicht wenn AN/AUS gedrückt
move_state=status_start;
break;

case status_bemerkt_rechts:
setLEDs(0b000010);
if(Aus){rotate(speed,RIGHT,winkel,1);} // ...nicht wenn AN/AUS gedrückt
move_state=status_start;
break;

case status_bemerkt_beide:
setLEDs(0b010010);
if(Aus){rotate(speed,RIGHT,winkel,1);} // ...nicht wenn AN/AUS gedrückt
move_state=status_start;
break;

case status_BWD:
setLEDs(0b011011);
if(Aus){setMotorDir(BWD, BWD); moveAtSpeed(speed,speed);}
move_state=status_fahren;
break;

case status_KurveR:
setLEDs(0b000111);
moveAtSpeed(speed-50, speed);
do

//Nach ACS und Bumpern und RC5 suchen...
task_abfrage();
while (move_state==status_KurveR);
break;

case status_KurveL:
setLEDs(0b000111);
moveAtSpeed(speed, speed-50);
do

//Nach ACS und Bumpern und RC5 suchen...
task_abfrage();
while (move_state==status_KurveL);
break;

case status_STOP:
moveAtSpeed(0,0);
move_state=status_fahren;
break;

case status_master:
setLEDs(0b111000);
do
task_abfrage();
while(move_state==status_master);
break;
}
}
return 0;
}



Ohne euch hätte ich das Problem nicht behoben bekommen


[shadow=red:7c3d44e888][glow=red:7c3d44e888]!DANKE![/glow:7c3d44e888][/shadow:7c3d44e888]

Viele Grüsse an Alle
Adleo