/*************************************************************************** /* /* FILE: Delivery/delivery.nqc /* /* NQC code for Legolog delivery example /* /* WRITTEN BY: Maurice Pagnucco and Hector J. Levesque /* REVISED: June 15, 2000 /* TESTED: nqc 2.1 r1 /* /*************************************************************************** /* June 15, 2000 /* /* This software was developed by the Cognitive Robotics Group under the /* direction of Hector Levesque and Ray Reiter. /* /* Do not distribute without permission. /* Include this notice in any copy made. /* /* /* Copyright (c) 2000 by The University of Toronto, /* Toronto, Ontario, Canada. /* /* All Rights Reserved /* /* Permission to use, copy, and modify, this software and its /* documentation for non-commercial research purpose is hereby granted /* without fee, provided that the above copyright notice appears in all /* copies and that both the copyright notice and this permission notice /* appear in supporting documentation, and that the name of The University /* of Toronto not be used in advertising or publicity pertaining to /* distribution of the software without specific, written prior /* permission. The University of Toronto makes no representations about /* the suitability of this software for any purpose. It is provided "as /* is" without express or implied warranty. /* /* THE UNIVERSITY OF TORONTO DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS /* SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND /* FITNESS, IN NO EVENT SHALL THE UNIVERSITY OF TORONTO BE LIABLE FOR ANY /* SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER /* RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF /* CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN /* CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. /* /*************************************************************************** /* /* This file provides application dependent code: /* -- void initialize(): perform any necessary initialization of RCX /* e.g., define input/output types, display sensor values, /* start exogenous action monitors, etc. /* -- void startBehaviour(int behNum): start action corresponding to behNum. /* This number must correspond to that in an application dependent /* Golog file (e.g., delivery.pl) defined using predicate actionNum/2 /* -- void stopAllBehaviours(): terminate all currently running behaviour tasks. /* Can reset variables here too /* -- void panicAction(): action to take when RCX status becomes PANIC i.e., /* when Prolog is not accepting transmissions /* Plus tasks, functions, subroutines for the behaviours. /* /***************************************************************************/ /***************************************************************************/ /* 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 // Exogenous Actions #define ARRIVE_AT_STATION 3 #define STOP_ABNORMALLY 4 #define PUSH_GO_BUTTON 5 // Sensors and motors #define GO_BUTTON SENSOR_1 #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 42 // Below this for black line #define STOPPER_THRESHOLD 57 // Above this for way station // Other constants #define TURN_SPEED 4 // Needs to be a bit higher than move speed to turn #define MOVE_SPEED 1 // Normal forward speed #define DUMPER_SPEED 1 // Speed for dumper motor #define DUMPER_TIME 50 // Time dumper motor is engaged #define INITIAL_TURN_TIME 3 // 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(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(); } // signalArrival: Robot has arrived at station void signalArrival() { SetPower(DUMPER, DUMPER_SPEED); OnRev(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); Fwd(LEFT_MOTOR); Rev(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) { // 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 > STOPPER_THRESHOLD) { // Found a stopper Off(RIGHT_MOTOR + LEFT_MOTOR); exogAction = ARRIVE_AT_STATION; 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 (GO_BUTTON == 1 ) { PlaySound(SOUND_UP); exogAction = PUSH_GO_BUTTON; OnFwd(DUMPER); Wait(DUMPER_TIME); Float(DUMPER); ClearSensor(GO_BUTTON); } } /*************************************************************************** /* EOF: Delivery/delivery.nqc /***************************************************************************/