/***************************************************************************/
/* INCLUDES
/***************************************************************************/
#include "delivery.nqh"
#include "../Main/control.nqh"
/***************************************************************************/
/* CONSTANTS
/***************************************************************************/
// Behaviour message map: should correspond to Prolog actionNo(+Action, -Num)
// predicate
// Primitive Actions
#define TURN_AROUND 0
#define SIGNAL_ARRIVAL 1
#define START_TO_NEXT_STATION 2
#define CLOSE 3
// Exogenous Actions
#define ARRIVE_AT_CENTER 7
#define ARRIVE_AT_SUBURBS 8
#define STOP_ABNORMALLY 4
#define PUSH_GRASP_BUTTON 5
#define PUSH_GO_BUTTON 9
// Sensors and motors
#define GRASP_BUTTON SENSOR_1 // corresponds to exogen action "PUSH_GRASP_BUTTON"
#define GO_BUTTON SENSOR_2 // corresponds to exogen action "PUSH_GO_BUTTON"
#define LIGHT_SENSOR SENSOR_3
#define LEFT_MOTOR OUT_C
#define RIGHT_MOTOR OUT_A
#define DUMPER OUT_B
// Light sensor thresholds
#define LINE_THRESHOLD 38 // Below this for black line
#define SUBURBS_THRESHOLD 50 // Above this for SUBURBS
#define CENTER_THRESHOLD 25 // Below this for CENTER
// Other constants
#define TURN_SPEED 1
#define MOVE_SPEED 1 // Normal forward speed
#define DUMPER_SPEED 7 // Speed for dumper motor
#define DUMPER_TIME 50 // Time dumper motor is engaged
#define INITIAL_TURN_TIME 2 // Time for first "sweep" when locating line
#define TURN_TIME 40 // Time to complete one revolution (roughly)
#define MOVE_WAIT 40 // Time to wait before line tracking
#define RCX_PYRAMID_DELAY 30 // Time it is safe to wait before Prolog
// will time out
/***************************************************************************/
/* VARIABLES
/***************************************************************************/
// initialize: Anything that needs to be set up when RCX is started
void initialize()
{
// Initialize sensors
SetSensor(LIGHT_SENSOR, SENSOR_LIGHT);
SetSensor(GRASP_BUTTON, SENSOR_TOUCH);
SetSensor(GO_BUTTON, SENSOR_TOUCH);
// Display value of light sensor (not essential)
SelectDisplay(DISPLAY_SENSOR_3);
// Initialize exogenous action monitors
start monitorPushButton;
}
// panicAction: What to do when we get in a PANIC state
void panicAction()
{
PlayTone(1000, 5);
stopAllBehaviours();
Off(RIGHT_MOTOR + LEFT_MOTOR); // Stop motors
}
// stopAllBehaviours: Stop behaviours
void stopAllBehaviours()
{
stop goToNextStation;
Off(RIGHT_MOTOR + LEFT_MOTOR); // Stop motors
// Reset variables
exogAction = NO_EXOG_ACTION;
}
// startBehaviour: Start a behaviour associated with action BehNum.
// Need one case for each primitive action
void startBehaviour(int behNum)
{
if (behNum == TURN_AROUND)
turnAround();
else if (behNum == SIGNAL_ARRIVAL)
signalArrival();
else if (behNum == START_TO_NEXT_STATION)
startToNextStation();
else if (behNum == CLOSE)
close();
}
// signalArrival: Robot has arrived at station
void signalArrival()
{
ClearSensor(GRASP_BUTTON);
ClearSensor(GO_BUTTON);
Off(DUMPER);
SetPower(DUMPER, DUMPER_SPEED);
OnRev(DUMPER);
Wait(DUMPER_TIME);
Off(DUMPER);
sensorVal = 0; // This is not a sensing action; return 0
}
// close: close hands
void close()
{
Off(DUMPER);
SetPower(DUMPER, DUMPER_SPEED);
OnFwd(DUMPER);
Wait(DUMPER_TIME);
Off(DUMPER);
sensorVal = 0; // This is not a sensing action; return 0
}
// turnAround: Turn the robot 180 degrees (roughly)
void turnAround()
{
int once = 1;
ClearTimer(1);
SetPower(LEFT_MOTOR + RIGHT_MOTOR, TURN_SPEED);
if (Random(5000)>2500) { //random choice of the turn direction
Fwd(LEFT_MOTOR);
Rev(RIGHT_MOTOR);
} else {
Rev(LEFT_MOTOR);
Fwd(RIGHT_MOTOR);
}
On(LEFT_MOTOR + RIGHT_MOTOR);
Wait(MOVE_WAIT); // Move off line
while (Timer(1) < TURN_TIME && LIGHT_SENSOR > LINE_THRESHOLD) {
if (Timer(1) > RCX_PYRAMID_DELAY && once == 1) { // Ask for more time -
SendMessage(RCX_DELAY_MESSAGE); // once only
once = 0;
}
}
Off(LEFT_MOTOR + RIGHT_MOTOR);
sensorVal = 0;
}
// startToNextStation: Start moving towards the next way station
void startToNextStation()
{
start goToNextStation;
sensorVal = 0;
}
// goToNextStation: Line following behaviour
// Some ideas in this code are based on Ch 8 of "Dave Baum's Definitive
// Guide to LEGO MINDSTORMS", APress 2000. http://www.apress.com/Mindstorms/
task goToNextStation()
{
int direction = 1, time = INITIAL_TURN_TIME, firstLoop = 1, done = 0;
ClearTimer(0);
// Start moving - move clear of landmark
SetPower(LEFT_MOTOR + RIGHT_MOTOR, MOVE_SPEED);
OnFwd(LEFT_MOTOR + RIGHT_MOTOR);
Wait(MOVE_WAIT);
while(done != 1) {
if (LIGHT_SENSOR < LINE_THRESHOLD && LIGHT_SENSOR > CENTER_THRESHOLD) { // On the line
ClearTimer(0);
time = INITIAL_TURN_TIME;
firstLoop = 1;
SetPower(RIGHT_MOTOR + LEFT_MOTOR, MOVE_SPEED);
Fwd(RIGHT_MOTOR + LEFT_MOTOR);
} else if (LIGHT_SENSOR < CENTER_THRESHOLD) { // Found the center
Off(RIGHT_MOTOR + LEFT_MOTOR);
exogAction = ARRIVE_AT_CENTER;
done = 1;
} else if (LIGHT_SENSOR > SUBURBS_THRESHOLD) { // Found a suburb
Off(RIGHT_MOTOR + LEFT_MOTOR);
exogAction = ARRIVE_AT_SUBURBS;
done = 1;
} else if (Timer(0) > time || firstLoop == 1) { // Find line again
if (firstLoop != 1) { // Do this just after we come off the line
direction *= -1;
time *= 3;
if (Timer(0) > TURN_TIME) { // RCX is lost
done = 1;
exogAction = STOP_ABNORMALLY;
}
}
firstLoop = 0;
SetPower(RIGHT_MOTOR + LEFT_MOTOR, TURN_SPEED);
if (direction == 1) {
Rev(LEFT_MOTOR);
Fwd(RIGHT_MOTOR);
} else {
Fwd(LEFT_MOTOR);
Rev(RIGHT_MOTOR);
}
}
}
}
/***************************************************************************
/* EXOGENOUS MONITORING
/***************************************************************************/
// monitorPushButton: Monitor status of pushbutton - it acts as a "go" button
task monitorPushButton()
{
while(true) {
if (GRASP_BUTTON == 1 ) {
exogAction = PUSH_GRASP_BUTTON;
OnFwd(DUMPER);
Wait(DUMPER_TIME);
};
if (GO_BUTTON == 1 ) {
exogAction = PUSH_GO_BUTTON;
}
}
}
/***************************************************************************
/* EOF: Delivery/delivery.nqc
/***************************************************************************/