Move05 ändern

[StGla90]

Hallo,
ich versuche nun schon seit über 2 Std. das Beispiel Move05 zu ändern es funktioniert aber nicht, ich dachte ich finde den Fehler nach langen probieren selbst aber das ist leider nicht der fall.
Ziel soll sein das der Roboter stehen bleibt wenn es dunkel wird und wieder los fährt wenn es hell wird.
Aktuell ist es so, das wenn ich das Programm starte zwar auf ACS und Bumper reagiert wird aber er nicht fährt, also cruise nicht geht.



#include "RP6RobotBaseLib.h"

#define IDLE 0

typedef struct {
uint8_t speed_left;
uint8_t speed_right;
unsigned dir:2;
unsigned move:1;
unsigned rotate:1;
uint16_t move_value;
uint8_t state;
} behaviour_command_t;

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


#define CRUISE_SPEED_FWD 100
#define MOVE_FORWARDS 1

behaviour_command_t cruise = {CRUISE_SPEED_FWD, CRUISE_SPEED_FWD, FWD,
false, false, 0, MOVE_FORWARDS};

void behaviour_cruise(void)
{
}

#define ESCAPE_SPEED_BWD 100
#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};

void behaviour_escape(void)
{
static uint8_t bump_count = 0;

switch(escape.state)
{
case IDLE:
break;
case ESCAPE_FRONT:
escape.speed_left = ESCAPE_SPEED_BWD;
escape.dir = BWD;
escape.move = true;
if(bump_count > 3)
escape.move_value = 220;
else
escape.move_value = 160;
escape.state = ESCAPE_FRONT_WAIT;
bump_count+=2;
break;
case ESCAPE_FRONT_WAIT:
if(!escape.move)
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
if(bump_count > 3)
{
escape.move_value = 100;
escape.dir = RIGHT;
bump_count = 0;
}
else
{
escape.dir = LEFT;
escape.move_value = 70;
}
escape.rotate = true;
escape.state = ESCAPE_WAIT_END;
}
break;
case ESCAPE_LEFT:
escape.speed_left = ESCAPE_SPEED_BWD;
escape.dir = BWD;
escape.move = true;
if(bump_count > 3)
escape.move_value = 190;
else
escape.move_value = 150;
escape.state = ESCAPE_LEFT_WAIT;
bump_count++;
break;
case ESCAPE_LEFT_WAIT:
if(!escape.move)
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
escape.dir = RIGHT;
escape.rotate = true;
if(bump_count > 3)
{
escape.move_value = 110;
bump_count = 0;
}
else
escape.move_value = 80;
escape.state = ESCAPE_WAIT_END;
}
break;
case ESCAPE_RIGHT:
escape.speed_left = ESCAPE_SPEED_BWD ;
escape.dir = BWD;
escape.move = true;
if(bump_count > 3)
escape.move_value = 190;
else
escape.move_value = 150;
escape.state = ESCAPE_RIGHT_WAIT;
bump_count++;
break;
case ESCAPE_RIGHT_WAIT:
if(!escape.move)
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
escape.dir = LEFT;
escape.rotate = true;
if(bump_count > 3)
{
escape.move_value = 110;
bump_count = 0;
}
else
escape.move_value = 80;
escape.state = ESCAPE_WAIT_END;
}
break;
case ESCAPE_WAIT_END:
if(!(escape.move || escape.rotate))
escape.state = IDLE;
break;
}
}

void bumpersStateChanged(void)
{
if(bumper_left && bumper_right)
{
escape.state = ESCAPE_FRONT;
}
else if(bumper_left)
{
if(escape.state != ESCAPE_FRONT_WAIT)
escape.state = ESCAPE_LEFT;
}
else if(bumper_right)
{
if(escape.state != ESCAPE_FRONT_WAIT)
escape.state = ESCAPE_RIGHT;
}
}


#define AVOID_SPEED_L_ARC_LEFT 30
#define AVOID_SPEED_L_ARC_RIGHT 90
#define AVOID_SPEED_R_ARC_LEFT 90
#define AVOID_SPEED_R_ARC_RIGHT 30
#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};

