Robust/src/Benchmarks/SSJava/JavaNator/RobotMain.java

   1 /*\r
   2  * RobotMain.java\r
   3  *\r
   4  * Copyright (c) 1993-2002 Sun Microsystems, Inc. All Rights Reserved.\r
   5  *\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
  10  * with Sun.\r
  11  *\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
  18  */\r
  19 \r
  20 /**\r
  21  * Robot's Main class - instantiates all required classes and resources.\r
  22  * \r
  23  * @author Michael Gesundheit\r
  24  * @version 1.0\r
  25  */\r
  26 public class RobotMain {\r
  27 \r
  28   private static boolean DEBUG1 = true;\r
  29   private static boolean DEBUG = false;\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
  39 \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
  55 \r
  56   private static PWMManager pwmm;\r
  57   private static StrategyMgr strategyMgr;\r
  58 \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
  72 \r
  73   /**\r
  74    * Constructor for the class for the robot main thread.\r
  75    */\r
  76   RobotMain() {\r
  77   }\r
  78 \r
  79   /**\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
  83    */\r
  84   static void processSonars() {\r
  85     strategyMgr.processSonars(sonarSensors);\r
  86   }\r
  87 \r
  88   /**\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
  92    */\r
  93   static void processLineSensors() {\r
  94     strategyMgr.processLineSensors(lineSensorsMask);\r
  95     resume();\r
  96   }\r
  97 \r
  98   /**\r
  99    * Resumes motors as per the last sonar command.\r
 100    */\r
 101   public static void resume() {\r
 102     processSonars();\r
 103   }\r
 104 \r
 105   /**\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
 108    */\r
 109   private static void processIOCommand() {\r
 110 \r
 111     int data = 0;\r
 112     int opCode = 0;\r
 113     // synchronized (obj1) {\r
 114     data = (int) (currentCommand & ALL_DATA);\r
 115     opCode = (int) (currentCommand & 0xe0); // ALL_COMMANDS);\r
 116     // }\r
 117 \r
 118     if (DEBUG) {\r
 119       // System.out.println("processIOCommand: Getting in opCode = "\r
 120       // + Integer.toHexString((int) opCode) + " data = " +\r
 121       // Integer.toHexString((int) data));\r
 122     }\r
 123     switch ((int) opCode) {\r
 124     case ON_OFF:\r
 125       if (DEBUG) {\r
 126         System.out.println("processIO: ON_OFF....");\r
 127       }\r
 128       if ((data & 0x1) == 0x1) {\r
 129         System.out.println("processIO: ON MODE........");\r
 130         onOffMode = ON_MODE;\r
 131         processLineSensors();\r
 132         processSonars();\r
 133       } else {\r
 134         System.out.println("processIO: OFF MODE.......");\r
 135         onOffMode = OFF_MODE;\r
 136         strategyMgr.stop();\r
 137       }\r
 138       break;\r
 139     case MANUAL_AUTONOMUS:\r
 140       if (DEBUG) {\r
 141         System.out.println("processIO: Setting manual_autonomus mode");\r
 142       }\r
 143       if ((data & 0x1) == 0x1) {\r
 144         manualAutonomusMode = MANUAL_MODE;\r
 145         pwmm.setManualMode();\r
 146       } else {\r
 147         manualAutonomusMode = AUTONOMUS_MODE;\r
 148         pwmm.setAutomatedMode();\r
 149         processLineSensors();\r
 150         processSonars();\r
 151       }\r
 152       break;\r
 153     case LINE_SENSORS:\r
 154       if (DEBUG) {\r
 155         // System.out.println("processIO: Line Sensors.................."\r
 156         // + Integer.toBinaryString((int) (data & LS_ALL)));\r
 157       }\r
 158       lineSensorsMask = (byte) (data & LS_ALL);\r
 159 \r
 160       /*\r
 161        * Line sensors with '0' data means the robot moved off the edge line, and\r
 162        * there is nothing to do.\r
 163        */\r
 164       if (((data & LS_ALL) == 0) || ((data & LS_ALL) == privLineSensors)) {\r
 165         privLineSensors = (byte) (data & LS_ALL);\r
 166         return;\r
 167       }\r
 168       if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {\r
 169         if (DEBUG)\r
 170           System.out.println("processIO: Line Sensors..Process...!!!");\r
 171         processLineSensors();\r
 172       }\r
 173       break;\r
 174     case SONAR_SENSORS:\r
 175       if (DEBUG) {\r
 176         // System.out.println("processIO: SONAR_SENORS: bits = ......"\r
 177         // + Integer.toBinaryString((int) (currentCommand & ALL_SONARS)));\r
 178       }\r
 179       currentSonars = (byte) (data & ALL_SONARS);\r
 180       /*\r
 181        * No need to synchronized on this variable assignment since all referring\r
 182        * code is on the same logical thread\r
 183        */\r
 184       sonarSensors = (byte) currentSonars;\r
 185       if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {\r
 186         processSonars();\r
 187       }\r
 188       break;\r
 189     default:\r
 190       System.out.println("processIOCommand: Default: opCode = " + Integer.toString((int) opCode)\r
 191           + " data = " + Integer.toString((int) data));\r
 192     }\r
 193   }\r
 194 \r
 195   /**\r
 196    * Sets the simulation mode on in the IOManager.\r
 197    */\r
 198   // static public void setSimulation() {\r
 199   // sm.setSimulation();\r
 200   // }\r
 201 \r
 202   /**\r
 203    * Resets the simulation mode in the IOManager.\r
 204    */\r
 205   // static public void resetSimulation() {\r
 206   // sm.resetSimulation();\r
 207   // }\r
 208 \r
 209   /**\r
 210    * Saves the current IOManager command byte locally.\r
 211    */\r
 212   static public void setIOManagerCommand(byte[] cmd) {\r
 213     // synchronized (obj1) {\r
 214     currentCommand = cmd[0];\r
 215     // }\r
 216     // synchronized (obj) {\r
 217     try {\r
 218       // obj.notify();\r
 219     } catch (IllegalMonitorStateException ie) {\r
 220       System.out.println("IllegalMonitorStateException caught trying to notify");\r
 221     }\r
 222     // }\r
 223   }\r
 224 \r
 225   /**\r
 226    * Sets debug messaging state: true - Activate debug messages false -\r
 227    * deactivate debug messages\r
 228    */\r
 229   static public void setDebug(boolean debug) {\r
 230     DEBUG = debug;\r
 231   }\r
 232 \r
 233   /**\r
 234    * Runs the robot's main thread.\r
 235    */\r
 236   public static void main(String args[]) {\r
 237 \r
 238     boolean active = true;\r
 239     /**\r
 240      * RealTime management of the robot behaviour based on sensors and commands\r
 241      * input.\r
 242      */\r
 243 \r
 244     /**\r
 245      * Set the low level PWM interface type, e.g. "rtsj" or "native" (ajile\r
 246      * library-based).\r
 247      */\r
 248     pwmSelection = "rtsj";\r
 249 \r
 250     System.out.println("PWM Selction is: " + pwmSelection);\r
 251 \r
 252     pwmm = new PWMManager(pwmSelection);\r
 253 \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
 258 \r
 259     /*\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
 265      */\r
 266     pwmm.setSpeedAgilityFactors(100, 100);\r
 267 \r
 268     /*\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
 271      */\r
 272     // issueCommand("OFF");\r
 273 \r
 274     int count = 0;\r
 275 \r
 276     while (active && count < 10000) {\r
 277       try {\r
 278         if (DEBUG) {\r
 279           System.out.println("Main: Waiting for remote commands");\r
 280         }\r
 281         // try {\r
 282         // obj.wait();\r
 283         // } catch (IllegalMonitorStateException ie) {\r
 284         // System.out.println("IllegalMonitorStateException caught in main loop");\r
 285         // }\r
 286         currentCommand = TestSensorInput.getCommand();\r
 287         processIOCommand();\r
 288         // Nothing to do\r
 289       } catch (Exception e) {\r
 290       }\r
 291       count++;\r
 292     }\r
 293     System.exit(0);\r
 294   }\r
 295 \r
 296 }\r