Robust/src/Benchmarks/SSJava/JavaNator/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 @LATTICE("UP<V,V<T,V*")\r
  28 @METHODDEFAULT("OUT<THIS,THIS<V,V<IN,V*,THISLOC=THIS,RETURNLOC=OUT")\r
  29 public class MotorControl {\r
  30   @LOC("T")\r
  31   boolean DEBUG = true;\r
  32   @LOC("UP")\r
  33   int motorLeftUpTime = 150;\r
  34   @LOC("UP")\r
  35   int motorRightUpTime = 150;\r
  36   @LOC("T")\r
  37   int speedFactor;\r
  38   @LOC("T")\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   @RETURNLOC("THIS,MotorControl.T")\r
  49   private int normalizeTime(@LOC("IN") 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(@LOC("IN") 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     @LOC("THIS,MotorControl.V") int timePos = normalizeTime(timePosition);\r
  71 \r
  72     @LOC("THIS,MotorControl.V") 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(@LOC("IN") 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     @LOC("THIS,MotorControl.V") int timePos = normalizeTime(timePosition);\r
 115     @LOC("THIS,MotorControl.V") 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(@LOC("IN") 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     @LOC("THIS,MotorControl.V") int timePosLocal = normalizeTime(timePosition);\r
 153     @LOC("THIS,MotorControl.V") int motorUpTime =\r
 154         (timePosLocal * 100 + ((100 - timePosLocal) * (100 - agilityFactor))) * speedFactor;\r
 155     if (motorUpTime == 0) {\r
 156       motorUpTime = 1;\r
 157       // System.out.println("returning....");\r
 158       // return; // ndr\r
 159     } else if (motorUpTime == 1000000) {\r
 160       motorUpTime--;\r
 161     }\r
 162     // if (DEBUG) {\r
 163     // System.out.println("setSpeedTurnLeft: output-> = " +\r
 164     // Integer.toString(motorUpTime));\r
 165     // }\r
 166     // synchronized (this) {\r
 167     /* Factor in the speed and agility factors */\r
 168     motorLeftUpTime = motorUpTime;\r
 169     // }\r
 170     if (DEBUG) {\r
 171       System.out.println("MotorControl: setSpeedTurnLeft: output-> " + motorLeftUpTime);\r
 172     }\r
 173   }\r
 174 \r
 175   /**\r
 176    * setSpeedTurnM1 - set speed considering agility factor for motor 2\r
 177    * \r
 178    * @param uptime\r
 179    *          pulse width (time position)\r
 180    */\r
 181   public void setSpeedTurnRight(@LOC("IN") int timePosition) {\r
 182     /* Speed comes in as a number between 0 - 100 */\r
 183     /* It represents values between 1 to 2 ms */\r
 184     if (DEBUG) {\r
 185       System.out.println("setSpeedTurnRight: input-> " + Integer.toString(timePosition));\r
 186     }\r
 187     @LOC("THIS,MotorControl.V") int timePos = normalizeTime(timePosition);\r
 188     @LOC("THIS,MotorControl.V") int motorUpTime =\r
 189         ((timePos * 100) + ((100 - timePos) * (100 - agilityFactor))) * speedFactor;\r
 190     if (motorUpTime == 0) {\r
 191       motorUpTime = 1;\r
 192       // System.out.println("returning....");\r
 193       // return; // ndr\r
 194     } else if (motorUpTime == 1000000) {\r
 195       motorUpTime--;\r
 196     }\r
 197 \r
 198     // synchronized (this) {\r
 199     /* Factor in the speed and agility factors */\r
 200     motorRightUpTime = motorUpTime;\r
 201     // }\r
 202     if (DEBUG) {\r
 203       System.out.println("MotorControl: setSpeedTurnRight: output-> " + motorRightUpTime);\r
 204     }\r
 205   }\r
 206 \r
 207   /**\r
 208    * setSpeedLeft - speed control for motor 1.\r
 209    * \r
 210    * @param uptime\r
 211    *          pulse width (time position)\r
 212    */\r
 213   public void setSpeedLeft(@LOC("IN") int timePosition) {\r
 214     /* Speed comes in as a number between 0 - 100 */\r
 215     /* It represents values between 1 to 2 ms */\r
 216     /* 100-input to get reverse polarity for this motor */\r
 217     /* since it's mounted in reverse direction to the other motor */\r
 218     if (DEBUG) {\r
 219       System.out.println("setSpeedLeft: input-> " + Integer.toString(timePosition));\r
 220     }\r
 221     @LOC("THIS,MotorControl.V") int timePos = normalizeTime(timePosition);\r
 222     @LOC("THIS,MotorControl.V") int motorUpTime = (int) (timePos * 100) * speedFactor;\r
 223     /*\r
 224      * System.out.println("motorUpTime = " + Integer.toStri@LOC("IN")\r
 225      * ng(motorUpTime) + " timePos = " + Integer.toString((int)timePos) +\r
 226      * " timePosition = " + Integer.toString((int)timePosition) +\r
 227      * " speedFactor = " + Integer.toString(speedFactor));\r
 228      */\r
 229     if (motorUpTime == 0) {\r
 230       motorUpTime = 1;\r
 231       // System.out.println("returning....");\r
 232       // return; // ndr\r
 233     } else if (motorUpTime == 1000000) {\r
 234       motorUpTime--;\r
 235     }\r
 236 \r
 237     // synchronized (this) {\r
 238     /* Factor in speedFactor */\r
 239     motorLeftUpTime = motorUpTime;\r
 240     // }\r
 241     if (DEBUG) {\r
 242       System.out\r
 243           .println("MotorContol: setSpeedLeft: output-> " + Integer.toString(motorLeftUpTime));\r
 244     }\r
 245   }\r
 246 \r
 247   /**\r
 248    * setSpeedRight - speed control for motor 1.\r
 249    * \r
 250    * @param uptime\r
 251    *          pulse width (time position)\r
 252    */\r
 253   public void setSpeedRight(@LOC("IN") int timePosition) {\r
 254     if (DEBUG) {\r
 255       System.out.println("setSpeedRight: input-> " + Integer.toString(timePosition));\r
 256     }\r
 257     /* Speed comes in as a number between 0 - 100 */\r
 258     /* It represents values between 1 to 2 ms */\r
 259     @LOC("THIS,MotorControl.V") int timePos = normalizeTime(timePosition);\r
 260     @LOC("THIS,MotorControl.V") int motorUpTime = (int) (timePos * 100) * speedFactor;\r
 261     if (motorUpTime == 0) {\r
 262       motorUpTime = 1;\r
 263       // System.out.println("returning....");\r
 264       // return; // ndr\r
 265     } else if (motorUpTime == 1000000) {\r
 266       motorUpTime--;\r
 267     }\r
 268     // synchronized (this) {\r
 269     /* Factor in speedFactor */\r
 270     motorRightUpTime = motorUpTime;\r
 271     // }\r
 272     if (DEBUG) {\r
 273       System.out.println("MotorControl: setSpeedRight: output-> " + motorRightUpTime);\r
 274     }\r
 275   }\r
 276 \r
 277   public void setUrgentReverse() {\r
 278     // synchronized (this) {\r
 279     motorLeftUpTime = 1;\r
 280     motorRightUpTime = 1;\r
 281     // }\r
 282     if (DEBUG) {\r
 283       System.out.println("MotorControl: setUrgentReverse: motorLeftUpTime-> " + motorLeftUpTime);\r
 284       System.out.println("MotorControl: setUrgentReverse: motorRightUpTime-> " + motorRightUpTime);\r
 285     }\r
 286   }\r
 287 \r
 288   public void setUrgentStraight() {\r
 289     // synchronized (this) {\r
 290     motorLeftUpTime = 99;\r
 291     motorRightUpTime = 99;\r
 292     if (DEBUG) {\r
 293       System.out.println("MotorControl: setUrgentStraight: motorLeftUpTime-> " + motorLeftUpTime);\r
 294       System.out.println("MotorControl: setUrgentStraight: motorRightUpTime-> " + motorRightUpTime);\r
 295     }\r
 296     // }\r
 297   }\r
 298 \r
 299   public void justSync() {\r
 300     // synchronized (this) {\r
 301     motorLeftUpTime = motorLeftUpTime;\r
 302     motorRightUpTime = motorRightUpTime;\r
 303     if (DEBUG) {\r
 304       System.out.println("MotorControl: justSync: motorLeftUpTime-> " + motorLeftUpTime);\r
 305       System.out.println("MotorControl: justSync: motorRightUpTime-> " + motorRightUpTime);\r
 306     }\r
 307     // }\r
 308   }\r
 309 \r
 310   /**\r
 311    * Control debug messageing. true - Activate debug messages false - deactivate\r
 312    * debug messages\r
 313    */\r
 314   public void setDebug(boolean debug) {\r
 315     DEBUG = debug;\r
 316   }\r
 317 \r
 318 }\r