void behaviour_avoid(void)
{
static uint8_t last_obstacle = LEFT;
static uint8_t obstacle_counter = 0;
switch(avoid.state)
{
case IDLE:
if(obstacle_right && obstacle_left)
avoid.state = AVOID_OBSTACLE_MIDDLE;
else if(obstacle_left)
avoid.state = AVOID_OBSTACLE_LEFT;
else if(obstacle_right)
avoid.state = AVOID_OBSTACLE_RIGHT;
break;
case AVOID_OBSTACLE_MIDDLE:
avoid.dir = last_obstacle;
avoid.speed_left = AVOID_SPEED_ROTATE;
avoid.speed_right = AVOID_SPEED_ROTATE;
if(!(obstacle_left || obstacle_right))
{
if(obstacle_counter > 3)
{
obstacle_counter = 0;
setStopwatch4(0);
}
else
setStopwatch4(400);
startStopwatch4();
avoid.state = AVOID_END;
}
break;
case AVOID_OBSTACLE_RIGHT:
avoid.dir = FWD;
avoid.speed_left = AVOID_SPEED_L_ARC_LEFT;
avoid.speed_right = AVOID_SPEED_L_ARC_RIGHT;
if(obstacle_right && obstacle_left)
avoid.state = AVOID_OBSTACLE_MIDDLE;
if(!obstacle_right)
{
setStopwatch4(500);
startStopwatch4();
avoid.state = AVOID_END;
}
last_obstacle = RIGHT;
obstacle_counter++;
break;
case AVOID_OBSTACLE_LEFT:
avoid.dir = FWD;
avoid.speed_left = AVOID_SPEED_R_ARC_LEFT;
avoid.speed_right = AVOID_SPEED_R_ARC_RIGHT;
if(obstacle_right && obstacle_left)
avoid.state = AVOID_OBSTACLE_MIDDLE;
if(!obstacle_left)
{
setStopwatch4(500);
startStopwatch4();
avoid.state = AVOID_END;
}
last_obstacle = LEFT;
obstacle_counter++;
break;
case AVOID_END:
if(getStopwatch4() > 1000)
{
stopStopwatch4();
setStopwatch4(0);
avoid.state = IDLE;
}
break;
}
}

void acsStateChanged(void)
{
if(obstacle_left && obstacle_right)
statusLEDs.byte = 0b100100;
else
statusLEDs.byte = 0b000000;
statusLEDs.LED5 = obstacle_left;
statusLEDs.LED4 = (!obstacle_left);
statusLEDs.LED2 = obstacle_right;
statusLEDs.LED1 = (!obstacle_right);
updateStatusLEDs();
}

#define LIGHT_OFF_SPEED 0
#define LIGHT_OFF 1

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

void behaviour_light(void)
{
switch(light.state)
{
case IDLE:
break;
case LIGHT_OFF:
light.speed_left = LIGHT_OFF_SPEED;
light.dir = FWD;
break;
}

}

#define RANGE 300
void LightOnOff(void)
{
if(adcLSL <= RANGE || adcLSR <= RANGE)
{
light.state = LIGHT_OFF;
}
}

void moveCommand(behaviour_command_t * cmd)
{
if(cmd->move_value > 0)
{
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;
}
else if(cmd->light)
{
move(cmd->speed_left, cmd->dir, DIST_MM(cmd->move_value), false);
}
}
else if(!(cmd->move || cmd->rotate))
{
changeDirection(cmd->dir);
moveAtSpeed(cmd->speed_left,cmd->speed_right);
}
else if(isMovementComplete())
{
cmd->rotate = false;
cmd->move = false;
}
}


void behaviourController(void)
{

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

if(escape.state != IDLE)
moveCommand(&escape);
else if(avoid.state != IDLE)
moveCommand(&avoid);
else if(light.state != IDLE)
moveCommand(&light);
else if(cruise.state != IDLE)
moveCommand(&cruise);
else
moveCommand(&STOP);
}

int main(void)
{
initRobotBase();
setLEDs(0b111111);
mSleep(2500);
setLEDs(0b100100);

powerON();
setACSPwrMed();

BUMPERS_setStateChangedHandler(bumpersStateChanged );
ACS_setStateChangedHandler(acsStateChanged);
LightOnOff();

while(true)
{
behaviourController();
task_RP6System();
}
return 0;
}


