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
26 public class RobotMain {
\r
28 private static boolean DEBUG1 = true;
\r
29 private static boolean DEBUG = true;
\r
30 private static final int OFF_MODE = 1;
\r
31 private static final int ON_MODE = 2;
\r
32 private static final int MANUAL_MODE = 1;
\r
33 private static final int AUTONOMUS_MODE = 2;
\r
34 private static final int ON_OFF = 128;// 0x80
\r
35 private static final int MANUAL_AUTONOMUS = 32; // 0x20
\r
36 public static final int LINE_SENSORS = 64; // 0x40
\r
37 private static final int SONAR_SENSORS = 96; // 0x60
\r
38 public static final int ALL_COMMANDS = 0xe0;
\r
40 public static byte LF_FRONT = 0x1;
\r
41 public static byte RT_FRONT = 0x2;
\r
42 public static byte RT_SIDE = 0x4;
\r
43 public static byte BK_SIDE = 0x8;
\r
44 public static byte LF_SIDE = 0x10;
\r
45 public static byte ALL_SONARS = 0x1f;
\r
46 public static byte LF_RT_FRONT = 0x3; // LF_FRONT | RT_FRONT;
\r
47 public static byte LS_LEFT = 0x1;
\r
48 public static byte LS_RIGHT = 0x2;
\r
49 public static byte LS_REAR = 0x4;
\r
50 public static byte LS_LEFT_RIGHT = 0x3;
\r
51 public static byte LS_LEFT_REAR = 0x5;
\r
52 public static byte LS_RIGHT_REAR = 0x6;
\r
53 public static byte LS_ALL = 0x7;
\r
54 private static int ALL_DATA = 0x1f;
\r
56 private static PWMManager pwmm;
\r
57 private static StrategyMgr strategyMgr;
\r
59 private static int onOffMode = ON_MODE;
\r
60 private static int manualAutonomusMode = AUTONOMUS_MODE;
\r
61 private static byte lineSensorsMask;
\r
62 private static byte sonarSensors;
\r
63 private static byte[] command;
\r
64 private static byte currentCommand;
\r
65 private static Object obj;
\r
66 private static Object obj1;
\r
67 private static byte privSonars;
\r
68 private static byte privLineSensors;
\r
69 private static byte currentSonars;
\r
70 private static byte currentLineSensors;
\r
71 public static String pwmSelection;
\r
74 * Constructor for the class for the robot main thread.
\r
80 * Processes sonar sensor input. This method is called each time new sonar
\r
81 * sensor data arrives, and each time that a mode switch occurs between ON/OFF
\r
82 * and Manual Override.
\r
84 static void processSonars() {
\r
85 strategyMgr.processSonars(sonarSensors);
\r
89 * Processes line sensor input. This method is called each time new line
\r
90 * sensor data arrives, and each time that a mode switch occurs between ON/OFF
\r
91 * and Manual Override.
\r
93 static void processLineSensors() {
\r
94 strategyMgr.processLineSensors(lineSensorsMask);
\r
99 * Resumes motors as per the last sonar command.
\r
101 public static void resume() {
\r
106 * Extracts sonar sensor data from the adjunct sensor controller and from line
\r
107 * sensor data from the JStamp line sensor pins.
\r
109 private static void processIOCommand() {
\r
113 // synchronized (obj1) {
\r
114 data = (int) (currentCommand & ALL_DATA);
\r
115 opCode = (int) (currentCommand & 0xe0); // ALL_COMMANDS);
\r
119 System.out.println("processIOCommand: Default: opCode = " + Integer.toString((int) opCode)
\r
120 + " data = " + Integer.toString((int) data));
\r
122 switch ((int) opCode) {
\r
125 System.out.println("processIO: ON_OFF....");
\r
127 if ((data & 0x1) == 0x1) {
\r
128 System.out.println("processIO: ON MODE........");
\r
129 onOffMode = ON_MODE;
\r
130 processLineSensors();
\r
133 System.out.println("processIO: OFF MODE.......");
\r
134 onOffMode = OFF_MODE;
\r
135 strategyMgr.stop();
\r
138 case MANUAL_AUTONOMUS:
\r
140 System.out.println("processIO: Setting manual_autonomus mode");
\r
142 if ((data & 0x1) == 0x1) {
\r
143 manualAutonomusMode = MANUAL_MODE;
\r
144 pwmm.setManualMode();
\r
146 manualAutonomusMode = AUTONOMUS_MODE;
\r
147 pwmm.setAutomatedMode();
\r
148 processLineSensors();
\r
154 // System.out.println("processIO: Line Sensors.................."
\r
155 // + Integer.toBinaryString((int) (data & LS_ALL)));
\r
157 lineSensorsMask = (byte) (data & LS_ALL);
\r
160 * Line sensors with '0' data means the robot moved off the edge line, and
\r
161 * there is nothing to do.
\r
163 if (((data & LS_ALL) == 0) || ((data & LS_ALL) == privLineSensors)) {
\r
164 privLineSensors = (byte) (data & LS_ALL);
\r
167 if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {
\r
169 System.out.println("processIO: Line Sensors..Process...!!!");
\r
170 processLineSensors();
\r
173 case SONAR_SENSORS:
\r
175 // System.out.println("processIO: SONAR_SENORS: bits = ......"
\r
176 // + Integer.toBinaryString((int) (currentCommand & ALL_SONARS)));
\r
178 currentSonars = (byte) (data & ALL_SONARS);
\r
180 * No need to synchronized on this variable assignment since all referring
\r
181 * code is on the same logical thread
\r
183 sonarSensors = (byte) currentSonars;
\r
184 if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {
\r
189 System.out.println("processIOCommand: Default: opCode = " + Integer.toString((int) opCode)
\r
190 + " data = " + Integer.toString((int) data));
\r
195 * Sets the simulation mode on in the IOManager.
\r
197 // static public void setSimulation() {
\r
198 // sm.setSimulation();
\r
202 * Resets the simulation mode in the IOManager.
\r
204 // static public void resetSimulation() {
\r
205 // sm.resetSimulation();
\r
209 * Saves the current IOManager command byte locally.
\r
211 static public void setIOManagerCommand(byte[] cmd) {
\r
212 // synchronized (obj1) {
\r
213 currentCommand = cmd[0];
\r
215 // synchronized (obj) {
\r
218 } catch (IllegalMonitorStateException ie) {
\r
219 System.out.println("IllegalMonitorStateException caught trying to notify");
\r
225 * Sets debug messaging state: true - Activate debug messages false -
\r
226 * deactivate debug messages
\r
228 static public void setDebug(boolean debug) {
\r
233 * Runs the robot's main thread.
\r
235 public static void main(String args[]) {
\r
237 TestSensorInput.init();
\r
238 boolean active = true;
\r
240 * RealTime management of the robot behaviour based on sensors and commands
\r
245 * Set the low level PWM interface type, e.g. "rtsj" or "native" (ajile
\r
248 pwmSelection = "rtsj";
\r
250 System.out.println("PWM Selction is: " + pwmSelection);
\r
252 pwmm = new PWMManager(pwmSelection);
\r
254 // Pass in the PWMManager for callbacks.
\r
255 // sm = new IOManager(pwmm);
\r
256 // ram = new RemoteApplicationManager(pwmm);
\r
257 strategyMgr = new StrategyMgr(pwmm);
\r
260 * Sets initial values for the speed and agility parameters. Speed and
\r
261 * agility are arbitrary scale factors for the overall speed and speed of
\r
262 * turns, respectively. These work with PWM via the native ajile libraries,
\r
263 * but do not work well with the RTSJ implementation due to limits on the
\r
264 * granularity of timing for the PWM pulse (quantization).
\r
266 pwmm.setSpeedAgilityFactors(100, 100);
\r
269 * Robot's initial state is "ON" by default. Set this parameter to "OFF" if
\r
270 * the robot is to be started over the TCP/IP link.
\r
272 // issueCommand("OFF");
\r
276 while (active && count < 100000) {
\r
279 // System.out.println("Main: Waiting for remote commands");
\r
283 // } catch (IllegalMonitorStateException ie) {
\r
284 // System.out.println("IllegalMonitorStateException caught in main loop");
\r
286 currentCommand = TestSensorInput.getCommand();
\r
287 if (currentCommand == -1) {
\r
290 System.out.println("currentCommand="+currentCommand);
\r
291 processIOCommand();
\r
293 } catch (Exception e) {
\r