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 @LATTICE("MGR<MASK,PREV<CMD,MASK<CMD,CMD<CCMD,CCMD<T,PREV*,MASK*")
\r
27 @METHODDEFAULT("THIS,THISLOC=THIS,GLOBALLOC=THIS")
\r
28 public class RobotMain {
\r
31 private static boolean DEBUG1 = true;
\r
33 private static boolean DEBUG = true;
\r
35 private static final int OFF_MODE = 1;
\r
36 private static final int ON_MODE = 2;
\r
37 private static final int MANUAL_MODE = 1;
\r
38 private static final int AUTONOMUS_MODE = 2;
\r
39 private static final int ON_OFF = 128;// 0x80
\r
40 private static final int MANUAL_AUTONOMUS = 32; // 0x20
\r
41 public static final int LINE_SENSORS = 64; // 0x40
\r
42 private 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 private static final int ALL_DATA = 0x1f;
\r
62 private static PWMManager pwmm;
\r
64 private static StrategyMgr strategyMgr;
\r
67 private static int onOffMode = ON_MODE;
\r
69 private static int manualAutonomusMode = AUTONOMUS_MODE;
\r
71 private static byte lineSensorsMask;
\r
73 private static byte sonarSensors;
\r
75 private static byte currentCommand;
\r
77 private static byte privSonars;
\r
79 private static byte privLineSensors;
\r
81 private static byte currentSonars;
\r
83 public static 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
\r
93 * sensor data arrives, and each time that a mode switch occurs between ON/OFF
\r
94 * and Manual Override.
\r
96 static void processSonars() {
\r
97 strategyMgr.processSonars(sonarSensors);
\r
101 * Processes line sensor input. This method is called each time new line
\r
102 * sensor data arrives, and each time that a mode switch occurs between ON/OFF
\r
103 * and Manual Override.
\r
105 static void processLineSensors() {
\r
106 strategyMgr.processLineSensors(lineSensorsMask);
\r
111 * Resumes motors as per the last sonar command.
\r
113 public static void resume() {
\r
118 * Extracts sonar sensor data from the adjunct sensor controller and from line
\r
119 * sensor data from the JStamp line sensor pins.
\r
121 @LATTICE("THIS,THISLOC=THIS,GLOBALLOC=THIS")
\r
122 private static void processIOCommand() {
\r
126 // synchronized (obj1) {
\r
127 @LOC("THIS,RobotMain.CMD") int data = (int) (currentCommand & ALL_DATA);
\r
128 @LOC("THIS,RobotMain.CMD") int opCode = (int) (currentCommand & 0xe0); // ALL_COMMANDS);
\r
132 System.out.println("processIOCommand: Default: opCode = " + Integer.toString((int) opCode)
\r
133 + " data = " + Integer.toString((int) data));
\r
135 switch ((int) opCode) {
\r
138 System.out.println("processIO: ON_OFF....");
\r
140 if ((data & 0x1) == 0x1) {
\r
141 System.out.println("processIO: ON MODE........");
\r
142 onOffMode = ON_MODE;
\r
143 processLineSensors();
\r
146 System.out.println("processIO: OFF MODE.......");
\r
147 onOffMode = OFF_MODE;
\r
148 strategyMgr.stop();
\r
151 case MANUAL_AUTONOMUS:
\r
153 System.out.println("processIO: Setting manual_autonomus mode");
\r
155 if ((data & 0x1) == 0x1) {
\r
156 manualAutonomusMode = MANUAL_MODE;
\r
157 pwmm.setManualMode();
\r
159 manualAutonomusMode = AUTONOMUS_MODE;
\r
160 pwmm.setAutomatedMode();
\r
161 processLineSensors();
\r
167 // System.out.println("processIO: Line Sensors.................."
\r
168 // + Integer.toBinaryString((int) (data & LS_ALL)));
\r
170 lineSensorsMask = (byte) (data & LS_ALL);
\r
173 * Line sensors with '0' data means the robot moved off the edge line, and
\r
174 * there is nothing to do.
\r
176 if (((data & LS_ALL) == 0) || ((data & LS_ALL) == privLineSensors)) {
\r
177 privLineSensors = (byte) (data & LS_ALL);
\r
180 if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {
\r
182 System.out.println("processIO: Line Sensors..Process...!!!");
\r
183 processLineSensors();
\r
186 case SONAR_SENSORS:
\r
188 // System.out.println("processIO: SONAR_SENORS: bits = ......"
\r
189 // + Integer.toBinaryString((int) (currentCommand & ALL_SONARS)));
\r
191 currentSonars = (byte) (data & ALL_SONARS);
\r
193 * No need to synchronized on this variable assignment since all referring
\r
194 * code is on the same logical thread
\r
196 sonarSensors = (byte) currentSonars;
\r
197 if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {
\r
202 System.out.println("processIOCommand: Default: opCode = " + Integer.toString((int) opCode)
\r
203 + " data = " + Integer.toString((int) data));
\r
208 * Sets the simulation mode on in the IOManager.
\r
210 // static public void setSimulation() {
\r
211 // sm.setSimulation();
\r
215 * Resets the simulation mode in the IOManager.
\r
217 // static public void resetSimulation() {
\r
218 // sm.resetSimulation();
\r
222 * Saves the current IOManager command byte locally.
\r
224 static public void setIOManagerCommand(byte[] cmd) {
\r
225 // synchronized (obj1) {
\r
226 currentCommand = cmd[0];
\r
228 // synchronized (obj) {
\r
231 } catch (IllegalMonitorStateException ie) {
\r
232 System.out.println("IllegalMonitorStateException caught trying to notify");
\r
238 * Sets debug messaging state: true - Activate debug messages false -
\r
239 * deactivate debug messages
\r
241 static public void setDebug(boolean debug) {
\r
246 * Runs the robot's main thread.
\r
248 @LATTICE("THIS<C,THIS<MC,MC<IN,C<IN,IN<T,C*,THISLOC=THIS,GLOBALLOC=THIS")
\r
249 public static void main(@LOC("IN") String args[]) {
\r
251 TestSensorInput.init();
\r
252 @LOC("T") boolean active = true;
\r
254 * RealTime management of the robot behaviour based on sensors and commands
\r
259 * Set the low level PWM interface type, e.g. "rtsj" or "native" (ajile
\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 @LOC("MC") 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
\r
276 * agility are arbitrary scale factors for the overall speed and speed of
\r
277 * turns, respectively. These work with PWM via the native ajile libraries,
\r
278 * but do not work well with the RTSJ implementation due to limits on the
\r
279 * granularity of timing for the PWM pulse (quantization).
\r
281 pwmm.setSpeedAgilityFactors(100, 100);
\r
284 * Robot's initial state is "ON" by default. Set this parameter to "OFF" if
\r
285 * the robot is to be started over the TCP/IP link.
\r
287 // issueCommand("OFF");
\r
289 @LOC("C") int count = 0;
\r
290 SSJAVA: while (active) {
\r
292 if (count > 100000) {
\r
297 // System.out.println("Main: Waiting for remote commands");
\r
301 // } catch (IllegalMonitorStateException ie) {
\r
302 // System.out.println("IllegalMonitorStateException caught in main loop");
\r
304 currentCommand = TestSensorInput.getCommand();
\r
305 if (currentCommand == -1) {
\r
308 System.out.println("currentCommand=" + currentCommand);
\r
309 processIOCommand();
\r