Code:
/*
* ****************************************************************************
* 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_requestedDataReady);
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
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;
}
Zitieren
Lesezeichen