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 @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
  29 \r
  30   @LOC("T")\r
  31   private static boolean DEBUG1 = true;\r
  32   @LOC("T")\r
  33   private static boolean DEBUG = true;\r
  34 \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
  44 \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
  60 \r
  61   @LOC("MGR")\r
  62   private static PWMManager pwmm;\r
  63   @LOC("MGR")\r
  64   private static StrategyMgr strategyMgr;\r
  65 \r
  66   @LOC("MASK")\r
  67   private static int onOffMode = ON_MODE;\r
  68   @LOC("MASK")\r
  69   private static int manualAutonomusMode = AUTONOMUS_MODE;\r
  70   @LOC("MASK")\r
  71   private static byte lineSensorsMask;\r
  72   @LOC("MASK")\r
  73   private static byte sonarSensors;\r
  74   @LOC("CCMD")\r
  75   private static byte currentCommand;\r
  76   @LOC("MASK")\r
  77   private static byte privSonars;\r
  78   @LOC("PREV")\r
  79   private static byte privLineSensors;\r
  80   @LOC("MASK")\r
  81   private static byte currentSonars;\r
  82   @LOC("MASK")\r
  83   public static String pwmSelection;\r
  84 \r
  85   /**\r
  86    * Constructor for the class for the robot main thread.\r
  87    */\r
  88   RobotMain() {\r
  89   }\r
  90 \r
  91   /**\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
  95    */\r
  96   static void processSonars() {\r
  97     strategyMgr.processSonars(sonarSensors);\r
  98   }\r
  99 \r
 100   /**\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
 104    */\r
 105   static void processLineSensors() {\r
 106     strategyMgr.processLineSensors(lineSensorsMask);\r
 107     resume();\r
 108   }\r
 109 \r
 110   /**\r
 111    * Resumes motors as per the last sonar command.\r
 112    */\r
 113   public static void resume() {\r
 114     processSonars();\r
 115   }\r
 116 \r
 117   /**\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
 120    */\r
 121   @LATTICE("THIS,THISLOC=THIS,GLOBALLOC=THIS")\r
 122   private static void processIOCommand() {\r
 123 \r
 124     // int data = 0;\r
 125     // int opCode = 0;\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
 129     // }\r
 130 \r
 131     if (DEBUG) {\r
 132       System.out.println("processIOCommand: Default: opCode = " + Integer.toString((int) opCode)\r
 133           + " data = " + Integer.toString((int) data));\r
 134     }\r
 135     switch ((int) opCode) {\r
 136     case ON_OFF:\r
 137       if (DEBUG) {\r
 138         System.out.println("processIO: ON_OFF....");\r
 139       }\r
 140       if ((data & 0x1) == 0x1) {\r
 141         System.out.println("processIO: ON MODE........");\r
 142         onOffMode = ON_MODE;\r
 143         processLineSensors();\r
 144         processSonars();\r
 145       } else {\r
 146         System.out.println("processIO: OFF MODE.......");\r
 147         onOffMode = OFF_MODE;\r
 148         strategyMgr.stop();\r
 149       }\r
 150       break;\r
 151     case MANUAL_AUTONOMUS:\r
 152       if (DEBUG) {\r
 153         System.out.println("processIO: Setting manual_autonomus mode");\r
 154       }\r
 155       if ((data & 0x1) == 0x1) {\r
 156         manualAutonomusMode = MANUAL_MODE;\r
 157         pwmm.setManualMode();\r
 158       } else {\r
 159         manualAutonomusMode = AUTONOMUS_MODE;\r
 160         pwmm.setAutomatedMode();\r
 161         processLineSensors();\r
 162         processSonars();\r
 163       }\r
 164       break;\r
 165     case LINE_SENSORS:\r
 166       if (DEBUG) {\r
 167         // System.out.println("processIO: Line Sensors.................."\r
 168         // + Integer.toBinaryString((int) (data & LS_ALL)));\r
 169       }\r
 170       lineSensorsMask = (byte) (data & LS_ALL);\r
 171 \r
 172       /*\r
 173        * Line sensors with '0' data means the robot moved off the edge line, and\r
 174        * there is nothing to do.\r
 175        */\r
 176       if (((data & LS_ALL) == 0) || ((data & LS_ALL) == privLineSensors)) {\r
 177         privLineSensors = (byte) (data & LS_ALL);\r
 178         return;\r
 179       }\r
 180       if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {\r
 181         if (DEBUG)\r
 182           System.out.println("processIO: Line Sensors..Process...!!!");\r
 183         processLineSensors();\r
 184       }\r
 185       break;\r
 186     case SONAR_SENSORS:\r
 187       if (DEBUG) {\r
 188         // System.out.println("processIO: SONAR_SENORS: bits = ......"\r
 189         // + Integer.toBinaryString((int) (currentCommand & ALL_SONARS)));\r
 190       }\r
 191       currentSonars = (byte) (data & ALL_SONARS);\r
 192       /*\r
 193        * No need to synchronized on this variable assignment since all referring\r
 194        * code is on the same logical thread\r
 195        */\r
 196       sonarSensors = (byte) currentSonars;\r
 197       if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {\r
 198         processSonars();\r
 199       }\r
 200       break;\r
 201     default:\r
 202       strategyMgr.stop();\r
 203       System.out.println("processIOCommand: Default: opCode = " + Integer.toString((int) opCode)\r
 204           + " data = " + Integer.toString((int) data));\r
 205     }\r
 206   }\r
 207 \r
 208   /**\r
 209    * Sets the simulation mode on in the IOManager.\r
 210    */\r
 211   // static public void setSimulation() {\r
 212   // sm.setSimulation();\r
 213   // }\r
 214 \r
 215   /**\r
 216    * Resets the simulation mode in the IOManager.\r
 217    */\r
 218   // static public void resetSimulation() {\r
 219   // sm.resetSimulation();\r
 220   // }\r
 221 \r
 222   /**\r
 223    * Saves the current IOManager command byte locally.\r
 224    */\r
 225   static public void setIOManagerCommand(byte[] cmd) {\r
 226     // synchronized (obj1) {\r
 227     currentCommand = cmd[0];\r
 228     // }\r
 229     // synchronized (obj) {\r
 230     try {\r
 231       // obj.notify();\r
 232     } catch (IllegalMonitorStateException ie) {\r
 233       System.out.println("IllegalMonitorStateException caught trying to notify");\r
 234     }\r
 235     // }\r
 236   }\r
 237 \r
 238   /**\r
 239    * Sets debug messaging state: true - Activate debug messages false -\r
 240    * deactivate debug messages\r
 241    */\r
 242   static public void setDebug(boolean debug) {\r
 243     DEBUG = debug;\r
 244   }\r
 245 \r
 246   /**\r
 247    * Runs the robot's main thread.\r
 248    */\r
 249   @LATTICE("THIS<C,THIS<MC,MC<IN,C<IN,IN<T,C*,THISLOC=THIS,GLOBALLOC=THIS")\r
 250   public static void main(@LOC("IN") String args[]) {\r
 251 \r
 252     TestSensorInput.init();\r
 253     @LOC("T") boolean active = true;\r
 254     /**\r
 255      * RealTime management of the robot behaviour based on sensors and commands\r
 256      * input.\r
 257      */\r
 258 \r
 259     /**\r
 260      * Set the low level PWM interface type, e.g. "rtsj" or "native" (ajile\r
 261      * library-based).\r
 262      */\r
 263     pwmSelection = "rtsj";\r
 264 \r
 265     System.out.println("PWM Selction is: " + pwmSelection);\r
 266 \r
 267     pwmm = new PWMManager(pwmSelection);\r
 268 \r
 269     // Pass in the PWMManager for callbacks.\r
 270     // sm = new IOManager(pwmm);\r
 271     // ram = new RemoteApplicationManager(pwmm);\r
 272     @LOC("MC") MotorControl mc = new MotorControl(100, 100);\r
 273     strategyMgr = new StrategyMgr(mc);\r
 274 \r
 275     /*\r
 276      * Sets initial values for the speed and agility parameters. Speed and\r
 277      * agility are arbitrary scale factors for the overall speed and speed of\r
 278      * turns, respectively. These work with PWM via the native ajile libraries,\r
 279      * but do not work well with the RTSJ implementation due to limits on the\r
 280      * granularity of timing for the PWM pulse (quantization).\r
 281      */\r
 282     pwmm.setSpeedAgilityFactors(100, 100);\r
 283 \r
 284     /*\r
 285      * Robot's initial state is "ON" by default. Set this parameter to "OFF" if\r
 286      * the robot is to be started over the TCP/IP link.\r
 287      */\r
 288     // issueCommand("OFF");\r
 289 \r
 290     SSJAVA: while (active) {\r
 291 \r
 292       // if (DEBUG) {\r
 293       // System.out.println("Main: Waiting for remote commands");\r
 294       // }\r
 295       // try {\r
 296       // obj.wait();\r
 297       // } catch (IllegalMonitorStateException ie) {\r
 298       // System.out.println("IllegalMonitorStateException caught in main loop");\r
 299       // }\r
 300       currentCommand = TestSensorInput.getCommand();\r
 301       if (currentCommand == -1) {\r
 302         break;\r
 303       }\r
 304       System.out.println("currentCommand=" + currentCommand);\r
 305       processIOCommand();\r
 306     }\r
 307     System.exit(0);\r
 308   }\r
 309 \r
 310 }\r