Robust/src/Benchmarks/SSJava/JavaNatorInfer/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 \r
  27 \r
  28 public class RobotMain {\r
  29 \r
  30   \r
  31   private static boolean DEBUG1 = true;\r
  32   \r
  33   private static boolean DEBUG = true;\r
  34 \r
  35   public static final int OFF_MODE = 1;\r
  36   public static final int ON_MODE = 2;\r
  37   public static final int MANUAL_MODE = 1;\r
  38   public static final int AUTONOMUS_MODE = 2;\r
  39   public static final int ON_OFF = 128;// 0x80\r
  40   public static final int MANUAL_AUTONOMUS = 32; // 0x20\r
  41   public static final int LINE_SENSORS = 64; // 0x40\r
  42   public 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   public static final int ALL_DATA = 0x1f;\r
  60 \r
  61   \r
  62   private PWMManager pwmm;\r
  63   \r
  64   private StrategyMgr strategyMgr;\r
  65 \r
  66   \r
  67   private int onOffMode = ON_MODE;\r
  68   \r
  69   private int manualAutonomusMode = AUTONOMUS_MODE;\r
  70   \r
  71   private byte lineSensorsMask;\r
  72   \r
  73   private byte sonarSensors;\r
  74   \r
  75   private byte currentCommand;\r
  76   \r
  77   private byte privSonars;\r
  78   \r
  79   private byte privLineSensors;\r
  80   \r
  81   private byte currentSonars;\r
  82   \r
  83   public 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 sensor data arrives,\r
  93    * and each time that a mode switch occurs between ON/OFF and Manual Override.\r
  94    */\r
  95   void processSonars() {\r
  96     strategyMgr.processSonars(sonarSensors);\r
  97   }\r
  98 \r
  99   /**\r
 100    * Processes line sensor input. This method is called each time new line sensor data arrives, and\r
 101    * each time that a mode switch occurs between ON/OFF and Manual Override.\r
 102    */\r
 103   void processLineSensors() {\r
 104     strategyMgr.processLineSensors(lineSensorsMask);\r
 105     resume();\r
 106   }\r
 107 \r
 108   /**\r
 109    * Resumes motors as per the last sonar command.\r
 110    */\r
 111   void resume() {\r
 112     processSonars();\r
 113   }\r
 114 \r
 115   /**\r
 116    * Extracts sonar sensor data from the adjunct sensor controller and from line sensor data from\r
 117    * the JStamp line sensor pins.\r
 118    */\r
 119   \r
 120   private void processIOCommand() {\r
 121 \r
 122     // int data = 0;\r
 123     // int opCode = 0;\r
 124     // synchronized (obj1) {\r
 125      int data = (int) (currentCommand & ALL_DATA);\r
 126      int opCode = (int) (currentCommand & 0xe0); // ALL_COMMANDS);\r
 127     // }\r
 128 \r
 129     if (DEBUG) {\r
 130       System.out.println("processIOCommand: Default: opCode = " + Integer.toString((int) opCode)\r
 131           + " data = " + Integer.toString((int) data));\r
 132     }\r
 133     switch ((int) opCode) {\r
 134     case ON_OFF:\r
 135       if (DEBUG) {\r
 136         System.out.println("processIO: ON_OFF....");\r
 137       }\r
 138       if ((data & 0x1) == 0x1) {\r
 139         System.out.println("processIO: ON MODE........");\r
 140         onOffMode = ON_MODE;\r
 141         processLineSensors();\r
 142         processSonars();\r
 143       } else {\r
 144         System.out.println("processIO: OFF MODE.......");\r
 145         onOffMode = OFF_MODE;\r
 146         strategyMgr.stop();\r
 147       }\r
 148       break;\r
 149     case MANUAL_AUTONOMUS:\r
 150       if (DEBUG) {\r
 151         System.out.println("processIO: Setting manual_autonomus mode");\r
 152       }\r
 153       if ((data & 0x1) == 0x1) {\r
 154         manualAutonomusMode = MANUAL_MODE;\r
 155         pwmm.setManualMode();\r
 156       } else {\r
 157         manualAutonomusMode = AUTONOMUS_MODE;\r
 158         pwmm.setAutomatedMode();\r
 159         processLineSensors();\r
 160         processSonars();\r
 161       }\r
 162       break;\r
 163     case LINE_SENSORS:\r
 164       if (DEBUG) {\r
 165         // System.out.println("processIO: Line Sensors.................."\r
 166         // + Integer.toBinaryString((int) (data & LS_ALL)));\r
 167       }\r
 168       lineSensorsMask = (byte) (data & LS_ALL);\r
 169 \r
 170       /*\r
 171        * Line sensors with '0' data means the robot moved off the edge line, and there is nothing to\r
 172        * do.\r
 173        */\r
 174       if (((data & LS_ALL) == 0) || ((data & LS_ALL) == privLineSensors)) {\r
 175         privLineSensors = (byte) (data & LS_ALL);\r
 176         return;\r
 177       }\r
 178       if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {\r
 179         if (DEBUG)\r
 180           System.out.println("processIO: Line Sensors..Process...!!!");\r
 181         processLineSensors();\r
 182       }\r
 183       break;\r
 184     case SONAR_SENSORS:\r
 185       if (DEBUG) {\r
 186         // System.out.println("processIO: SONAR_SENORS: bits = ......"\r
 187         // + Integer.toBinaryString((int) (currentCommand & ALL_SONARS)));\r
 188       }\r
 189       currentSonars = (byte) (data & ALL_SONARS);\r
 190       /*\r
 191        * No need to synchronized on this variable assignment since all referring code is on the same\r
 192        * logical thread\r
 193        */\r
 194       sonarSensors = (byte) currentSonars;\r
 195       if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {\r
 196         processSonars();\r
 197       }\r
 198       break;\r
 199     default:\r
 200       strategyMgr.stop();\r
 201       System.out.println("processIOCommand: Default: opCode = " + Integer.toString((int) opCode)\r
 202           + " data = " + Integer.toString((int) data));\r
 203     }\r
 204   }\r
 205 \r
 206   /**\r
 207    * Sets the simulation mode on in the IOManager.\r
 208    */\r
 209   // static public void setSimulation() {\r
 210   // sm.setSimulation();\r
 211   // }\r
 212 \r
 213   /**\r
 214    * Resets the simulation mode in the IOManager.\r
 215    */\r
 216   // static public void resetSimulation() {\r
 217   // sm.resetSimulation();\r
 218   // }\r
 219 \r
 220   /**\r
 221    * Saves the current IOManager command byte locally.\r
 222    */\r
 223   public void setIOManagerCommand(byte[] cmd) {\r
 224     // synchronized (obj1) {\r
 225     currentCommand = cmd[0];\r
 226     // }\r
 227     // synchronized (obj) {\r
 228     try {\r
 229       // obj.notify();\r
 230     } catch (IllegalMonitorStateException ie) {\r
 231       System.out.println("IllegalMonitorStateException caught trying to notify");\r
 232     }\r
 233     // }\r
 234   }\r
 235 \r
 236   /**\r
 237    * Sets debug messaging state: true - Activate debug messages false - deactivate debug messages\r
 238    */\r
 239   static public void setDebug(boolean debug) {\r
 240     DEBUG = debug;\r
 241   }\r
 242 \r
 243   /**\r
 244    * Runs the robot's main thread.\r
 245    */\r
 246   public static void main( String args[]) {\r
 247 \r
 248     TestSensorInput.init();\r
 249     RobotMain r = new RobotMain();\r
 250     r.doit();\r
 251   }\r
 252 \r
 253   \r
 254   public void doit() {\r
 255      boolean active = true;\r
 256     /**\r
 257      * RealTime management of the robot behaviour based on sensors and commands input.\r
 258      */\r
 259 \r
 260     /**\r
 261      * Set the low level PWM interface type, e.g. "rtsj" or "native" (ajile 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      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 agility are arbitrary\r
 277      * scale factors for the overall speed and speed of turns, respectively. These work with PWM via\r
 278      * the native ajile libraries, but do not work well with the RTSJ implementation due to limits\r
 279      * on the granularity of timing for the PWM pulse (quantization).\r
 280      */\r
 281     pwmm.setSpeedAgilityFactors(100, 100);\r
 282 \r
 283     /*\r
 284      * Robot's initial state is "ON" by default. Set this parameter to "OFF" if the robot is to be\r
 285      * started over the TCP/IP link.\r
 286      */\r
 287     // issueCommand("OFF");\r
 288 \r
 289     SSJAVA: while (active) {\r
 290 \r
 291       Command com = HWSimulator.getCommand();\r
 292       currentCommand = com.command;\r
 293       sonarSensors = com.sonarSensors;\r
 294       lineSensorsMask = com.lineSensorsMask;\r
 295       manualAutonomusMode = com.manualAutonomusMode;\r
 296       onOffMode = com.onOffMode;\r
 297 \r
 298       if (currentCommand == -1) {\r
 299         break;\r
 300       }\r
 301       System.out.println("currentCommand=" + currentCommand);\r
 302       processIOCommand();\r
 303       // erase current settings\r
 304       initialize();\r
 305     }\r
 306     System.exit(0);\r
 307   }\r
 308 \r
 309   public void initialize() {\r
 310     privLineSensors = 0;\r
 311     currentCommand = 0;\r
 312     privSonars = 0;\r
 313     currentSonars = 0;\r
 314     pwmSelection = "";\r
 315   }\r
 316 \r
 317 }\r