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 = 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
  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: Default: opCode = " + Integer.toString((int) opCode)\r
 120           + " data = " + Integer.toString((int) data));\r
 121     }\r
 122     switch ((int) opCode) {\r
 123     case ON_OFF:\r
 124       if (DEBUG) {\r
 125         System.out.println("processIO: ON_OFF....");\r
 126       }\r
 127       if ((data & 0x1) == 0x1) {\r
 128         System.out.println("processIO: ON MODE........");\r
 129         onOffMode = ON_MODE;\r
 130         processLineSensors();\r
 131         processSonars();\r
 132       } else {\r
 133         System.out.println("processIO: OFF MODE.......");\r
 134         onOffMode = OFF_MODE;\r
 135         strategyMgr.stop();\r
 136       }\r
 137       break;\r
 138     case MANUAL_AUTONOMUS:\r
 139       if (DEBUG) {\r
 140         System.out.println("processIO: Setting manual_autonomus mode");\r
 141       }\r
 142       if ((data & 0x1) == 0x1) {\r
 143         manualAutonomusMode = MANUAL_MODE;\r
 144         pwmm.setManualMode();\r
 145       } else {\r
 146         manualAutonomusMode = AUTONOMUS_MODE;\r
 147         pwmm.setAutomatedMode();\r
 148         processLineSensors();\r
 149         processSonars();\r
 150       }\r
 151       break;\r
 152     case LINE_SENSORS:\r
 153       if (DEBUG) {\r
 154         // System.out.println("processIO: Line Sensors.................."\r
 155         // + Integer.toBinaryString((int) (data & LS_ALL)));\r
 156       }\r
 157       lineSensorsMask = (byte) (data & LS_ALL);\r
 158 \r
 159       /*\r
 160        * Line sensors with '0' data means the robot moved off the edge line, and\r
 161        * there is nothing to do.\r
 162        */\r
 163       if (((data & LS_ALL) == 0) || ((data & LS_ALL) == privLineSensors)) {\r
 164         privLineSensors = (byte) (data & LS_ALL);\r
 165         return;\r
 166       }\r
 167       if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {\r
 168         if (DEBUG)\r
 169           System.out.println("processIO: Line Sensors..Process...!!!");\r
 170         processLineSensors();\r
 171       }\r
 172       break;\r
 173     case SONAR_SENSORS:\r
 174       if (DEBUG) {\r
 175         // System.out.println("processIO: SONAR_SENORS: bits = ......"\r
 176         // + Integer.toBinaryString((int) (currentCommand & ALL_SONARS)));\r
 177       }\r
 178       currentSonars = (byte) (data & ALL_SONARS);\r
 179       /*\r
 180        * No need to synchronized on this variable assignment since all referring\r
 181        * code is on the same logical thread\r
 182        */\r
 183       sonarSensors = (byte) currentSonars;\r
 184       if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {\r
 185         processSonars();\r
 186       }\r
 187       break;\r
 188     default:\r
 189       System.out.println("processIOCommand: Default: opCode = " + Integer.toString((int) opCode)\r
 190           + " data = " + Integer.toString((int) data));\r
 191     }\r
 192   }\r
 193 \r
 194   /**\r
 195    * Sets the simulation mode on in the IOManager.\r
 196    */\r
 197   // static public void setSimulation() {\r
 198   // sm.setSimulation();\r
 199   // }\r
 200 \r
 201   /**\r
 202    * Resets the simulation mode in the IOManager.\r
 203    */\r
 204   // static public void resetSimulation() {\r
 205   // sm.resetSimulation();\r
 206   // }\r
 207 \r
 208   /**\r
 209    * Saves the current IOManager command byte locally.\r
 210    */\r
 211   static public void setIOManagerCommand(byte[] cmd) {\r
 212     // synchronized (obj1) {\r
 213     currentCommand = cmd[0];\r
 214     // }\r
 215     // synchronized (obj) {\r
 216     try {\r
 217       // obj.notify();\r
 218     } catch (IllegalMonitorStateException ie) {\r
 219       System.out.println("IllegalMonitorStateException caught trying to notify");\r
 220     }\r
 221     // }\r
 222   }\r
 223 \r
 224   /**\r
 225    * Sets debug messaging state: true - Activate debug messages false -\r
 226    * deactivate debug messages\r
 227    */\r
 228   static public void setDebug(boolean debug) {\r
 229     DEBUG = debug;\r
 230   }\r
 231 \r
 232   /**\r
 233    * Runs the robot's main thread.\r
 234    */\r
 235   public static void main(String args[]) {\r
 236 \r
 237     TestSensorInput.init();\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 < 100000) {\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         if (currentCommand == -1) {\r
 288           break;\r
 289         }\r
 290         System.out.println("currentCommand="+currentCommand);\r
 291         processIOCommand();\r
 292         // Nothing to do\r
 293       } catch (Exception e) {\r
 294       }\r
 295       count++;\r
 296     }\r
 297     System.exit(0);\r
 298   }\r
 299 \r
 300 }\r