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 = false;\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-> = " + Integer.toString(motorUpTime));\r
  88     }\r
  89     // synchronized (this) {\r
  90     /* Factor in the speed and agility factors */\r
  91     motorLeftUpTime = motorUpTime;\r
  92     // }\r
  93   }\r
  94 \r
  95   /**\r
  96    * setSpeedSpinRight - Set speed for the spin case right motor.\r
  97    * \r
  98    * @param uptime\r
  99    *          pulse width (time position)\r
 100    */\r
 101   public void setSpeedSpinRight(@LOC("IN") int timePosition) {\r
 102     /* Speed comes in as a number between 0 - 100 */\r
 103     /* It represents values between 1 to 2 ms */\r
 104     /* An input of 50 should result in 0 speed. */\r
 105     /* 100 should result in full speed forward */\r
 106     /* while 0 should result in full speed backwords */\r
 107     if (DEBUG) {\r
 108       System.out.println("setSpeedSpinRight: input-> " + Integer.toString(timePosition));\r
 109     }\r
 110     @LOC("THIS,MotorControl.V") int timePos = normalizeTime(timePosition);\r
 111     @LOC("THIS,MotorControl.V") int motorUpTime = (int) ((timePos) * agilityFactor * speedFactor);\r
 112     // Bug in wait. Cant take 0 nanoseconds\r
 113     if (motorUpTime == 0) {\r
 114       motorUpTime = 1;\r
 115       // System.out.println("returning....");\r
 116       // return; // ndr\r
 117     } else if (motorUpTime == 1000000) {\r
 118       motorUpTime--;\r
 119     }\r
 120 \r
 121     if (DEBUG) {\r
 122       System.out.println("setSpeedSpinRight: output-> = " + Integer.toString(motorUpTime));\r
 123     }\r
 124     // synchronized (this) {\r
 125     /* Factor in the speed and agility factors */\r
 126     motorRightUpTime = motorUpTime;\r
 127     // }\r
 128   }\r
 129 \r
 130   /**\r
 131    * setSpeedTurnM1 - set speed considering agility factor for motor 1\r
 132    * \r
 133    * @param uptime\r
 134    *          pulse width (time position)\r
 135    */\r
 136   public void setSpeedTurnLeft(@LOC("IN") int timePosition) {\r
 137     /* Speed comes in as a number between 0 - 100 */\r
 138     /* It represents values between 1 to 2 ms */\r
 139     /* 100-input to get reverse polarity for this motor */\r
 140     /* since it's mounted in reverse direction to the other motor */\r
 141     if (DEBUG) {\r
 142       System.out.println("setSpeedTurnLeft: input-> " + Integer.toString(timePosition));\r
 143     }\r
 144     @LOC("THIS,MotorControl.V") int timePosLocal = normalizeTime(timePosition);\r
 145     @LOC("THIS,MotorControl.V") int motorUpTime =\r
 146         (timePosLocal * 100 + ((100 - timePosLocal) * (100 - agilityFactor))) * speedFactor;\r
 147     if (motorUpTime == 0) {\r
 148       motorUpTime = 1;\r
 149       // System.out.println("returning....");\r
 150       // return; // ndr\r
 151     } else if (motorUpTime == 1000000) {\r
 152       motorUpTime--;\r
 153     }\r
 154     if (DEBUG) {\r
 155       System.out.println("setSpeedTurnLeft: output-> = " + Integer.toString(motorUpTime));\r
 156     }\r
 157     // synchronized (this) {\r
 158     /* Factor in the speed and agility factors */\r
 159     motorLeftUpTime = motorUpTime;\r
 160     // }\r
 161   }\r
 162 \r
 163   /**\r
 164    * setSpeedTurnM1 - set speed considering agility factor for motor 2\r
 165    * \r
 166    * @param uptime\r
 167    *          pulse width (time position)\r
 168    */\r
 169   public void setSpeedTurnRight(@LOC("IN") int timePosition) {\r
 170     /* Speed comes in as a number between 0 - 100 */\r
 171     /* It represents values between 1 to 2 ms */\r
 172     if (DEBUG) {\r
 173       System.out.println("setSpeedTurnRight: input-> " + Integer.toString(timePosition));\r
 174     }\r
 175     @LOC("THIS,MotorControl.V") int timePos = normalizeTime(timePosition);\r
 176     @LOC("THIS,MotorControl.V") int motorUpTime =\r
 177         ((timePos * 100) + ((100 - timePos) * (100 - agilityFactor))) * speedFactor;\r
 178     if (motorUpTime == 0) {\r
 179       motorUpTime = 1;\r
 180       // System.out.println("returning....");\r
 181       // return; // ndr\r
 182     } else if (motorUpTime == 1000000) {\r
 183       motorUpTime--;\r
 184     }\r
 185 \r
 186     if (DEBUG) {\r
 187       System.out.println("setSpeedTurnRight: output-> " + Integer.toString(motorUpTime));\r
 188     }\r
 189     // synchronized (this) {\r
 190     /* Factor in the speed and agility factors */\r
 191     motorRightUpTime = motorUpTime;\r
 192     // }\r
 193   }\r
 194 \r
 195   /**\r
 196    * setSpeedLeft - speed control for motor 1.\r
 197    * \r
 198    * @param uptime\r
 199    *          pulse width (time position)\r
 200    */\r
 201   public void setSpeedLeft(@LOC("IN") int timePosition) {\r
 202     /* Speed comes in as a number between 0 - 100 */\r
 203     /* It represents values between 1 to 2 ms */\r
 204     /* 100-input to get reverse polarity for this motor */\r
 205     /* since it's mounted in reverse direction to the other motor */\r
 206     if (DEBUG) {\r
 207       System.out.println("setSpeedLeft: input-> " + Integer.toString(timePosition));\r
 208     }\r
 209     @LOC("THIS,MotorControl.V") int timePos = normalizeTime(timePosition);\r
 210     @LOC("THIS,MotorControl.V") int motorUpTime = (int) (timePos * 100) * speedFactor;\r
 211     /*\r
 212      * System.out.println("motorUpTime = " + Integer.toStri@LOC("IN")\r
 213      * ng(motorUpTime) + " timePos = " + Integer.toString((int)timePos) +\r
 214      * " timePosition = " + Integer.toString((int)timePosition) +\r
 215      * " speedFactor = " + Integer.toString(speedFactor));\r
 216      */\r
 217     if (motorUpTime == 0) {\r
 218       motorUpTime = 1;\r
 219       // System.out.println("returning....");\r
 220       // return; // ndr\r
 221     } else if (motorUpTime == 1000000) {\r
 222       motorUpTime--;\r
 223     }\r
 224     if (DEBUG) {\r
 225       System.out.println("setSpeedLeft: output-> " + Integer.toString(motorUpTime));\r
 226     }\r
 227     // synchronized (this) {\r
 228     /* Factor in speedFactor */\r
 229     motorLeftUpTime = motorUpTime;\r
 230     // }\r
 231   }\r
 232 \r
 233   /**\r
 234    * setSpeedRight - speed control for motor 1.\r
 235    * \r
 236    * @param uptime\r
 237    *          pulse width (time position)\r
 238    */\r
 239   public void setSpeedRight(@LOC("IN") int timePosition) {\r
 240     if (DEBUG) {\r
 241       System.out.println("setSpeedRight: input-> " + Integer.toString(timePosition));\r
 242     }\r
 243     /* Speed comes in as a number between 0 - 100 */\r
 244     /* It represents values between 1 to 2 ms */\r
 245     @LOC("THIS,MotorControl.V") int timePos = normalizeTime(timePosition);\r
 246     @LOC("THIS,MotorControl.V") int motorUpTime = (int) (timePos * 100) * speedFactor;\r
 247     if (motorUpTime == 0) {\r
 248       motorUpTime = 1;\r
 249       // System.out.println("returning....");\r
 250       // return; // ndr\r
 251     } else if (motorUpTime == 1000000) {\r
 252       motorUpTime--;\r
 253     }\r
 254     if (DEBUG) {\r
 255       System.out.println("setSpeedRight: output-> " + Integer.toString(motorUpTime));\r
 256     }\r
 257     // synchronized (this) {\r
 258     /* Factor in speedFactor */\r
 259     motorRightUpTime = motorUpTime;\r
 260     // }\r
 261   }\r
 262 \r
 263   public void setUrgentReverse() {\r
 264     // synchronized (this) {\r
 265     motorLeftUpTime = 1;\r
 266     motorRightUpTime = 1;\r
 267     // }\r
 268   }\r
 269 \r
 270   public void setUrgentStraight() {\r
 271     // synchronized (this) {\r
 272     motorLeftUpTime = 99;\r
 273     motorRightUpTime = 99;\r
 274     // }\r
 275   }\r
 276 \r
 277   public void justSync() {\r
 278     // synchronized (this) {\r
 279     motorLeftUpTime = motorLeftUpTime;\r
 280     motorRightUpTime = motorRightUpTime;\r
 281     // }\r
 282   }\r
 283 \r
 284   /**\r
 285    * Control debug messageing. true - Activate debug messages false - deactivate\r
 286    * debug messages\r
 287    */\r
 288   public void setDebug(boolean debug) {\r
 289     DEBUG = debug;\r
 290   }\r
 291 \r
 292 }\r