--- /dev/null
+/*\r
+ * RobotMain.java\r
+ *\r
+ * Copyright (c) 1993-2002 Sun Microsystems, Inc. All Rights Reserved.\r
+ *\r
+ * This software is the confidential and proprietary information of Sun\r
+ * Microsystems, Inc. ("Confidential Information"). You shall not\r
+ * disclose such Confidential Information and shall use it only in\r
+ * accordance with the terms of the license agreement you entered into\r
+ * with Sun.\r
+ *\r
+ * SUN MAKES NO REPRESENTATIONS OR WARRANTIES ABOUT THE SUITABILITY OF\r
+ * THE SOFTWARE, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED\r
+ * TO THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A\r
+ * PARTICULAR PURPOSE, OR NON-INFRINGEMENT. SUN SHALL NOT BE LIABLE FOR\r
+ * ANY DAMAGES SUFFERED BY LICENSEE AS A RESULT OF USING, MODIFYING OR\r
+ * DISTRIBUTING THIS SOFTWARE OR ITS DERIVATIVES.\r
+ */\r
+\r
+/**\r
+ * Robot's Main class - instantiates all required classes and resources.\r
+ * \r
+ * @author Michael Gesundheit\r
+ * @version 1.0\r
+ */\r
+public class RobotMain {\r
+\r
+ private static boolean DEBUG1 = true;\r
+ private static boolean DEBUG = false;\r
+ private static final int OFF_MODE = 1;\r
+ private static final int ON_MODE = 2;\r
+ private static final int MANUAL_MODE = 1;\r
+ private static final int AUTONOMUS_MODE = 2;\r
+ private static final int ON_OFF = 128;// 0x80\r
+ private static final int MANUAL_AUTONOMUS = 32; // 0x20\r
+ public static final int LINE_SENSORS = 64; // 0x40\r
+ private static final int SONAR_SENSORS = 96; // 0x60\r
+ public static final int ALL_COMMANDS = 0xe0;\r
+\r
+ public static byte LF_FRONT = 0x1;\r
+ public static byte RT_FRONT = 0x2;\r
+ public static byte RT_SIDE = 0x4;\r
+ public static byte BK_SIDE = 0x8;\r
+ public static byte LF_SIDE = 0x10;\r
+ public static byte ALL_SONARS = 0x1f;\r
+ public static byte LF_RT_FRONT = 0x3; // LF_FRONT | RT_FRONT;\r
+ public static byte LS_LEFT = 0x1;\r
+ public static byte LS_RIGHT = 0x2;\r
+ public static byte LS_REAR = 0x4;\r
+ public static byte LS_LEFT_RIGHT = 0x3;\r
+ public static byte LS_LEFT_REAR = 0x5;\r
+ public static byte LS_RIGHT_REAR = 0x6;\r
+ public static byte LS_ALL = 0x7;\r
+ private static int ALL_DATA = 0x1f;\r
+\r
+ private static PWMManager pwmm;\r
+ private static StrategyMgr strategyMgr;\r
+\r
+ private static int onOffMode = ON_MODE;\r
+ private static int manualAutonomusMode = AUTONOMUS_MODE;\r
+ private static byte lineSensorsMask;\r
+ private static byte sonarSensors;\r
+ private static byte[] command;\r
+ private static byte currentCommand;\r
+ private static Object obj;\r
+ private static Object obj1;\r
+ private static byte privSonars;\r
+ private static byte privLineSensors;\r
+ private static byte currentSonars;\r
+ private static byte currentLineSensors;\r
+ public static String pwmSelection;\r
+\r
+ /**\r
+ * Constructor for the class for the robot main thread.\r
+ */\r
+ RobotMain() {\r
+ }\r
+\r
+ /**\r
+ * Processes sonar sensor input. This method is called each time new sonar\r
+ * sensor data arrives, and each time that a mode switch occurs between ON/OFF\r
+ * and Manual Override.\r
+ */\r
+ static void processSonars() {\r
+ strategyMgr.processSonars(sonarSensors);\r
+ }\r
+\r
+ /**\r
+ * Processes line sensor input. This method is called each time new line\r
+ * sensor data arrives, and each time that a mode switch occurs between ON/OFF\r
+ * and Manual Override.\r
+ */\r
+ static void processLineSensors() {\r
+ strategyMgr.processLineSensors(lineSensorsMask);\r
+ resume();\r
+ }\r
+\r
+ /**\r
+ * Resumes motors as per the last sonar command.\r
+ */\r
+ public static void resume() {\r
+ processSonars();\r
+ }\r
+\r
+ /**\r
+ * Extracts sonar sensor data from the adjunct sensor controller and from line\r
+ * sensor data from the JStamp line sensor pins.\r
+ */\r
+ private static void processIOCommand() {\r
+\r
+ int data = 0;\r
+ int opCode = 0;\r
+ // synchronized (obj1) {\r
+ data = (int) (currentCommand & ALL_DATA);\r
+ opCode = (int) (currentCommand & 0xe0); // ALL_COMMANDS);\r
+ // }\r
+\r
+ if (DEBUG) {\r
+ // System.out.println("processIOCommand: Getting in opCode = "\r
+ // + Integer.toHexString((int) opCode) + " data = " +\r
+ // Integer.toHexString((int) data));\r
+ }\r
+ switch ((int) opCode) {\r
+ case ON_OFF:\r
+ if (DEBUG) {\r
+ System.out.println("processIO: ON_OFF....");\r
+ }\r
+ if ((data & 0x1) == 0x1) {\r
+ System.out.println("processIO: ON MODE........");\r
+ onOffMode = ON_MODE;\r
+ processLineSensors();\r
+ processSonars();\r
+ } else {\r
+ System.out.println("processIO: OFF MODE.......");\r
+ onOffMode = OFF_MODE;\r
+ strategyMgr.stop();\r
+ }\r
+ break;\r
+ case MANUAL_AUTONOMUS:\r
+ if (DEBUG) {\r
+ System.out.println("processIO: Setting manual_autonomus mode");\r
+ }\r
+ if ((data & 0x1) == 0x1) {\r
+ manualAutonomusMode = MANUAL_MODE;\r
+ pwmm.setManualMode();\r
+ } else {\r
+ manualAutonomusMode = AUTONOMUS_MODE;\r
+ pwmm.setAutomatedMode();\r
+ processLineSensors();\r
+ processSonars();\r
+ }\r
+ break;\r
+ case LINE_SENSORS:\r
+ if (DEBUG) {\r
+ // System.out.println("processIO: Line Sensors.................."\r
+ // + Integer.toBinaryString((int) (data & LS_ALL)));\r
+ }\r
+ lineSensorsMask = (byte) (data & LS_ALL);\r
+\r
+ /*\r
+ * Line sensors with '0' data means the robot moved off the edge line, and\r
+ * there is nothing to do.\r
+ */\r
+ if (((data & LS_ALL) == 0) || ((data & LS_ALL) == privLineSensors)) {\r
+ privLineSensors = (byte) (data & LS_ALL);\r
+ return;\r
+ }\r
+ if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {\r
+ if (DEBUG)\r
+ System.out.println("processIO: Line Sensors..Process...!!!");\r
+ processLineSensors();\r
+ }\r
+ break;\r
+ case SONAR_SENSORS:\r
+ if (DEBUG) {\r
+ // System.out.println("processIO: SONAR_SENORS: bits = ......"\r
+ // + Integer.toBinaryString((int) (currentCommand & ALL_SONARS)));\r
+ }\r
+ currentSonars = (byte) (data & ALL_SONARS);\r
+ /*\r
+ * No need to synchronized on this variable assignment since all referring\r
+ * code is on the same logical thread\r
+ */\r
+ sonarSensors = (byte) currentSonars;\r
+ if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {\r
+ processSonars();\r
+ }\r
+ break;\r
+ default:\r
+ System.out.println("processIOCommand: Default: opCode = " + Integer.toString((int) opCode)\r
+ + " data = " + Integer.toString((int) data));\r
+ }\r
+ }\r
+\r
+ /**\r
+ * Sets the simulation mode on in the IOManager.\r
+ */\r
+ // static public void setSimulation() {\r
+ // sm.setSimulation();\r
+ // }\r
+\r
+ /**\r
+ * Resets the simulation mode in the IOManager.\r
+ */\r
+ // static public void resetSimulation() {\r
+ // sm.resetSimulation();\r
+ // }\r
+\r
+ /**\r
+ * Saves the current IOManager command byte locally.\r
+ */\r
+ static public void setIOManagerCommand(byte[] cmd) {\r
+ // synchronized (obj1) {\r
+ currentCommand = cmd[0];\r
+ // }\r
+ // synchronized (obj) {\r
+ try {\r
+ // obj.notify();\r
+ } catch (IllegalMonitorStateException ie) {\r
+ System.out.println("IllegalMonitorStateException caught trying to notify");\r
+ }\r
+ // }\r
+ }\r
+\r
+ /**\r
+ * Sets debug messaging state: true - Activate debug messages false -\r
+ * deactivate debug messages\r
+ */\r
+ static public void setDebug(boolean debug) {\r
+ DEBUG = debug;\r
+ }\r
+\r
+ /**\r
+ * Runs the robot's main thread.\r
+ */\r
+ public static void main(String args[]) {\r
+\r
+ boolean active = true;\r
+ /**\r
+ * RealTime management of the robot behaviour based on sensors and commands\r
+ * input.\r
+ */\r
+\r
+ /**\r
+ * Set the low level PWM interface type, e.g. "rtsj" or "native" (ajile\r
+ * library-based).\r
+ */\r
+ pwmSelection = "rtsj";\r
+\r
+ System.out.println("PWM Selction is: " + pwmSelection);\r
+\r
+ pwmm = new PWMManager(pwmSelection);\r
+\r
+ // Pass in the PWMManager for callbacks.\r
+ // sm = new IOManager(pwmm);\r
+ // ram = new RemoteApplicationManager(pwmm);\r
+ strategyMgr = new StrategyMgr(pwmm);\r
+\r
+ /*\r
+ * Sets initial values for the speed and agility parameters. Speed and\r
+ * agility are arbitrary scale factors for the overall speed and speed of\r
+ * turns, respectively. These work with PWM via the native ajile libraries,\r
+ * but do not work well with the RTSJ implementation due to limits on the\r
+ * granularity of timing for the PWM pulse (quantization).\r
+ */\r
+ pwmm.setSpeedAgilityFactors(100, 100);\r
+\r
+ /*\r
+ * Robot's initial state is "ON" by default. Set this parameter to "OFF" if\r
+ * the robot is to be started over the TCP/IP link.\r
+ */\r
+ // issueCommand("OFF");\r
+\r
+ int count = 0;\r
+\r
+ while (active && count < 10000) {\r
+ try {\r
+ if (DEBUG) {\r
+ System.out.println("Main: Waiting for remote commands");\r
+ }\r
+ // try {\r
+ // obj.wait();\r
+ // } catch (IllegalMonitorStateException ie) {\r
+ // System.out.println("IllegalMonitorStateException caught in main loop");\r
+ // }\r
+ currentCommand = TestSensorInput.getCommand();\r
+ processIOCommand();\r
+ // Nothing to do\r
+ } catch (Exception e) {\r
+ }\r
+ count++;\r
+ }\r
+ System.exit(0);\r
+ }\r
+\r
+}\r
projects / IRC.git / blobdiff