Danke im vorraus

- - - Aktualisiert - - -

Habe gerade entdeckt das ich in der moveCommand noch keinen eintrag gemacht habe. Das habe ich jetzt gemacht funktioniert aber auch nicht, ich bekomme einen Fehler bei MakeAll. Aktuelles Programm siehe oben.
Fehler: RP6Base_Move_05_Licht_Fahren.c:302: error: 'behaviour_command_t' has no member named 'light'

- - - Aktualisiert - - -

So ich habe jetzt nochmal komplett neu angesetzt und habe es hinbekommen! Er reagiert im dunkeln jetzt zwar noch auf ACS und Bumpers das ändere ich dann als nächstes. PowerOFF soll dann auch noch dazu kommen.
MfG

[StGla90]

So das Programm ist nun fertig (nach vielen Versuchen und viel Geduld) es ist jetzt so wie ich es mir vorstelle.
Oben im Programm in "LightOnOff" wird geprüft ob es hell oder dunkel ist und entsprechend der cruise state geändert. Die werte sind dann entsprechend in void behaviour_cruise(void) eingetragen.

Dann habe ich noch die Funktion void energieSave(void) eingefügt. Ich hatte PowerOFF() anfangs direkt in der Funktion LightOnOff (wenn dunkel --> powerOFF), das hat allerdings nicht funktioniert da immer wenn er grade bei avoid bzw. escape war und ich das Licht aus gemacht habe ein Fehler kam:
Das Programm hat sich "aufgehangen" also er ist gar nicht mehr gefahren (auch wenn es Hell war) und die 4 Roten LEDs haben geblinkt. Warum weis ich nicht?
Jetzt habe ich es so gemacht in void energieSave(void) wird erst geprüft ob avoid bzw. escape fertig ist und erst dann geht er in powerOFF.

Jetzt trotzdem nochmal die Frage an euch, ist das Programm so sinnvoll oder hätte man es besser/ kürzer schreiben können?





#include "RP6RobotBaseLib.h"

#define IDLE 0

typedef struct {
uint8_t speed_left;
uint8_t speed_right;
unsigned dir:2;
unsigned move:1;
unsigned rotate:1;
uint16_t move_value;
uint8_t state;
} behaviour_command_t;

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


#define CRUISE_SPEED_FWD 100
#define RANGE 300
#define CRUISE_SPEED_LIGHT_OFF 0
#define MOVE_FORWARDS 1
#define LIGHT_OFF 2
behaviour_command_t cruise = {0, 0, FWD, false, false, 0, IDLE};


void behaviour_cruise(void)
{
switch(cruise.state)
{
case IDLE:
break;
case MOVE_FORWARDS:
cruise.speed_left = CRUISE_SPEED_FWD;
cruise.speed_right = CRUISE_SPEED_FWD;
cruise.dir = FWD;
break;
case LIGHT_OFF:
cruise.speed_left = CRUISE_SPEED_LIGHT_OFF;
cruise.speed_right = CRUISE_SPEED_LIGHT_OFF;
cruise.dir = FWD;
break;
}
}



void LightOnOff(void)
{

if(adcLSL > RANGE || adcLSR > RANGE)
{
cruise.state = MOVE_FORWARDS;
}
else if(adcLSL <= RANGE || adcLSR <= RANGE)
{
cruise.state = LIGHT_OFF;
}
}


#define ESCAPE_SPEED_BWD 100
#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};

