Robust/src/Benchmarks/SSJava/JavaNatorInfer/PWMRtsj.java

   1 /*\r
   2  * PWMRtsj.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  * This class is motor controller specific. In our case the motor controller\r
  22  * require for 0 speed 1.5 ms out of 10-20ms cycle and a change between 1 to 2\r
  23  * seconds for max reverse and forward speed. These values: 1, 1.5, 2 and 20 ms\r
  24  * are thus this class specific\r
  25  * \r
  26  * @author Michael Gesundheit\r
  27  * @version 1.0\r
  28  */\r
  29 \r
  30 public class PWMRtsj extends PWMControl {\r
  31 \r
  32   // RelativeTime zeroSpeedDuration;\r
  33   // RelativeTime timeUpDuration;\r
  34   // RelativeTime timeDownDuration;\r
  35   // RelativeTime cycleTime;\r
  36   boolean updateTime;\r
  37   int sleepUpDurationNano = 500000;\r
  38   long sleepUpDurationMilli = 1;\r
  39   int sleepDownDurationNano = 500000;\r
  40   long sleepDownDurationMilli = 18;\r
  41   int pulseWidth;\r
  42   // Object obj;\r
  43 \r
  44   private static final int GPIO_A_OER = 0x09002000;\r
  45   private static final int GPIO_A_OUT = GPIO_A_OER + 4;\r
  46   private static final int GPIO_A_IN = GPIO_A_OER + 8;\r
  47 \r
  48   /**\r
  49    * Constructor - Start a standard Java thread. The thread is suspended and\r
  50    * will wake up evey 18 ms. It will issue a 1-2ms pulse and suspends itself\r
  51    * again to 18ms. This is to have a total of 20ms or less yet it's the maxim\r
  52    * possible cycle so as to load the cpu as little as possible.\r
  53    */\r
  54   public PWMRtsj(PWMManager pwmMan, int motor1bit, int motor2bit) {\r
  55 \r
  56     super(pwmMan, motor1bit, motor2bit); // This is papa\r
  57 \r
  58     motorLeftUpTime = 450000; // Nano seconds\r
  59     motorRightUpTime = 450000; // Nano seconds\r
  60 \r
  61     if (DEBUG) {\r
  62       System.out.println("PWMRtsj: About to start RoboThread...");\r
  63     }\r
  64 \r
  65     // t2 = new RoboThread();\r
  66     // t2.start();\r
  67     // t2.setPriority(10);\r
  68   }\r
  69 \r
  70   // A poor's man ajustimg for the 0 speed which found\r
  71   // to be 450000 nano seconds.\r
  72   private int normalizeTime(int timePosition) {\r
  73     if ((timePosition <= 50) && (timePosition >= 44)) {\r
  74       return 45;\r
  75     }\r
  76     return timePosition;\r
  77   }\r
  78 \r
  79   /**\r
  80    * setSpeedSpin - Set speed for the spin case motor 1.\r
  81    * \r
  82    * @param uptime\r
  83    *          pulse width (time position)\r
  84    */\r
  85   public void setSpeedSpinLeft(int timePosition) {\r
  86     /* Speed comes in as a number between 0 - 100 */\r
  87     /* It represents values between 1 to 2 ms */\r
  88     /* 100-input to get reverse polarity for this motor */\r
  89     /* since it's mounted in reverse direction to the other motor */\r
  90     if (DEBUG) {\r
  91       System.out.println("setSpeedSpinLeft: input-> " + Integer.toString(timePosition));\r
  92     }\r
  93     int timePos = normalizeTime(timePosition);\r
  94 \r
  95     int motorUpTime = (int) (timePos * agilityFactor * speedFactor);\r
  96     // System.out.println("Left Motor UpTime1: " +\r
  97     // Integer.toString(motorUpTime));\r
  98     // Since the right motor is hanging in reverse position\r
  99     // the direction should be opposit\r
 100     // Bug in wait. Can't take 0 nanoseconds\r
 101     if (motorUpTime == 0) {\r
 102       motorUpTime = 1;\r
 103       // System.out.println("returning....");\r
 104       // return; // ndr\r
 105     } else if (motorUpTime == 1000000) {\r
 106       motorUpTime--;\r
 107     }\r
 108 \r
 109     if (DEBUG) {\r
 110       System.out.println("setSpeedSpinLeft: output-> = " + Integer.toString(motorUpTime));\r
 111     }\r
 112 //    synchronized (this) {\r
 113       /* Factor in the speed and agility factors */\r
 114       motorLeftUpTime = motorUpTime;\r
 115 //    }\r
 116   }\r
 117 \r
 118   /**\r
 119    * setSpeedSpinRight - Set speed for the spin case right motor.\r
 120    * \r
 121    * @param uptime\r
 122    *          pulse width (time position)\r
 123    */\r
 124   public void setSpeedSpinRight(int timePosition) {\r
 125     /* Speed comes in as a number between 0 - 100 */\r
 126     /* It represents values between 1 to 2 ms */\r
 127     /* An input of 50 should result in 0 speed. */\r
 128     /* 100 should result in full speed forward */\r
 129     /* while 0 should result in full speed backwords */\r
 130     if (DEBUG) {\r
 131       System.out.println("setSpeedSpinRight: input-> " + Integer.toString(timePosition));\r
 132     }\r
 133     int timePos = normalizeTime(timePosition);\r
 134 \r
 135     int motorUpTime = (int) ((timePos) * agilityFactor * speedFactor);\r
 136     // Bug in wait. Cant take 0 nanoseconds\r
 137     if (motorUpTime == 0) {\r
 138       motorUpTime = 1;\r
 139       // System.out.println("returning....");\r
 140       // return; // ndr\r
 141     } else if (motorUpTime == 1000000) {\r
 142       motorUpTime--;\r
 143     }\r
 144 \r
 145     if (DEBUG) {\r
 146       System.out.println("setSpeedSpinRight: output-> = " + Integer.toString(motorUpTime));\r
 147     }\r
 148 //    synchronized (this) {\r
 149       /* Factor in the speed and agility factors */\r
 150       motorRightUpTime = motorUpTime;\r
 151 //    }\r
 152   }\r
 153 \r
 154   /**\r
 155    * setSpeedTurnM1 - set speed considering agility factor for motor 1\r
 156    * \r
 157    * @param uptime\r
 158    *          pulse width (time position)\r
 159    */\r
 160   public void setSpeedTurnLeft(int timePosition) {\r
 161     /* Speed comes in as a number between 0 - 100 */\r
 162     /* It represents values between 1 to 2 ms */\r
 163     /* 100-input to get reverse polarity for this motor */\r
 164     /* since it's mounted in reverse direction to the other motor */\r
 165     if (DEBUG) {\r
 166       System.out.println("setSpeedTurnLeft: input-> " + Integer.toString(timePosition));\r
 167     }\r
 168     int timePosLocal = normalizeTime(timePosition);\r
 169     int motorUpTime =\r
 170         (timePosLocal * 100 + ((100 - timePosLocal) * (100 - agilityFactor))) * speedFactor;\r
 171     if (motorUpTime == 0) {\r
 172       motorUpTime = 1;\r
 173       // System.out.println("returning....");\r
 174       // return; // ndr\r
 175     } else if (motorUpTime == 1000000) {\r
 176       motorUpTime--;\r
 177     }\r
 178     if (DEBUG) {\r
 179       System.out.println("setSpeedTurnLeft: output-> = " + Integer.toString(motorUpTime));\r
 180     }\r
 181 //    synchronized (this) {\r
 182       /* Factor in the speed and agility factors */\r
 183       motorLeftUpTime = motorUpTime;\r
 184 //    }\r
 185   }\r
 186 \r
 187   /**\r
 188    * setSpeedTurnM1 - set speed considering agility factor for motor 2\r
 189    * \r
 190    * @param uptime\r
 191    *          pulse width (time position)\r
 192    */\r
 193   public void setSpeedTurnRight(int timePosition) {\r
 194     /* Speed comes in as a number between 0 - 100 */\r
 195     /* It represents values between 1 to 2 ms */\r
 196     if (DEBUG) {\r
 197       System.out.println("setSpeedTurnRight: input-> " + Integer.toString(timePosition));\r
 198     }\r
 199     int timePos = normalizeTime(timePosition);\r
 200     int motorUpTime = ((timePos * 100) + ((100 - timePos) * (100 - agilityFactor))) * speedFactor;\r
 201     if (motorUpTime == 0) {\r
 202       motorUpTime = 1;\r
 203       // System.out.println("returning....");\r
 204       // return; // ndr\r
 205     } else if (motorUpTime == 1000000) {\r
 206       motorUpTime--;\r
 207     }\r
 208 \r
 209     if (DEBUG) {\r
 210       System.out.println("setSpeedTurnRight: output-> " + Integer.toString(motorUpTime));\r
 211     }\r
 212 //    synchronized (this) {\r
 213       /* Factor in the speed and agility factors */\r
 214       motorRightUpTime = motorUpTime;\r
 215 //    }\r
 216   }\r
 217 \r
 218   /**\r
 219    * setSpeedLeft - speed control for motor 1.\r
 220    * \r
 221    * @param uptime\r
 222    *          pulse width (time position)\r
 223    */\r
 224   public void setSpeedLeft(int timePosition) {\r
 225     /* Speed comes in as a number between 0 - 100 */\r
 226     /* It represents values between 1 to 2 ms */\r
 227     /* 100-input to get reverse polarity for this motor */\r
 228     /* since it's mounted in reverse direction to the other motor */\r
 229     if (DEBUG) {\r
 230       System.out.println("setSpeedLeft: input-> " + Integer.toString(timePosition));\r
 231     }\r
 232     int timePos = normalizeTime(timePosition);\r
 233     int motorUpTime = (int) (timePos * 100) * speedFactor;\r
 234     /*\r
 235      * System.out.println("motorUpTime = " + Integer.toString(motorUpTime) +\r
 236      * " timePos = " + Integer.toString((int)timePos) + " timePosition = " +\r
 237      * Integer.toString((int)timePosition) + " speedFactor = " +\r
 238      * Integer.toString(speedFactor));\r
 239      */\r
 240     if (motorUpTime == 0) {\r
 241       motorUpTime = 1;\r
 242       // System.out.println("returning....");\r
 243       // return; // ndr\r
 244     } else if (motorUpTime == 1000000) {\r
 245       motorUpTime--;\r
 246     }\r
 247     if (DEBUG) {\r
 248       System.out.println("setSpeedLeft: output-> " + Integer.toString(motorUpTime));\r
 249     }\r
 250 //    synchronized (this) {\r
 251       /* Factor in speedFactor */\r
 252       motorLeftUpTime = motorUpTime;\r
 253 //    }\r
 254   }\r
 255 \r
 256   /**\r
 257    * setSpeedRight - speed control for motor 1.\r
 258    * \r
 259    * @param uptime\r
 260    *          pulse width (time position)\r
 261    */\r
 262   public void setSpeedRight(int timePosition) {\r
 263     if (DEBUG) {\r
 264       System.out.println("setSpeedRight: input-> " + Integer.toString(timePosition));\r
 265     }\r
 266     /* Speed comes in as a number between 0 - 100 */\r
 267     /* It represents values between 1 to 2 ms */\r
 268     int timePos = normalizeTime(timePosition);\r
 269     int motorUpTime = (int) (timePos * 100) * speedFactor;\r
 270     if (motorUpTime == 0) {\r
 271       motorUpTime = 1;\r
 272       // System.out.println("returning....");\r
 273       // return; // ndr\r
 274     } else if (motorUpTime == 1000000) {\r
 275       motorUpTime--;\r
 276     }\r
 277     if (DEBUG) {\r
 278       System.out.println("setSpeedRight: output-> " + Integer.toString(motorUpTime));\r
 279     }\r
 280 //    synchronized (this) {\r
 281       /* Factor in speedFactor */\r
 282       motorRightUpTime = motorUpTime;\r
 283 //    }\r
 284   }\r
 285 \r
 286   public void setSpeedAgilityFactors(int speed, int agility) {\r
 287 //    synchronized (this) {\r
 288       speedFactor = speed;\r
 289       agilityFactor = agility;\r
 290 //    }\r
 291   }\r
 292 \r
 293   public void setUrgentReverse() {\r
 294 //    synchronized (this) {\r
 295       motorLeftUpTime = 1;\r
 296       motorRightUpTime = 1;\r
 297 //    }\r
 298   }\r
 299 \r
 300   public void setUrgentStraight() {\r
 301 //    synchronized (this) {\r
 302       motorLeftUpTime = 99;\r
 303       motorRightUpTime = 99;\r
 304 //    }\r
 305   }\r
 306 \r
 307   public void justSync() {\r
 308 //    synchronized (this) {\r
 309       motorLeftUpTime = motorLeftUpTime;\r
 310       motorRightUpTime = motorRightUpTime;\r
 311 //    }\r
 312   }\r
 313 \r
 314   /**\r
 315    * Control debug messageing. true - Activate debug messages false - deactivate\r
 316    * debug messages\r
 317    */\r
 318   public void setDebug(boolean debug) {\r
 319     DEBUG = debug;\r
 320   }\r
 321 \r
 322 }\r