4 * Copyright (c) 1993-2002 Sun Microsystems, Inc. All Rights Reserved.
\r
6 * This software is the confidential and proprietary information of Sun
\r
7 * Microsystems, Inc. ("Confidential Information"). You shall not
\r
8 * disclose such Confidential Information and shall use it only in
\r
9 * accordance with the terms of the license agreement you entered into
\r
12 * SUN MAKES NO REPRESENTATIONS OR WARRANTIES ABOUT THE SUITABILITY OF
\r
13 * THE SOFTWARE, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
\r
14 * TO THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
\r
15 * PARTICULAR PURPOSE, OR NON-INFRINGEMENT. SUN SHALL NOT BE LIABLE FOR
\r
16 * ANY DAMAGES SUFFERED BY LICENSEE AS A RESULT OF USING, MODIFYING OR
\r
17 * DISTRIBUTING THIS SOFTWARE OR ITS DERIVATIVES.
\r
21 * Robot's Main class - instantiates all required classes and resources.
\r
23 * @author Michael Gesundheit
\r
28 public class RobotMain {
\r
31 private static boolean DEBUG1 = true;
\r
33 private static boolean DEBUG = true;
\r
35 public static final int OFF_MODE = 1;
\r
36 public static final int ON_MODE = 2;
\r
37 public static final int MANUAL_MODE = 1;
\r
38 public static final int AUTONOMUS_MODE = 2;
\r
39 public static final int ON_OFF = 128;// 0x80
\r
40 public static final int MANUAL_AUTONOMUS = 32; // 0x20
\r
41 public static final int LINE_SENSORS = 64; // 0x40
\r
42 public static final int SONAR_SENSORS = 96; // 0x60
\r
43 public static final int ALL_COMMANDS = 0xe0;
\r
45 public static final byte LF_FRONT = 0x1;
\r
46 public static final byte RT_FRONT = 0x2;
\r
47 public static final byte RT_SIDE = 0x4;
\r
48 public static final byte BK_SIDE = 0x8;
\r
49 public static final byte LF_SIDE = 0x10;
\r
50 public static final byte ALL_SONARS = 0x1f;
\r
51 public static final byte LF_RT_FRONT = 0x3; // LF_FRONT | RT_FRONT;
\r
52 public static final byte LS_LEFT = 0x1;
\r
53 public static final byte LS_RIGHT = 0x2;
\r
54 public static final byte LS_REAR = 0x4;
\r
55 public static final byte LS_LEFT_RIGHT = 0x3;
\r
56 public static final byte LS_LEFT_REAR = 0x5;
\r
57 public static final byte LS_RIGHT_REAR = 0x6;
\r
58 public static final byte LS_ALL = 0x7;
\r
59 public static final int ALL_DATA = 0x1f;
\r
62 private PWMManager pwmm;
\r
64 private StrategyMgr strategyMgr;
\r
67 private int onOffMode = ON_MODE;
\r
69 private int manualAutonomusMode = AUTONOMUS_MODE;
\r
71 private byte lineSensorsMask;
\r
73 private byte sonarSensors;
\r
75 private byte currentCommand;
\r
77 private byte privSonars;
\r
79 private byte privLineSensors;
\r
81 private byte currentSonars;
\r
83 public String pwmSelection;
\r
86 * Constructor for the class for the robot main thread.
\r
92 * Processes sonar sensor input. This method is called each time new sonar sensor data arrives,
\r
93 * and each time that a mode switch occurs between ON/OFF and Manual Override.
\r
95 void processSonars() {
\r
96 strategyMgr.processSonars(sonarSensors);
\r
100 * Processes line sensor input. This method is called each time new line sensor data arrives, and
\r
101 * each time that a mode switch occurs between ON/OFF and Manual Override.
\r
103 void processLineSensors() {
\r
104 strategyMgr.processLineSensors(lineSensorsMask);
\r
109 * Resumes motors as per the last sonar command.
\r
116 * Extracts sonar sensor data from the adjunct sensor controller and from line sensor data from
\r
117 * the JStamp line sensor pins.
\r
120 private void processIOCommand() {
\r
124 // synchronized (obj1) {
\r
125 int data = (int) (currentCommand & ALL_DATA);
\r
126 int opCode = (int) (currentCommand & 0xe0); // ALL_COMMANDS);
\r
130 System.out.println("processIOCommand: Default: opCode = " + Integer.toString((int) opCode)
\r
131 + " data = " + Integer.toString((int) data));
\r
133 switch ((int) opCode) {
\r
136 System.out.println("processIO: ON_OFF....");
\r
138 if ((data & 0x1) == 0x1) {
\r
139 System.out.println("processIO: ON MODE........");
\r
140 onOffMode = ON_MODE;
\r
141 processLineSensors();
\r
144 System.out.println("processIO: OFF MODE.......");
\r
145 onOffMode = OFF_MODE;
\r
146 strategyMgr.stop();
\r
149 case MANUAL_AUTONOMUS:
\r
151 System.out.println("processIO: Setting manual_autonomus mode");
\r
153 if ((data & 0x1) == 0x1) {
\r
154 manualAutonomusMode = MANUAL_MODE;
\r
155 pwmm.setManualMode();
\r
157 manualAutonomusMode = AUTONOMUS_MODE;
\r
158 pwmm.setAutomatedMode();
\r
159 processLineSensors();
\r
165 // System.out.println("processIO: Line Sensors.................."
\r
166 // + Integer.toBinaryString((int) (data & LS_ALL)));
\r
168 lineSensorsMask = (byte) (data & LS_ALL);
\r
171 * Line sensors with '0' data means the robot moved off the edge line, and there is nothing to
\r
174 if (((data & LS_ALL) == 0) || ((data & LS_ALL) == privLineSensors)) {
\r
175 privLineSensors = (byte) (data & LS_ALL);
\r
178 if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {
\r
180 System.out.println("processIO: Line Sensors..Process...!!!");
\r
181 processLineSensors();
\r
184 case SONAR_SENSORS:
\r
186 // System.out.println("processIO: SONAR_SENORS: bits = ......"
\r
187 // + Integer.toBinaryString((int) (currentCommand & ALL_SONARS)));
\r
189 currentSonars = (byte) (data & ALL_SONARS);
\r
191 * No need to synchronized on this variable assignment since all referring code is on the same
\r
194 sonarSensors = (byte) currentSonars;
\r
195 if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {
\r
200 strategyMgr.stop();
\r
201 System.out.println("processIOCommand: Default: opCode = " + Integer.toString((int) opCode)
\r
202 + " data = " + Integer.toString((int) data));
\r
207 * Sets the simulation mode on in the IOManager.
\r
209 // static public void setSimulation() {
\r
210 // sm.setSimulation();
\r
214 * Resets the simulation mode in the IOManager.
\r
216 // static public void resetSimulation() {
\r
217 // sm.resetSimulation();
\r
221 * Saves the current IOManager command byte locally.
\r
223 public void setIOManagerCommand(byte[] cmd) {
\r
224 // synchronized (obj1) {
\r
225 currentCommand = cmd[0];
\r
227 // synchronized (obj) {
\r
230 } catch (IllegalMonitorStateException ie) {
\r
231 System.out.println("IllegalMonitorStateException caught trying to notify");
\r
237 * Sets debug messaging state: true - Activate debug messages false - deactivate debug messages
\r
239 static public void setDebug(boolean debug) {
\r
244 * Runs the robot's main thread.
\r
246 public static void main( String args[]) {
\r
248 TestSensorInput.init();
\r
249 RobotMain r = new RobotMain();
\r
254 public void doit() {
\r
256 * RealTime management of the robot behaviour based on sensors and commands input.
\r
260 * Set the low level PWM interface type, e.g. "rtsj" or "native" (ajile library-based).
\r
262 pwmSelection = "rtsj";
\r
264 System.out.println("PWM Selction is: " + pwmSelection);
\r
266 pwmm = new PWMManager(pwmSelection);
\r
268 // Pass in the PWMManager for callbacks.
\r
269 // sm = new IOManager(pwmm);
\r
270 // ram = new RemoteApplicationManager(pwmm);
\r
271 MotorControl mc = new MotorControl(100, 100);
\r
272 strategyMgr = new StrategyMgr(mc);
\r
275 * Sets initial values for the speed and agility parameters. Speed and agility are arbitrary
\r
276 * scale factors for the overall speed and speed of turns, respectively. These work with PWM via
\r
277 * the native ajile libraries, but do not work well with the RTSJ implementation due to limits
\r
278 * on the granularity of timing for the PWM pulse (quantization).
\r
280 pwmm.setSpeedAgilityFactors(100, 100);
\r
284 * Robot's initial state is "ON" by default. Set this parameter to "OFF" if the robot is to be
\r
285 * started over the TCP/IP link.
\r
287 // issueCommand("OFF");
\r
293 public void start(){
\r
295 boolean active = true;
\r
297 SSJAVA: while (active) {
\r
299 Command com = HWSimulator.getCommand();
\r
300 currentCommand = com.command;
\r
301 sonarSensors = com.sonarSensors;
\r
302 lineSensorsMask = com.lineSensorsMask;
\r
303 manualAutonomusMode = com.manualAutonomusMode;
\r
304 onOffMode = com.onOffMode;
\r
306 if (currentCommand == -1) {
\r
309 System.out.println("currentCommand=" + currentCommand);
\r
310 processIOCommand();
\r
311 // erase current settings
\r
316 public void initialize() {
\r
317 privLineSensors = 0;
\r
318 currentCommand = 0;
\r