void behaviour_escape(void)
{
static uint8_t bump_count = 0;

switch(escape.state)
{
case IDLE:
break;
case ESCAPE_FRONT:
escape.speed_left = ESCAPE_SPEED_BWD;
escape.dir = BWD;
escape.move = true;
if(bump_count > 3)
escape.move_value = 220;
else
escape.move_value = 160;
escape.state = ESCAPE_FRONT_WAIT;
bump_count+=2;
break;
case ESCAPE_FRONT_WAIT:
if(!escape.move)
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
if(bump_count > 3)
{
escape.move_value = 100;
escape.dir = RIGHT;
bump_count = 0;
}
else
{
escape.dir = LEFT;
escape.move_value = 70;
}
escape.rotate = true;
escape.state = ESCAPE_WAIT_END;
}
break;
case ESCAPE_LEFT:
escape.speed_left = ESCAPE_SPEED_BWD;
escape.dir = BWD;
escape.move = true;
if(bump_count > 3)
escape.move_value = 190;
else
escape.move_value = 150;
escape.state = ESCAPE_LEFT_WAIT;
bump_count++;
break;
case ESCAPE_LEFT_WAIT:
if(!escape.move)
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
escape.dir = RIGHT;
escape.rotate = true;
if(bump_count > 3)
{
escape.move_value = 110;
bump_count = 0;
}
else
escape.move_value = 80;
escape.state = ESCAPE_WAIT_END;
}
break;
case ESCAPE_RIGHT:
escape.speed_left = ESCAPE_SPEED_BWD ;
escape.dir = BWD;
escape.move = true;
if(bump_count > 3)
escape.move_value = 190;
else
escape.move_value = 150;
escape.state = ESCAPE_RIGHT_WAIT;
bump_count++;
break;
case ESCAPE_RIGHT_WAIT:
if(!escape.move)
{
escape.speed_left = ESCAPE_SPEED_ROTATE;
escape.dir = LEFT;
escape.rotate = true;
if(bump_count > 3)
{
escape.move_value = 110;
bump_count = 0;
}
else
escape.move_value = 80;
escape.state = ESCAPE_WAIT_END;
}
break;
case ESCAPE_WAIT_END:
if(!(escape.move || escape.rotate))
escape.state = IDLE;
break;
}
}

void bumpersStateChanged(void)
{
if(bumper_left && bumper_right)
{
escape.state = ESCAPE_FRONT;
}
else if(bumper_left)
{
if(escape.state != ESCAPE_FRONT_WAIT)
escape.state = ESCAPE_LEFT;
}
else if(bumper_right)
{
if(escape.state != ESCAPE_FRONT_WAIT)
escape.state = ESCAPE_RIGHT;
}
}


#define AVOID_SPEED_L_ARC_LEFT 30
#define AVOID_SPEED_L_ARC_RIGHT 90
#define AVOID_SPEED_R_ARC_LEFT 90
#define AVOID_SPEED_R_ARC_RIGHT 30
#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};

void behaviour_avoid(void)
{
static uint8_t last_obstacle = LEFT;
static uint8_t obstacle_counter = 0;
switch(avoid.state)
{
case IDLE:
if(obstacle_right && obstacle_left)
avoid.state = AVOID_OBSTACLE_MIDDLE;
else if(obstacle_left)
avoid.state = AVOID_OBSTACLE_LEFT;
else if(obstacle_right)
avoid.state = AVOID_OBSTACLE_RIGHT;
break;
case AVOID_OBSTACLE_MIDDLE:
avoid.dir = last_obstacle;
avoid.speed_left = AVOID_SPEED_ROTATE;
avoid.speed_right = AVOID_SPEED_ROTATE;
if(!(obstacle_left || obstacle_right))
{
if(obstacle_counter > 3)
{
obstacle_counter = 0;
setStopwatch4(0);
}
else
setStopwatch4(400);
startStopwatch4();
avoid.state = AVOID_END;
}
break;
case AVOID_OBSTACLE_RIGHT:
avoid.dir = FWD;
avoid.speed_left = AVOID_SPEED_L_ARC_LEFT;
avoid.speed_right = AVOID_SPEED_L_ARC_RIGHT;
if(obstacle_right && obstacle_left)
avoid.state = AVOID_OBSTACLE_MIDDLE;
if(!obstacle_right)
{
setStopwatch4(500);
startStopwatch4();
avoid.state = AVOID_END;
}
last_obstacle = RIGHT;
obstacle_counter++;
break;
case AVOID_OBSTACLE_LEFT:
avoid.dir = FWD;
avoid.speed_left = AVOID_SPEED_R_ARC_LEFT;
avoid.speed_right = AVOID_SPEED_R_ARC_RIGHT;
if(obstacle_right && obstacle_left)
avoid.state = AVOID_OBSTACLE_MIDDLE;
if(!obstacle_left)
{
setStopwatch4(500);
startStopwatch4();
avoid.state = AVOID_END;
}
last_obstacle = LEFT;
obstacle_counter++;
break;
case AVOID_END:
if(getStopwatch4() > 1000)
{
stopStopwatch4();
setStopwatch4(0);
avoid.state = IDLE;
}
break;
}
}

