/***************************************************************************/

/* 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

/***************************************************************************/