Robust/src/Benchmarks/SSJava/JavaNatorInfer/MotorControl.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 a wrapper for a PWMControl introduced by porting to SSJava. It\r
  22  * maintains two key fields motorLeftUpTime and motorRightUpTime and expects\r
  23  * that the control thread who is running from the other side gets the current\r
  24  * settings.\r
  25  */\r
  26 \r
  27 \r
  28 \r
  29 public class MotorControl {\r
  30   \r
  31   boolean DEBUG = true;\r
  32   \r
  33   int motorLeftUpTime = 150;\r
  34   \r
  35   int motorRightUpTime = 150;\r
  36   \r
  37   int speedFactor;\r
  38   \r
  39   int agilityFactor;\r
  40 \r
  41   public MotorControl(int speedFactor, int agilityFactor) {\r
  42     this.speedFactor = speedFactor;\r
  43     this.agilityFactor = agilityFactor;\r
  44   }\r
  45 \r
  46   // A poor's man ajustimg for the 0 speed which found\r
  47   // to be 450000 nano seconds.\r
  48   \r
  49   private int normalizeTime( int timePosition) {\r
  50     if ((timePosition <= 50) && (timePosition >= 44)) {\r
  51       return 45;\r
  52     }\r
  53     return timePosition;\r
  54   }\r
  55 \r
  56   /**\r
  57    * setSpeedSpin - Set speed for the spin case motor 1.\r
  58    * \r
  59    * @param uptime\r
  60    *          pulse width (time position)\r
  61    */\r
  62   public void setSpeedSpinLeft( int timePosition) {\r
  63     /* Speed comes in as a number between 0 - 100 */\r
  64     /* It represents values between 1 to 2 ms */\r
  65     /* 100-input to get reverse polarity for this motor */\r
  66     /* since it's mounted in reverse direction to the other motor */\r
  67     if (DEBUG) {\r
  68       System.out.println("setSpeedSpinLeft: input-> " + Integer.toString(timePosition));\r
  69     }\r
  70      int timePos = normalizeTime(timePosition);\r
  71 \r
  72      int motorUpTime = (int) (timePos * agilityFactor * speedFactor);\r
  73     // System.out.println("Left Motor UpTime1: " +\r
  74     // Integer.toString(motorUpTime));\r
  75     // Since the right motor is hanging in reverse position\r
  76     // the direction should be opposit\r
  77     // Bug in wait. Can't take 0 nanoseconds\r
  78     if (motorUpTime == 0) {\r
  79       motorUpTime = 1;\r
  80       // System.out.println("returning....");\r
  81       // return; // ndr\r
  82     } else if (motorUpTime == 1000000) {\r
  83       motorUpTime--;\r
  84     }\r
  85 \r
  86     // if (DEBUG) {\r
  87     // System.out.println("setSpeedSpinLeft: output-> = " +\r
  88     // Integer.toString(motorUpTime));\r
  89     // }\r
  90     // synchronized (this) {\r
  91     /* Factor in the speed and agility factors */\r
  92     motorLeftUpTime = motorUpTime;\r
  93     // }\r
  94     if (DEBUG) {\r
  95       System.out.println("MotorControl: setSpeedSpinLeft: output-> " + motorLeftUpTime);\r
  96     }\r
  97   }\r
  98 \r
  99   /**\r
 100    * setSpeedSpinRight - Set speed for the spin case right motor.\r
 101    * \r
 102    * @param uptime\r
 103    *          pulse width (time position)\r
 104    */\r
 105   public void setSpeedSpinRight( int timePosition) {\r
 106     /* Speed comes in as a number between 0 - 100 */\r
 107     /* It represents values between 1 to 2 ms */\r
 108     /* An input of 50 should result in 0 speed. */\r
 109     /* 100 should result in full speed forward */\r
 110     /* while 0 should result in full speed backwords */\r
 111     if (DEBUG) {\r
 112       System.out.println("setSpeedSpinRight: input-> " + Integer.toString(timePosition));\r
 113     }\r
 114      int timePos = normalizeTime(timePosition);\r
 115      int motorUpTime = (int) ((timePos) * agilityFactor * speedFactor);\r
 116     // Bug in wait. Cant take 0 nanoseconds\r
 117     if (motorUpTime == 0) {\r
 118       motorUpTime = 1;\r
 119       // System.out.println("returning....");\r
 120       // return; // ndr\r
 121     } else if (motorUpTime == 1000000) {\r
 122       motorUpTime--;\r
 123     }\r
 124 \r
 125     // if (DEBUG) {\r
 126     // System.out.println("setSpeedSpinRight: output-> = " +\r
 127     // Integer.toString(motorUpTime));\r
 128     // }\r
 129     // synchronized (this) {\r
 130     /* Factor in the speed and agility factors */\r
 131     motorRightUpTime = motorUpTime;\r
 132     // }\r
 133     if (DEBUG) {\r
 134       System.out.println("MotorControl: setSpeedSpinRight: output-> " + motorRightUpTime);\r
 135     }\r
 136   }\r
 137 \r
 138   /**\r
 139    * setSpeedTurnM1 - set speed considering agility factor for motor 1\r
 140    * \r
 141    * @param uptime\r
 142    *          pulse width (time position)\r
 143    */\r
 144   public void setSpeedTurnLeft( int timePosition) {\r
 145     /* Speed comes in as a number between 0 - 100 */\r
 146     /* It represents values between 1 to 2 ms */\r
 147     /* 100-input to get reverse polarity for this motor */\r
 148     /* since it's mounted in reverse direction to the other motor */\r
 149     if (DEBUG) {\r
 150       System.out.println("setSpeedTurnLeft: input-> " + Integer.toString(timePosition));\r
 151     }\r
 152      int timePosLocal = normalizeTime(timePosition);\r
 153      int motorUpTime = (timePosLocal * 100 + ((100 - timePosLocal) * (100 - agilityFactor))) * speedFactor;\r
 154     if (motorUpTime == 0) {\r
 155       motorUpTime = 1;\r
 156       // System.out.println("returning....");\r
 157       // return; // ndr\r
 158     } else if (motorUpTime == 1000000) {\r
 159       motorUpTime--;\r
 160     }\r
 161     // if (DEBUG) {\r
 162     // System.out.println("setSpeedTurnLeft: output-> = " +\r
 163     // Integer.toString(motorUpTime));\r
 164     // }\r
 165     // synchronized (this) {\r
 166     /* Factor in the speed and agility factors */\r
 167     motorLeftUpTime = motorUpTime;\r
 168     // }\r
 169     if (DEBUG) {\r
 170       System.out.println("MotorControl: setSpeedTurnLeft: output-> " + motorLeftUpTime);\r
 171     }\r
 172   }\r
 173 \r
 174   /**\r
 175    * setSpeedTurnM1 - set speed considering agility factor for motor 2\r
 176    * \r
 177    * @param uptime\r
 178    *          pulse width (time position)\r
 179    */\r
 180   public void setSpeedTurnRight( int timePosition) {\r
 181     /* Speed comes in as a number between 0 - 100 */\r
 182     /* It represents values between 1 to 2 ms */\r
 183     if (DEBUG) {\r
 184       System.out.println("setSpeedTurnRight: input-> " + Integer.toString(timePosition));\r
 185     }\r
 186      int timePos = normalizeTime(timePosition);\r
 187      int motorUpTime = ((timePos * 100) + ((100 - timePos) * (100 - agilityFactor))) * speedFactor;\r
 188     if (motorUpTime == 0) {\r
 189       motorUpTime = 1;\r
 190       // System.out.println("returning....");\r
 191       // return; // ndr\r
 192     } else if (motorUpTime == 1000000) {\r
 193       motorUpTime--;\r
 194     }\r
 195 \r
 196     // synchronized (this) {\r
 197     /* Factor in the speed and agility factors */\r
 198     motorRightUpTime = motorUpTime;\r
 199     // }\r
 200     if (DEBUG) {\r
 201       System.out.println("MotorControl: setSpeedTurnRight: output-> " + motorRightUpTime);\r
 202     }\r
 203   }\r
 204 \r
 205   /**\r
 206    * setSpeedLeft - speed control for motor 1.\r
 207    * \r
 208    * @param uptime\r
 209    *          pulse width (time position)\r
 210    */\r
 211   public void setSpeedLeft( int timePosition) {\r
 212     /* Speed comes in as a number between 0 - 100 */\r
 213     /* It represents values between 1 to 2 ms */\r
 214     /* 100-input to get reverse polarity for this motor */\r
 215     /* since it's mounted in reverse direction to the other motor */\r
 216     if (DEBUG) {\r
 217       System.out.println("setSpeedLeft: input-> " + Integer.toString(timePosition));\r
 218     }\r
 219      int timePos = normalizeTime(timePosition);\r
 220      int motorUpTime = (int) (timePos * 100) * speedFactor;\r
 221     /*\r
 222      * System.out.println("motorUpTime = " + Integer.toStri\r
 223      * ng(motorUpTime) + " timePos = " + Integer.toString((int)timePos) +\r
 224      * " timePosition = " + Integer.toString((int)timePosition) +\r
 225      * " speedFactor = " + Integer.toString(speedFactor));\r
 226      */\r
 227     if (motorUpTime == 0) {\r
 228       motorUpTime = 1;\r
 229       // System.out.println("returning....");\r
 230       // return; // ndr\r
 231     } else if (motorUpTime == 1000000) {\r
 232       motorUpTime--;\r
 233     }\r
 234 \r
 235     // synchronized (this) {\r
 236     /* Factor in speedFactor */\r
 237     motorLeftUpTime = motorUpTime;\r
 238     // }\r
 239     if (DEBUG) {\r
 240       System.out\r
 241           .println("MotorContol: setSpeedLeft: output-> " + Integer.toString(motorLeftUpTime));\r
 242     }\r
 243   }\r
 244 \r
 245   /**\r
 246    * setSpeedRight - speed control for motor 1.\r
 247    * \r
 248    * @param uptime\r
 249    *          pulse width (time position)\r
 250    */\r
 251   public void setSpeedRight( int timePosition) {\r
 252     if (DEBUG) {\r
 253       System.out.println("setSpeedRight: input-> " + Integer.toString(timePosition));\r
 254     }\r
 255     /* Speed comes in as a number between 0 - 100 */\r
 256     /* It represents values between 1 to 2 ms */\r
 257      int timePos = normalizeTime(timePosition);\r
 258      int motorUpTime = (int) (timePos * 100) * speedFactor;\r
 259     if (motorUpTime == 0) {\r
 260       motorUpTime = 1;\r
 261       // System.out.println("returning....");\r
 262       // return; // ndr\r
 263     } else if (motorUpTime == 1000000) {\r
 264       motorUpTime--;\r
 265     }\r
 266     // synchronized (this) {\r
 267     /* Factor in speedFactor */\r
 268     motorRightUpTime = motorUpTime;\r
 269     // }\r
 270     if (DEBUG) {\r
 271       System.out.println("MotorControl: setSpeedRight: output-> " + motorRightUpTime);\r
 272     }\r
 273   }\r
 274 \r
 275   public void setUrgentReverse() {\r
 276     // synchronized (this) {\r
 277     motorLeftUpTime = 1;\r
 278     motorRightUpTime = 1;\r
 279     // }\r
 280     if (DEBUG) {\r
 281       System.out.println("MotorControl: setUrgentReverse: motorLeftUpTime-> " + motorLeftUpTime);\r
 282       System.out.println("MotorControl: setUrgentReverse: motorRightUpTime-> " + motorRightUpTime);\r
 283     }\r
 284   }\r
 285 \r
 286   public void setUrgentStraight() {\r
 287     // synchronized (this) {\r
 288     motorLeftUpTime = 99;\r
 289     motorRightUpTime = 99;\r
 290     if (DEBUG) {\r
 291       System.out.println("MotorControl: setUrgentStraight: motorLeftUpTime-> " + motorLeftUpTime);\r
 292       System.out.println("MotorControl: setUrgentStraight: motorRightUpTime-> " + motorRightUpTime);\r
 293     }\r
 294     // }\r
 295   }\r
 296 \r
 297   public void justSync() {\r
 298     // synchronized (this) {\r
 299     motorLeftUpTime = motorLeftUpTime;\r
 300     motorRightUpTime = motorRightUpTime;\r
 301     if (DEBUG) {\r
 302       System.out.println("MotorControl: justSync: motorLeftUpTime-> " + motorLeftUpTime);\r
 303       System.out.println("MotorControl: justSync: motorRightUpTime-> " + motorRightUpTime);\r
 304     }\r
 305     // }\r
 306   }\r
 307 \r
 308   /**\r
 309    * Control debug messageing. true - Activate debug messages false - deactivate\r
 310    * debug messages\r
 311    */\r
 312   public void setDebug(boolean debug) {\r
 313     DEBUG = debug;\r
 314   }\r
 315 \r
 316 }\r