Code:
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M32 Examples
* *****************************************************************************
*/
/*****************************************************************************/
// Includes:
#include "RP6ControlLib.h" // The RP6 Control Library.
#include "RP6I2CmasterTWI.h" // I2C Master Library
#include "RP6Control_MultiIOLib.h"
#include "RP6Control_I2CMasterLib.h"
#include "RP6Control_LFSBumperLib.h"
/*****************************************************************************/
#define IDLE 0
/*****************************************************************************/
/*****************************************************************************/
// Behaviour command type:
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};
// *****************************************************************************
// Cruise Behaviour:
// *****************************************************************************
#define CRUISE_SPEED_FWD 120 // Standard Speed wenn keine Hindernisse endeckt werden!
#define MOVE_FORWARDS 1
behaviour_command_t cruise = {CRUISE_SPEED_FWD, CRUISE_SPEED_FWD, FWD, false, false, 0, MOVE_FORWARDS};
void behaviour_cruise(void)
{
}
// ****************************************************************************
// Escape Behaviour: Verhalten bei Bumperkontakt
// ****************************************************************************
// Geschwindigkeit für verschiedene Bewegungen:
#define ESCAPE_SPEED_BWD 120 // 100
#define ESCAPE_SPEED_ROTATE 100 // 60
// Menüpunkte
#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)
{
}
// ****************************************************************************
// MIO Escape Behaviour: Verhalten bei Bumperkontakt hinten
// ****************************************************************************
// Geschwindigkeit für verschiedene Bewegungen:
#define MIO_ESCAPE_SPEED_FWD 80// 100
#define MIO_ESCAPE_SPEED_ROTATE 60 // 60
// Menüpunkte
#define MIO_ESCAPE_BACK 1
#define MIO_ESCAPE_BACK_WAIT 2
#define MIO_ESCAPE_LEFT 3
#define MIO_ESCAPE_LEFT_WAIT 4
#define MIO_ESCAPE_RIGHT 5
#define MIO_ESCAPE_RIGHT_WAIT 6
#define MIO_ESCAPE_WAIT_END 7
behaviour_command_t mio_escape = {0, 0, BWD, false, false, 0, IDLE};
void behaviour_mio_escape(void)
{
}
// ****************************************************************************
// AVOID Behaviour: Verhalten beim Erkennen von Hindernissen
// ****************************************************************************
// Geschwindigkeit für verschiedene Bewegungen:
#define AVOID_SPEED_L_ARC_LEFT 50
#define AVOID_SPEED_L_ARC_RIGHT 60 // 90
#define AVOID_SPEED_R_ARC_LEFT 60 // 90
#define AVOID_SPEED_R_ARC_RIGHT 40
#define AVOID_SPEED_ROTATE 40 // 60
// Menüpunkte
#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) // Linker und Rechter Sensoren haben was endeckt
avoid.state = AVOID_OBSTACLE_MIDDLE;
else if(obstacle_left) // Linker Sensor hat was endeckt
avoid.state = AVOID_OBSTACLE_LEFT;
else if(obstacle_right) // Rechter Sensor hat was endeckt
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) // Wir verwendeten basierte Bewegung des Timings hier!
{
stopStopwatch4();
setStopwatch4(0);
avoid.state = IDLE;
}
break;
}
}
// ****************************************************************************
// Bumpers Event handler
// ****************************************************************************
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;
}
}
// ****************************************************************************
// Multi_IO Bumpers Event handler
// ****************************************************************************
void MULTIIO_BUMPERS_stateChanged(void)
{
if(iobumper_l && iobumper_r)
{
mio_escape.state = MIO_ESCAPE_BACK;
}
else if(iobumper_l)
{
if(mio_escape.state != MIO_ESCAPE_BACK_WAIT)
mio_escape.state = MIO_ESCAPE_LEFT;
}
else if(iobumper_r)
{
if(mio_escape.state != MIO_ESCAPE_BACK_WAIT)
mio_escape.state = MIO_ESCAPE_RIGHT;
}
}
// ****************************************************************************
// ACS Event handler
// ****************************************************************************
// Auf das Verhalten kein Einfluss!
void acsStateChanged(void)
{
}
//*****************************************************************************
// FAHR KOMMANDO:
//*****************************************************************************
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;
}
}
//*****************************************************************************
// Display Ausgabe:
//*****************************************************************************
void displayBehaviour(uint8_t behave)
{
static uint8_t compare = 0;
if(compare != behave)
{
compare = behave;
clearLCD();
switch(behave)
{
case 8: writeStringLCD_P("BATTERIE LEER"); setLEDs(0b0110); break;
case 5: writeStringLCD_P("MIO_ESCAPE"); setLEDs(0b0110); break;
case 4: writeStringLCD_P("ESCAPE"); setLEDs(0b0110); break;
case 3: writeStringLCD_P("AVOID"); setLEDs(0b0110); break;
case 2: writeStringLCD_P("CRUISE"); setLEDs(0b0000); break;
case 1: writeStringLCD_P("STOP"); setLEDs(0b0000); break;
}
}
}
//*****************************************************************************
// VERHALTENS CONTROLLER
//*****************************************************************************
void behaviourController(void)
{
behaviour_mio_escape();
behaviour_escape();
behaviour_avoid();
behaviour_cruise();
if (mio_escape.state != IDLE) // Priority - 5
{
displayBehaviour(5);
moveCommand(&mio_escape);
}
else if(escape.state != IDLE) // Priority - 4
{
displayBehaviour(4);
moveCommand(&escape);
}
else if(avoid.state != IDLE) // Priority - 3
{
displayBehaviour(3);
moveCommand(&avoid);
}
else if(cruise.state != IDLE) // Priority - 2
{
displayBehaviour(2);
moveCommand(&cruise);
}
else // Lowest priority - 1
{
displayBehaviour(1);
moveCommand(&STOP); // Default command - do nothing!
// In the current implementation this never
// happens.
}
}
//-----------------------------------------------------------------------------------
/*****************************************************************************/
// I2C Requests:
void I2C_requestedDataReady(uint8_t dataRequestID)
{
if(!checkRP6Status(dataRequestID))
{
// Here you can Check other sensors/microcontrollers with their own
// request IDs - if there are any...
}
}
/*****************************************************************************/
// I2C Error Event 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();
//----------------------------------------
MULTIIO_BUMPERS_setStateChangedHandler(MULTIIO_BUMPERS_stateChanged); //State Machine Bumper hinten
BUMPERS_setStateChangedHandler(bumpersStateChanged); //State Machine Bumper vorn
ACS_setStateChangedHandler(acsStateChanged);
// ---------------------------------------
I2CTWI_initMaster(100);
I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
multiio_init();// MultiIO init!!! ( Servo Controller/ Servo power/ Voltage & current sensor / Buzzer )
BUMPERBOARD_init(); // Startet LEDS_init; MULTIIO_BUMPERS_init; SHARPS_init();
sound(180,80,25);
sound(220,80,25);
setLEDs(0b0000); // M32 LED aus
setMultiIOLEDs(0b0000); // MIO LED aus
// Betriebstest
clearLCD();
LTC2990_measure();
// ---------------------------------------
// Setup ACS power:
I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_ACS_POWER, ACS_PWR_MED);
// Enable Watchdog for Interrupt requests:
I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT, true);
// Enable timed watchdog requests:
I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT_RQ, true);
//-----------------------------------------
mSleep(3000);
clearLCD();
//-----------------------------------------
while(true)
{
task_MULTIIO_BUMPERS();
task_checkINT0();
task_I2CTWI();
behaviourController();
}
return 0;
}
Zitieren
Berechtigungen
Lesezeichen