void acsStateChanged(void)
{
if(obstacle_left && obstacle_right)
statusLEDs.byte = 0b100100;
else
statusLEDs.byte = 0b000000;
statusLEDs.LED5 = obstacle_left;
statusLEDs.LED4 = (!obstacle_left);
statusLEDs.LED2 = obstacle_right;
statusLEDs.LED1 = (!obstacle_right);
updateStatusLEDs();
}


void energieSave(void)
{
if(avoid.state == IDLE && escape.state == IDLE)
{
if(cruise.state == MOVE_FORWARDS)
{
powerON();
setACSPwrMed();
}
else if(cruise.state == LIGHT_OFF)
{
powerOFF();
}

}
}



void moveCommand(behaviour_command_t * cmd)
{
if(cmd->move_value > 0)
{
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;
}
else if(!(cmd->move || cmd->rotate))
{
changeDirection(cmd->dir);
moveAtSpeed(cmd->speed_left,cmd->speed_right);
}
else if(isMovementComplete())
{
cmd->rotate = false;
cmd->move = false;
}
}


void behaviourController(void)
{

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

if(escape.state != IDLE)
moveCommand(&escape);
else if(avoid.state != IDLE)
moveCommand(&avoid);
else if(cruise.state != IDLE)
moveCommand(&cruise);
else
moveCommand(&STOP);
}

int main(void)
{
initRobotBase();
setLEDs(0b111111);
mSleep(2500);
setLEDs(0b100100);


BUMPERS_setStateChangedHandler(bumpersStateChanged );
ACS_setStateChangedHandler(acsStateChanged);


while(true)
{
behaviourController();
task_RP6System();
LightOnOff();
energieSave();
}
return 0;
}

[inka]

hi allerseits,

ich habe nun versucht den code für die m32 umzuschreiben. Das gelang, allerdings nur teilweise:

die funktionen powerON() und powerOFF() z.b. wurden beim compilieren nicht gefunden...

festgestellt habe ich, dass diese Funktionen in der "RP6RobotBaseLib.c" beinhaltet sind, in der "RP6Base_I2CSlave.c" (die ja in der base läuft) ebenfalls, in der "RP6ControlLib.c" , die bei der verwendung der m32 statt der baseLib inkludiert wird allerdings nicht. Ich verstehe nicht warum die dort fehlen?

Reicht es diese funktionen, also das hier


void powerON(void)
{
PORTB |= PWRON;
#ifdef POWER_ON_WARNING
if(leds_on < 4)
leds_on = 3;
#endif
}

void powerOFF(void)
{
PORTB &= ~PWRON;
#ifdef POWER_ON_WARNING
if(leds_on < 4)
leds_on = (leds_on ? 1 : (statusLEDs.byte && 1));
#endif
}

in der "RP6ControlLib.c" (und der *.h natürlich auch) nachzutragen? Oder sollte ich die funktionen in meiner ohnehin vorhandenen "standard.c" und *.h einfügen um die coltrolLib im originalzustand lassen zu können?

sonst noch was übersehen? wer hat einen tipp für mich?

[Dirk]

Hi inka,

der I2C-Slave in der RP6Base schaltet die POWER standardmäßig und dauerhaft EIN.
Du brauchst also powerON und powerOFF normalerweise nicht, wenn du die Base als Slave über einen Master ansteuerst, weil man die Power auf dem Slave am besten AN lassen sollte, weil sonst viele Funktionen nicht funktionieren.

Trotzdem kannst du diese Funktionen auch über einen Master "fernsteuern":
powerON: I2CTWI_transmit2Bytes(I2C_RP6_BASE_ADR, 0, CMD_POWER_ON);
powerOFF: I2CTWI_transmit2Bytes(I2C_RP6_BASE_ADR, 0, CMD_POWER_OFF);

Du könntest also als Funktionen in dein Programm (z.B. powerOFF) so aufnehmen:
void powerOFF(void)
{
I2CTWI_transmit2Bytes(I2C_RP6_BASE_ADR, 0, CMD_POWER_OFF);
}