--- /dev/null
+/*\r
+ * PWMRtsj.java\r
+ *\r
+ * Copyright (c) 1993-2002 Sun Microsystems, Inc. All Rights Reserved.\r
+ *\r
+ * This software is the confidential and proprietary information of Sun\r
+ * Microsystems, Inc. ("Confidential Information"). You shall not\r
+ * disclose such Confidential Information and shall use it only in\r
+ * accordance with the terms of the license agreement you entered into\r
+ * with Sun.\r
+ *\r
+ * SUN MAKES NO REPRESENTATIONS OR WARRANTIES ABOUT THE SUITABILITY OF\r
+ * THE SOFTWARE, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED\r
+ * TO THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A\r
+ * PARTICULAR PURPOSE, OR NON-INFRINGEMENT. SUN SHALL NOT BE LIABLE FOR\r
+ * ANY DAMAGES SUFFERED BY LICENSEE AS A RESULT OF USING, MODIFYING OR\r
+ * DISTRIBUTING THIS SOFTWARE OR ITS DERIVATIVES.\r
+ */\r
+\r
+/**\r
+ * This class is a wrapper for a PWMControl introduced by porting to SSJava. It\r
+ * maintains two key fields motorLeftUpTime and motorRightUpTime and expects\r
+ * that the control thread who is running from the other side gets the current\r
+ * settings.\r
+ */\r
+\r
+@LATTICE("UP<V,V<T,V*")\r
+@METHODDEFAULT("OUT<THIS,THIS<V,V<IN,V*,THISLOC=THIS,RETURNLOC=OUT")\r
+public class MotorControl {\r
+ @LOC("T")\r
+ boolean DEBUG = false;\r
+ @LOC("UP")\r
+ int motorLeftUpTime = 150;\r
+ @LOC("UP")\r
+ int motorRightUpTime = 150;\r
+ @LOC("T")\r
+ int speedFactor;\r
+ @LOC("T")\r
+ int agilityFactor;\r
+\r
+ public MotorControl(int speedFactor, int agilityFactor) {\r
+ this.speedFactor = speedFactor;\r
+ this.agilityFactor = agilityFactor;\r
+ }\r
+\r
+ // A poor's man ajustimg for the 0 speed which found\r
+ // to be 450000 nano seconds.\r
+ @RETURNLOC("THIS,MotorControl.T")\r
+ private int normalizeTime(@LOC("IN") int timePosition) {\r
+ if ((timePosition <= 50) && (timePosition >= 44)) {\r
+ return 45;\r
+ }\r
+ return timePosition;\r
+ }\r
+\r
+ /**\r
+ * setSpeedSpin - Set speed for the spin case motor 1.\r
+ * \r
+ * @param uptime\r
+ * pulse width (time position)\r
+ */\r
+ public void setSpeedSpinLeft(@LOC("IN") int timePosition) {\r
+ /* Speed comes in as a number between 0 - 100 */\r
+ /* It represents values between 1 to 2 ms */\r
+ /* 100-input to get reverse polarity for this motor */\r
+ /* since it's mounted in reverse direction to the other motor */\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedSpinLeft: input-> " + Integer.toString(timePosition));\r
+ }\r
+ @LOC("THIS,MotorControl.V") int timePos = normalizeTime(timePosition);\r
+\r
+ @LOC("THIS,MotorControl.V") int motorUpTime = (int) (timePos * agilityFactor * speedFactor);\r
+ // System.out.println("Left Motor UpTime1: " +\r
+ // Integer.toString(motorUpTime));\r
+ // Since the right motor is hanging in reverse position\r
+ // the direction should be opposit\r
+ // Bug in wait. Can't take 0 nanoseconds\r
+ if (motorUpTime == 0) {\r
+ motorUpTime = 1;\r
+ // System.out.println("returning....");\r
+ // return; // ndr\r
+ } else if (motorUpTime == 1000000) {\r
+ motorUpTime--;\r
+ }\r
+\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedSpinLeft: output-> = " + Integer.toString(motorUpTime));\r
+ }\r
+ // synchronized (this) {\r
+ /* Factor in the speed and agility factors */\r
+ motorLeftUpTime = motorUpTime;\r
+ // }\r
+ }\r
+\r
+ /**\r
+ * setSpeedSpinRight - Set speed for the spin case right motor.\r
+ * \r
+ * @param uptime\r
+ * pulse width (time position)\r
+ */\r
+ public void setSpeedSpinRight(@LOC("IN") int timePosition) {\r
+ /* Speed comes in as a number between 0 - 100 */\r
+ /* It represents values between 1 to 2 ms */\r
+ /* An input of 50 should result in 0 speed. */\r
+ /* 100 should result in full speed forward */\r
+ /* while 0 should result in full speed backwords */\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedSpinRight: input-> " + Integer.toString(timePosition));\r
+ }\r
+ @LOC("THIS,MotorControl.V") int timePos = normalizeTime(timePosition);\r
+ @LOC("THIS,MotorControl.V") int motorUpTime = (int) ((timePos) * agilityFactor * speedFactor);\r
+ // Bug in wait. Cant take 0 nanoseconds\r
+ if (motorUpTime == 0) {\r
+ motorUpTime = 1;\r
+ // System.out.println("returning....");\r
+ // return; // ndr\r
+ } else if (motorUpTime == 1000000) {\r
+ motorUpTime--;\r
+ }\r
+\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedSpinRight: output-> = " + Integer.toString(motorUpTime));\r
+ }\r
+ // synchronized (this) {\r
+ /* Factor in the speed and agility factors */\r
+ motorRightUpTime = motorUpTime;\r
+ // }\r
+ }\r
+\r
+ /**\r
+ * setSpeedTurnM1 - set speed considering agility factor for motor 1\r
+ * \r
+ * @param uptime\r
+ * pulse width (time position)\r
+ */\r
+ public void setSpeedTurnLeft(@LOC("IN") int timePosition) {\r
+ /* Speed comes in as a number between 0 - 100 */\r
+ /* It represents values between 1 to 2 ms */\r
+ /* 100-input to get reverse polarity for this motor */\r
+ /* since it's mounted in reverse direction to the other motor */\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedTurnLeft: input-> " + Integer.toString(timePosition));\r
+ }\r
+ @LOC("THIS,MotorControl.V") int timePosLocal = normalizeTime(timePosition);\r
+ @LOC("THIS,MotorControl.V") int motorUpTime =\r
+ (timePosLocal * 100 + ((100 - timePosLocal) * (100 - agilityFactor))) * speedFactor;\r
+ if (motorUpTime == 0) {\r
+ motorUpTime = 1;\r
+ // System.out.println("returning....");\r
+ // return; // ndr\r
+ } else if (motorUpTime == 1000000) {\r
+ motorUpTime--;\r
+ }\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedTurnLeft: output-> = " + Integer.toString(motorUpTime));\r
+ }\r
+ // synchronized (this) {\r
+ /* Factor in the speed and agility factors */\r
+ motorLeftUpTime = motorUpTime;\r
+ // }\r
+ }\r
+\r
+ /**\r
+ * setSpeedTurnM1 - set speed considering agility factor for motor 2\r
+ * \r
+ * @param uptime\r
+ * pulse width (time position)\r
+ */\r
+ public void setSpeedTurnRight(@LOC("IN") int timePosition) {\r
+ /* Speed comes in as a number between 0 - 100 */\r
+ /* It represents values between 1 to 2 ms */\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedTurnRight: input-> " + Integer.toString(timePosition));\r
+ }\r
+ @LOC("THIS,MotorControl.V") int timePos = normalizeTime(timePosition);\r
+ @LOC("THIS,MotorControl.V") int motorUpTime =\r
+ ((timePos * 100) + ((100 - timePos) * (100 - agilityFactor))) * speedFactor;\r
+ if (motorUpTime == 0) {\r
+ motorUpTime = 1;\r
+ // System.out.println("returning....");\r
+ // return; // ndr\r
+ } else if (motorUpTime == 1000000) {\r
+ motorUpTime--;\r
+ }\r
+\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedTurnRight: output-> " + Integer.toString(motorUpTime));\r
+ }\r
+ // synchronized (this) {\r
+ /* Factor in the speed and agility factors */\r
+ motorRightUpTime = motorUpTime;\r
+ // }\r
+ }\r
+\r
+ /**\r
+ * setSpeedLeft - speed control for motor 1.\r
+ * \r
+ * @param uptime\r
+ * pulse width (time position)\r
+ */\r
+ public void setSpeedLeft(@LOC("IN") int timePosition) {\r
+ /* Speed comes in as a number between 0 - 100 */\r
+ /* It represents values between 1 to 2 ms */\r
+ /* 100-input to get reverse polarity for this motor */\r
+ /* since it's mounted in reverse direction to the other motor */\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedLeft: input-> " + Integer.toString(timePosition));\r
+ }\r
+ @LOC("THIS,MotorControl.V") int timePos = normalizeTime(timePosition);\r
+ @LOC("THIS,MotorControl.V") int motorUpTime = (int) (timePos * 100) * speedFactor;\r
+ /*\r
+ * System.out.println("motorUpTime = " + Integer.toStri@LOC("IN")\r
+ * ng(motorUpTime) + " timePos = " + Integer.toString((int)timePos) +\r
+ * " timePosition = " + Integer.toString((int)timePosition) +\r
+ * " speedFactor = " + Integer.toString(speedFactor));\r
+ */\r
+ if (motorUpTime == 0) {\r
+ motorUpTime = 1;\r
+ // System.out.println("returning....");\r
+ // return; // ndr\r
+ } else if (motorUpTime == 1000000) {\r
+ motorUpTime--;\r
+ }\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedLeft: output-> " + Integer.toString(motorUpTime));\r
+ }\r
+ // synchronized (this) {\r
+ /* Factor in speedFactor */\r
+ motorLeftUpTime = motorUpTime;\r
+ // }\r
+ }\r
+\r
+ /**\r
+ * setSpeedRight - speed control for motor 1.\r
+ * \r
+ * @param uptime\r
+ * pulse width (time position)\r
+ */\r
+ public void setSpeedRight(@LOC("IN") int timePosition) {\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedRight: input-> " + Integer.toString(timePosition));\r
+ }\r
+ /* Speed comes in as a number between 0 - 100 */\r
+ /* It represents values between 1 to 2 ms */\r
+ @LOC("THIS,MotorControl.V") int timePos = normalizeTime(timePosition);\r
+ @LOC("THIS,MotorControl.V") int motorUpTime = (int) (timePos * 100) * speedFactor;\r
+ if (motorUpTime == 0) {\r
+ motorUpTime = 1;\r
+ // System.out.println("returning....");\r
+ // return; // ndr\r
+ } else if (motorUpTime == 1000000) {\r
+ motorUpTime--;\r
+ }\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedRight: output-> " + Integer.toString(motorUpTime));\r
+ }\r
+ // synchronized (this) {\r
+ /* Factor in speedFactor */\r
+ motorRightUpTime = motorUpTime;\r
+ // }\r
+ }\r
+\r
+ public void setUrgentReverse() {\r
+ // synchronized (this) {\r
+ motorLeftUpTime = 1;\r
+ motorRightUpTime = 1;\r
+ // }\r
+ }\r
+\r
+ public void setUrgentStraight() {\r
+ // synchronized (this) {\r
+ motorLeftUpTime = 99;\r
+ motorRightUpTime = 99;\r
+ // }\r
+ }\r
+\r
+ public void justSync() {\r
+ // synchronized (this) {\r
+ motorLeftUpTime = motorLeftUpTime;\r
+ motorRightUpTime = motorRightUpTime;\r
+ // }\r
+ }\r
+\r
+ /**\r
+ * Control debug messageing. true - Activate debug messages false - deactivate\r
+ * debug messages\r
+ */\r
+ public void setDebug(boolean debug) {\r
+ DEBUG = debug;\r
+ }\r
+\r
+}\r
+++ /dev/null
-
-public class MotorController {
-
-}
* @version 1.0\r
*/\r
\r
+@LATTICE("V")\r
+@METHODDEFAULT("OUT<THIS,THIS<V,V<IN,V*,THISLOC=THIS,RETURNLOC=OUT")\r
public class PWMManager {\r
\r
- private int GPIO_A_OER = 0x09002000;\r
- private int GPIO_A_OUT = GPIO_A_OER + 4;\r
- private int GPIO_E_OER = 0x09002400;\r
- private int GPIO_E_OUT = 0x09002404;\r
- private byte MOTOR_PORTID_6 = 6; // Bit 2 (...3,2,1,0)\r
- private byte MOTOR_PORTID_7 = 7; // Bit 3 (...3,2,1,0)\r
+ // private int GPIO_A_OER = 0x09002000;\r
+ // private int GPIO_A_OUT = GPIO_A_OER + 4;\r
+ // private int GPIO_E_OER = 0x09002400;\r
+ // private int GPIO_E_OUT = 0x09002404;\r
+ // private byte MOTOR_PORTID_6 = 6; // Bit 2 (...3,2,1,0)\r
+ // private byte MOTOR_PORTID_7 = 7; // Bit 3 (...3,2,1,0)\r
+ @LOC("V")\r
private int currentRegMask;\r
+ @LOC("V")\r
private boolean DEBUG = false;\r
- private int currentSpeedL;\r
- private int currentSpeedR;\r
- private int currentSpeed;\r
- private PWMControl pwmControl;\r
- private PWMNative pwmNative;\r
- private PWMRtsj pwmRtsj;\r
+ // private PWMControl pwmControl;\r
+ @LOC("V")\r
private RCBridge rcb;\r
+ @LOC("V")\r
private int speedFactor;\r
+ @LOC("V")\r
private int agilityFactor;\r
+ @LOC("V")\r
private int zeroSpeed = 45;\r
\r
/**\r
* Instantiate PWM Makers one for each motor. motorPortId is 1 to 8 and is\r
* corresponding to 8 bits in a byte.\r
*/\r
- if (pwmSelection.equals("rtsj"))\r
- pwmControl = new PWMRtsj(this, MOTOR_PORTID_6, MOTOR_PORTID_7);\r
- else {\r
- System.out.println("Selection PWMNative is activated");\r
- pwmControl = new PWMNative(this, MOTOR_PORTID_6, MOTOR_PORTID_7);\r
- System.out.println("Selection PWMNative is activated.... Return");\r
- }\r
+ // if (pwmSelection.equals("rtsj"))\r
+ // pwmControl = new PWMRtsj(this, MOTOR_PORTID_6, MOTOR_PORTID_7);\r
+ // else {\r
+ // System.out.println("Selection PWMNative is activated");\r
+ // pwmControl = new PWMNative(this, MOTOR_PORTID_6, MOTOR_PORTID_7);\r
+ // System.out.println("Selection PWMNative is activated.... Return");\r
+ // }\r
\r
// rcb = new RCBridge(this);\r
rcb = new RCBridge();\r
public void setManualMode() {\r
if (DEBUG)\r
System.out.println("PWMManager: setManualMode....");\r
- pwmControl.setManualMode();\r
+ // pwmControl.setManualMode();\r
rcb.setManualMode();\r
}\r
\r
public void setAutomatedMode() {\r
if (DEBUG)\r
System.out.println("PWMManager: setAutomatedMode....");\r
- pwmControl.setAutomatedMode();\r
+ // pwmControl.setAutomatedMode();\r
rcb.setAutomatedMode();\r
}\r
\r
- public PWMControl getPWMControl() {\r
- if (DEBUG)\r
- System.out.println("PWMManager: getPWMControl....");\r
- return pwmControl;\r
- }\r
+ // public PWMControl getPWMControl() {\r
+ // if (DEBUG)\r
+ // System.out.println("PWMManager: getPWMControl....");\r
+ // return pwmControl;\r
+ // }\r
\r
public synchronized void writeToPort(int myBit, int myValue) {\r
currentRegMask &= ~myBit; // e.g. 0x11110111\r
if (DEBUG)\r
System.out.println("PWMManager: setSpeedFactor....");\r
this.speedFactor = speedFactor;\r
- pwmControl.setSpeedAgilityFactors(speedFactor, agilityFactor);\r
+ // pwmControl.setSpeedAgilityFactors(speedFactor, agilityFactor);\r
}\r
\r
/**\r
if (DEBUG)\r
System.out.println("PWMManager: setAgilityFactor....");\r
this.agilityFactor = agilityFactor;\r
- pwmControl.setSpeedAgilityFactors(speedFactor, agilityFactor);\r
+ // pwmControl.setSpeedAgilityFactors(speedFactor, agilityFactor);\r
}\r
\r
/**\r
* @param speed\r
* @param agility\r
*/\r
- public synchronized void setSpeedAgilityFactors(int speed, int agility) {\r
+ public synchronized void setSpeedAgilityFactors(@LOC("IN") int speed, @LOC("IN") int agility) {\r
if (DEBUG)\r
System.out.println("PWMManager: setSpeedAgilityFactors....");\r
speedFactor = speed;\r
agilityFactor = agility;\r
- pwmControl.setSpeedAgilityFactors(speedFactor, agilityFactor);\r
+ // pwmControl.setSpeedAgilityFactors(speedFactor, agilityFactor);\r
}\r
\r
/**\r
* @author Michael Gesundheit
* @version 1.0
*/
+@METHODDEFAULT("IN")
public class RCBridge {
- public void setManualMode(){
-// rcLleft.activate();
-// rcLright.activate();
-}
+ public void setManualMode() {
+ // rcLleft.activate();
+ // rcLright.activate();
+ }
-public void setAutomatedMode(){
+ public void setAutomatedMode() {
// rcLleft.deActivate();
// rcLright.deActivate();
-}
+ }
}
\ No newline at end of file
* @author Michael Gesundheit\r
* @version 1.0\r
*/\r
+@LATTICE("MGR<MASK,PREV<CMD,MASK<CMD,CMD<CCMD,CCMD<T,PREV*,MASK*")\r
+@METHODDEFAULT("THIS,THISLOC=THIS,GLOBALLOC=THIS")\r
public class RobotMain {\r
\r
+ @LOC("T")\r
private static boolean DEBUG1 = true;\r
+ @LOC("T")\r
private static boolean DEBUG = true;\r
+\r
private static final int OFF_MODE = 1;\r
private static final int ON_MODE = 2;\r
private static final int MANUAL_MODE = 1;\r
private static final int SONAR_SENSORS = 96; // 0x60\r
public static final int ALL_COMMANDS = 0xe0;\r
\r
- public static byte LF_FRONT = 0x1;\r
- public static byte RT_FRONT = 0x2;\r
- public static byte RT_SIDE = 0x4;\r
- public static byte BK_SIDE = 0x8;\r
- public static byte LF_SIDE = 0x10;\r
- public static byte ALL_SONARS = 0x1f;\r
- public static byte LF_RT_FRONT = 0x3; // LF_FRONT | RT_FRONT;\r
- public static byte LS_LEFT = 0x1;\r
- public static byte LS_RIGHT = 0x2;\r
- public static byte LS_REAR = 0x4;\r
- public static byte LS_LEFT_RIGHT = 0x3;\r
- public static byte LS_LEFT_REAR = 0x5;\r
- public static byte LS_RIGHT_REAR = 0x6;\r
- public static byte LS_ALL = 0x7;\r
- private static int ALL_DATA = 0x1f;\r
+ public static final byte LF_FRONT = 0x1;\r
+ public static final byte RT_FRONT = 0x2;\r
+ public static final byte RT_SIDE = 0x4;\r
+ public static final byte BK_SIDE = 0x8;\r
+ public static final byte LF_SIDE = 0x10;\r
+ public static final byte ALL_SONARS = 0x1f;\r
+ public static final byte LF_RT_FRONT = 0x3; // LF_FRONT | RT_FRONT;\r
+ public static final byte LS_LEFT = 0x1;\r
+ public static final byte LS_RIGHT = 0x2;\r
+ public static final byte LS_REAR = 0x4;\r
+ public static final byte LS_LEFT_RIGHT = 0x3;\r
+ public static final byte LS_LEFT_REAR = 0x5;\r
+ public static final byte LS_RIGHT_REAR = 0x6;\r
+ public static final byte LS_ALL = 0x7;\r
+ private static final int ALL_DATA = 0x1f;\r
\r
+ @LOC("MGR")\r
private static PWMManager pwmm;\r
+ @LOC("MGR")\r
private static StrategyMgr strategyMgr;\r
\r
+ @LOC("MASK")\r
private static int onOffMode = ON_MODE;\r
+ @LOC("MASK")\r
private static int manualAutonomusMode = AUTONOMUS_MODE;\r
+ @LOC("MASK")\r
private static byte lineSensorsMask;\r
+ @LOC("MASK")\r
private static byte sonarSensors;\r
- private static byte[] command;\r
+ @LOC("CCMD")\r
private static byte currentCommand;\r
- private static Object obj;\r
- private static Object obj1;\r
+ @LOC("MASK")\r
private static byte privSonars;\r
+ @LOC("PREV")\r
private static byte privLineSensors;\r
+ @LOC("MASK")\r
private static byte currentSonars;\r
- private static byte currentLineSensors;\r
+ @LOC("MASK")\r
public static String pwmSelection;\r
\r
/**\r
* Extracts sonar sensor data from the adjunct sensor controller and from line\r
* sensor data from the JStamp line sensor pins.\r
*/\r
+ @LATTICE("THIS,THISLOC=THIS,GLOBALLOC=THIS")\r
private static void processIOCommand() {\r
\r
- int data = 0;\r
- int opCode = 0;\r
+ // int data = 0;\r
+ // int opCode = 0;\r
// synchronized (obj1) {\r
- data = (int) (currentCommand & ALL_DATA);\r
- opCode = (int) (currentCommand & 0xe0); // ALL_COMMANDS);\r
+ @LOC("THIS,RobotMain.CMD") int data = (int) (currentCommand & ALL_DATA);\r
+ @LOC("THIS,RobotMain.CMD") int opCode = (int) (currentCommand & 0xe0); // ALL_COMMANDS);\r
// }\r
\r
if (DEBUG) {\r
/**\r
* Runs the robot's main thread.\r
*/\r
- public static void main(String args[]) {\r
+ @LATTICE("THIS<C,THIS<MC,MC<IN,C<IN,IN<T,C*,THISLOC=THIS,GLOBALLOC=THIS")\r
+ public static void main(@LOC("IN") String args[]) {\r
\r
TestSensorInput.init();\r
- boolean active = true;\r
+ @LOC("T") boolean active = true;\r
/**\r
* RealTime management of the robot behaviour based on sensors and commands\r
* input.\r
// Pass in the PWMManager for callbacks.\r
// sm = new IOManager(pwmm);\r
// ram = new RemoteApplicationManager(pwmm);\r
- strategyMgr = new StrategyMgr(pwmm);\r
+ @LOC("MC") MotorControl mc = new MotorControl(100, 100);\r
+ strategyMgr = new StrategyMgr(mc);\r
\r
/*\r
* Sets initial values for the speed and agility parameters. Speed and\r
*/\r
// issueCommand("OFF");\r
\r
- int count = 0;\r
+ @LOC("C") int count = 0;\r
+ SSJAVA: while (active) {\r
+\r
+ if (count > 100000) {\r
+ break;\r
+ }\r
\r
- while (active && count < 100000) {\r
- try {\r
- // if (DEBUG) {\r
- // System.out.println("Main: Waiting for remote commands");\r
- // }\r
- // try {\r
- // obj.wait();\r
- // } catch (IllegalMonitorStateException ie) {\r
- // System.out.println("IllegalMonitorStateException caught in main loop");\r
- // }\r
- currentCommand = TestSensorInput.getCommand();\r
- if (currentCommand == -1) {\r
- break;\r
- }\r
- System.out.println("currentCommand="+currentCommand);\r
- processIOCommand();\r
- // Nothing to do\r
- } catch (Exception e) {\r
+ // if (DEBUG) {\r
+ // System.out.println("Main: Waiting for remote commands");\r
+ // }\r
+ // try {\r
+ // obj.wait();\r
+ // } catch (IllegalMonitorStateException ie) {\r
+ // System.out.println("IllegalMonitorStateException caught in main loop");\r
+ // }\r
+ currentCommand = TestSensorInput.getCommand();\r
+ if (currentCommand == -1) {\r
+ break;\r
}\r
+ System.out.println("currentCommand=" + currentCommand);\r
+ processIOCommand();\r
count++;\r
}\r
System.exit(0);\r
* @author Michael Gesundheit\r
* @version 1.0\r
*/\r
+@LATTICE("C<V,V<T,V*")\r
+@METHODDEFAULT("THIS<IN,IN*,THISLOC=THIS,GLOBALLOC=THIS")\r
public class StrategyMgr {\r
\r
- private PWMControl pwmControl;\r
- private int zeroSpeed = 45;\r
+ @LOC("C")\r
+ private MotorControl mc;\r
+ private static final int zeroSpeed = 45;\r
+ @LOC("T")\r
private Random rand;\r
+ @LOC("T")\r
private boolean DEBUGL = true;\r
// private boolean DEBUGL = false;\r
\r
// private boolean DEBUG = true;\r
+ @LOC("T")\r
private boolean DEBUG = true;\r
\r
/**\r
* Constructor - Invoke communication to remote application thread\r
*/\r
- StrategyMgr(PWMManager pwmManager) {\r
- this.pwmControl = pwmManager.getPWMControl();\r
+ public StrategyMgr(@DELEGATE MotorControl motorControl) {\r
+ mc = motorControl;\r
rand = new Random();\r
}\r
\r
- void processSonars(byte sonarSensors) {\r
+ void processSonars(@LOC("IN") byte sonarSensors) {\r
\r
// 5 sensors. 1,2 are fromnt left and right.\r
// Sensor 3 is right side, 4 back and 5 is left side.\r
}\r
}\r
\r
- void processLineSensors(byte lineSensorsMask) {\r
+ void processLineSensors(@LOC("IN") byte lineSensorsMask) {\r
+\r
+ @LOC("IN") int count = 0;\r
+\r
while ((lineSensorsMask & RobotMain.LS_ALL) != 0) {\r
+\r
+ if (count > 100) {\r
+ // if the robot fail to get out of weird condition wihtin 100 steps,\r
+ // terminate while loop for stabilizing behavior.\r
+ stop();\r
+ break;\r
+ }\r
+ count++;\r
+\r
if ((lineSensorsMask & RobotMain.LS_ALL) == RobotMain.LS_ALL) {\r
if (DEBUGL)\r
System.out.println("Line Sensors - ALL");\r
public void stop() {\r
if (DEBUG)\r
System.out.println("StrageyMgr: stop....");\r
- pwmControl.setSpeedLeft(zeroSpeed);\r
- pwmControl.setSpeedRight(zeroSpeed);\r
+ mc.setSpeedLeft(zeroSpeed);\r
+ mc.setSpeedRight(zeroSpeed);\r
}\r
\r
public void search() {\r
if (DEBUG)\r
System.out.println("StrategyMgr: search....");\r
- pwmControl.setSpeedLeft(70);\r
- pwmControl.setSpeedRight(50);\r
+ mc.setSpeedLeft(70);\r
+ mc.setSpeedRight(50);\r
}\r
\r
public void straight() {\r
if (DEBUG)\r
System.out.println("StrategyMgr: strait....");\r
- pwmControl.setSpeedLeft(100);\r
- pwmControl.setSpeedRight(100);\r
+ mc.setSpeedLeft(100);\r
+ mc.setSpeedRight(100);\r
}\r
\r
public void spinRight() {\r
if (DEBUG)\r
System.out.println("StrategyMgr: spinRight....");\r
- pwmControl.setSpeedSpinLeft(100);\r
- pwmControl.setSpeedSpinRight(0);\r
+ mc.setSpeedSpinLeft(100);\r
+ mc.setSpeedSpinRight(0);\r
}\r
\r
public void spinLeft() {\r
if (DEBUG)\r
System.out.println("StrategyMgr: spinLeft....");\r
- pwmControl.setSpeedSpinLeft(0);\r
- pwmControl.setSpeedSpinRight(100);\r
+ mc.setSpeedSpinLeft(0);\r
+ mc.setSpeedSpinRight(100);\r
}\r
\r
public void spin180() {\r
- int mod = (rand.nextInt() % 2);\r
+ @LOC("THIS,StrategyMgr.V") int mod = (rand.nextInt() % 2);\r
\r
if (DEBUG)\r
System.out.println("StrategyMgr: spin180....");\r
if (mod == 1) {\r
- pwmControl.setSpeedSpinLeft(0);\r
- pwmControl.setSpeedSpinRight(100);\r
+ mc.setSpeedSpinLeft(0);\r
+ mc.setSpeedSpinRight(100);\r
} else {\r
- pwmControl.setSpeedSpinLeft(100);\r
- pwmControl.setSpeedSpinRight(0);\r
+ mc.setSpeedSpinLeft(100);\r
+ mc.setSpeedSpinRight(0);\r
}\r
}\r
\r
public void right() {\r
if (DEBUG)\r
System.out.println("StrategyMgr: right....");\r
- pwmControl.setSpeedTurnLeft(100);\r
- pwmControl.setSpeedRight(zeroSpeed);\r
+ mc.setSpeedTurnLeft(100);\r
+ mc.setSpeedRight(zeroSpeed);\r
}\r
\r
public void left() {\r
if (DEBUG)\r
System.out.println("StrategyMgr: left....");\r
- pwmControl.setSpeedLeft(zeroSpeed);\r
- pwmControl.setSpeedTurnRight(100);\r
+ mc.setSpeedLeft(zeroSpeed);\r
+ mc.setSpeedTurnRight(100);\r
}\r
\r
public void bearRight() {\r
if (DEBUG)\r
System.out.println("StrategyMgr: bearRight....");\r
- pwmControl.setSpeedTurnLeft(100);\r
- pwmControl.setSpeedTurnRight(60);\r
+ mc.setSpeedTurnLeft(100);\r
+ mc.setSpeedTurnRight(60);\r
}\r
\r
public void bearLeft() {\r
if (DEBUG)\r
System.out.println("StrategyMgr: bearLeft....");\r
- pwmControl.setSpeedTurnLeft(60);\r
- pwmControl.setSpeedTurnRight(100);\r
+ mc.setSpeedTurnLeft(60);\r
+ mc.setSpeedTurnRight(100);\r
}\r
\r
public void back() {\r
if (DEBUG)\r
System.out.println("StrategyMgr: back....");\r
- pwmControl.setSpeedLeft(0);\r
- pwmControl.setSpeedRight(0);\r
+ mc.setSpeedLeft(0);\r
+ mc.setSpeedRight(0);\r
}\r
\r
public void backBearLeft() {\r
if (DEBUG)\r
System.out.println("StrategyMgr: backBearLeft....");\r
- pwmControl.setSpeedLeft(30);\r
- pwmControl.setSpeedRight(0);\r
+ mc.setSpeedLeft(30);\r
+ mc.setSpeedRight(0);\r
}\r
\r
public void backBearRight() {\r
if (DEBUG)\r
System.out.println("StrategyMgr: backBearRight....");\r
- pwmControl.setSpeedLeft(0);\r
- pwmControl.setSpeedRight(30);\r
+ mc.setSpeedLeft(0);\r
+ mc.setSpeedRight(30);\r
}\r
}\r
private static FileInputStream inputFile;
+ @TRUST
public static void init() {
inputFile = new FileInputStream("input.dat");
}
+ @TRUST
public static byte getCommand() {
String in = inputFile.readLine();
if (in == null) {
--- /dev/null
+/* Random.java -- a pseudo-random number generator
+ Copyright (C) 1998, 1999, 2000, 2001, 2002 Free Software Foundation, Inc.
+
+ This file is part of GNU Classpath.
+
+ GNU Classpath is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2, or (at your option)
+ any later version.
+
+ GNU Classpath is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with GNU Classpath; see the file COPYING. If not, write to the
+ Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ 02110-1301 USA.
+
+ Linking this library statically or dynamically with other modules is
+ making a combined work based on this library. Thus, the terms and
+ conditions of the GNU General Public License cover the whole
+ combination.
+
+ As a special exception, the copyright holders of this library give you
+ permission to link this library with independent modules to produce an
+ executable, regardless of the license terms of these independent
+ modules, and to copy and distribute the resulting executable under
+ terms of your choice, provided that you also meet, for each linked
+ independent module, the terms and conditions of the license of that
+ module. An independent module is a module which is not derived from
+ or based on this library. If you modify this library, you may extend
+ this exception to your version of the library, but you are not
+ obligated to do so. If you do not wish to do so, delete this
+ exception statement from your version. */
+
+/**
+ * This class generates pseudorandom numbers. It uses the same algorithm as the
+ * original JDK-class, so that your programs behave exactly the same way, if
+ * started with the same seed.
+ *
+ * The algorithm is described in <em>The Art of Computer Programming,
+ * Volume 2</em> by Donald Knuth in Section 3.2.1. It is a 48-bit seed, linear
+ * congruential formula.
+ *
+ * If two instances of this class are created with the same seed and the same
+ * calls to these classes are made, they behave exactly the same way. This
+ * should be even true for foreign implementations (like this), so every port
+ * must use the same algorithm as described here.
+ *
+ * If you want to implement your own pseudorandom algorithm, you should extend
+ * this class and overload the <code>next()</code> and
+ * <code>setSeed(long)</code> method. In that case the above paragraph doesn't
+ * apply to you.
+ *
+ * This class shouldn't be used for security sensitive purposes (like generating
+ * passwords or encryption keys. See <code>SecureRandom</code> in package
+ * <code>java.security</code> for this purpose.
+ *
+ * For simple random doubles between 0.0 and 1.0, you may consider using
+ * Math.random instead.
+ *
+ * @see java.security.SecureRandom
+ * @see Math#random()
+ * @author Jochen Hoenicke
+ * @author Eric Blake (ebb9@email.byu.edu)
+ * @status updated to 1.4
+ */
+@LATTICE("V,S,S*")
+@METHODDEFAULT("OUT<THIS,THIS<IN,THISLOC=THIS,RETURNLOC=OUT")
+public class Random {
+ /**
+ * True if the next nextGaussian is available. This is used by nextGaussian,
+ * which generates two gaussian numbers by one call, and returns the second on
+ * the second call.
+ *
+ * @serial whether nextNextGaussian is available
+ * @see #nextGaussian()
+ * @see #nextNextGaussian
+ */
+ @LOC("V")
+ private boolean haveNextNextGaussian;
+
+ /**
+ * The next nextGaussian, when available. This is used by nextGaussian, which
+ * generates two gaussian numbers by one call, and returns the second on the
+ * second call.
+ *
+ * @serial the second gaussian of a pair
+ * @see #nextGaussian()
+ * @see #haveNextNextGaussian
+ */
+ @LOC("V")
+ private double nextNextGaussian;
+
+ /**
+ * The seed. This is the number set by setSeed and which is used in next.
+ *
+ * @serial the internal state of this generator
+ * @see #next(int)
+ */
+ @LOC("S")
+ private long seed;
+
+ /**
+ * Creates a new pseudorandom number generator. The seed is initialized to the
+ * current time, as if by <code>setSeed(System.currentTimeMillis());</code>.
+ *
+ * @see System#currentTimeMillis()
+ */
+ public Random() {
+ setSeed(System.currentTimeMillis());
+ }
+
+ /**
+ * Creates a new pseudorandom number generator, starting with the specified
+ * seed, using <code>setSeed(seed);</code>.
+ *
+ * @param seed
+ * the initial seed
+ */
+ public Random(long seed) {
+ setSeed(seed);
+ }
+
+ /**
+ * Sets the seed for this pseudorandom number generator. As described above,
+ * two instances of the same random class, starting with the same seed, should
+ * produce the same results, if the same methods are called. The
+ * implementation for java.util.Random is:
+ *
+ * <pre>
+ * public synchronized void setSeed(long seed) {
+ * this.seed = (seed ˆ 0x5DEECE66DL) & ((1L << 48) - 1);
+ * haveNextNextGaussian = false;
+ * }
+ * </pre>
+ *
+ * @param seed
+ * the new seed
+ */
+ public synchronized void setSeed(long seed) {
+ this.seed = (seed ^ 0x5DEECE66DL) & ((1L << 48) - 1);
+ haveNextNextGaussian = false;
+ }
+
+ /**
+ * Generates the next pseudorandom number. This returns an int value whose
+ * <code>bits</code> low order bits are independent chosen random bits (0 and
+ * 1 are equally likely). The implementation for java.util.Random is:
+ *
+ * <pre>
+ * protected synchronized int next(int bits) {
+ * seed = (seed * 0x5DEECE66DL + 0xBL) & ((1L << 48) - 1);
+ * return (int) (seed >>> (48 - bits));
+ * }
+ * </pre>
+ *
+ * @param bits
+ * the number of random bits to generate, in the range 1..32
+ * @return the next pseudorandom value
+ * @since 1.1
+ */
+ protected synchronized int next(@LOC("IN") int bits) {
+ seed = (seed * 0x5DEECE66DL + 0xBL) & ((1L << 48) - 1);
+ return (int) (seed >>> (48 - bits));
+ }
+
+ /**
+ * Fills an array of bytes with random numbers. All possible values are
+ * (approximately) equally likely. The JDK documentation gives no
+ * implementation, but it seems to be:
+ *
+ * <pre>
+ * public void nextBytes(byte[] bytes)
+ * {
+ * for (int i = 0; i < bytes.length; i += 4)
+ * {
+ * int random = next(32);
+ * for (int j = 0; i + j < bytes.length && j < 4; j++)
+ * {
+ * bytes[i+j] = (byte) (random & 0xff)
+ * random >>= 8;
+ * }
+ * }
+ * }
+ * </pre>
+ *
+ * @param bytes
+ * the byte array that should be filled
+ * @throws NullPointerException
+ * if bytes is null
+ * @since 1.1
+ */
+ public void nextBytes(byte[] bytes) {
+ int random;
+ // Do a little bit unrolling of the above algorithm.
+ int max = bytes.length & ~0x3;
+ for (int i = 0; i < max; i += 4) {
+ random = next(32);
+ bytes[i] = (byte) random;
+ bytes[i + 1] = (byte) (random >> 8);
+ bytes[i + 2] = (byte) (random >> 16);
+ bytes[i + 3] = (byte) (random >> 24);
+ }
+ if (max < bytes.length) {
+ random = next(32);
+ for (int j = max; j < bytes.length; j++) {
+ bytes[j] = (byte) random;
+ random >>= 8;
+ }
+ }
+ }
+
+ /**
+ * Generates the next pseudorandom number. This returns an int value whose 32
+ * bits are independent chosen random bits (0 and 1 are equally likely). The
+ * implementation for java.util.Random is:
+ *
+ * <pre>
+ * public int nextInt() {
+ * return next(32);
+ * }
+ * </pre>
+ *
+ * @return the next pseudorandom value
+ */
+ public int nextInt() {
+ return next(32);
+ }
+
+ /**
+ * Generates the next pseudorandom number. This returns a value between
+ * 0(inclusive) and <code>n</code>(exclusive), and each value has the same
+ * likelihodd (1/<code>n</code>). (0 and 1 are equally likely). The
+ * implementation for java.util.Random is:
+ *
+ * <pre>
+ * public int nextInt(int n) {
+ * if (n <= 0)
+ * throw new IllegalArgumentException("n must be positive");
+ *
+ * if ((n & -n) == n) // i.e., n is a power of 2
+ * return (int) ((n * (long) next(31)) >> 31);
+ *
+ * int bits, val;
+ * do {
+ * bits = next(31);
+ * val = bits % n;
+ * } while (bits - val + (n - 1) < 0);
+ *
+ * return val;
+ * }
+ * </pre>
+ *
+ * <p>
+ * This algorithm would return every value with exactly the same probability,
+ * if the next()-method would be a perfect random number generator.
+ *
+ * The loop at the bottom only accepts a value, if the random number was
+ * between 0 and the highest number less then 1<<31, which is divisible by n.
+ * The probability for this is high for small n, and the worst case is 1/2
+ * (for n=(1<<30)+1).
+ *
+ * The special treatment for n = power of 2, selects the high bits of the
+ * random number (the loop at the bottom would select the low order bits).
+ * This is done, because the low order bits of linear congruential number
+ * generators (like the one used in this class) are known to be ``less
+ * random'' than the high order bits.
+ *
+ * @param n
+ * the upper bound
+ * @throws IllegalArgumentException
+ * if the given upper bound is negative
+ * @return the next pseudorandom value
+ * @since 1.2
+ */
+ public int nextInt(int n) {
+ if (n <= 0)
+ System.printString("ERROR: n must be positive\n");
+ if ((n & -n) == n) // i.e., n is a power of 2
+ return (int) ((n * (long) next(31)) >> 31);
+ int bits, val;
+ do {
+ bits = next(31);
+ val = bits % n;
+ } while (bits - val + (n - 1) < 0);
+ return val;
+ }
+
+ /**
+ * Generates the next pseudorandom long number. All bits of this long are
+ * independently chosen and 0 and 1 have equal likelihood. The implementation
+ * for java.util.Random is:
+ *
+ * <pre>
+ * public long nextLong() {
+ * return ((long) next(32) << 32) + next(32);
+ * }
+ * </pre>
+ *
+ * @return the next pseudorandom value
+ */
+ public long nextLong() {
+ return ((long) next(32) << 32) + next(32);
+ }
+
+ /**
+ * Generates the next pseudorandom boolean. True and false have the same
+ * probability. The implementation is:
+ *
+ * <pre>
+ * public boolean nextBoolean() {
+ * return next(1) != 0;
+ * }
+ * </pre>
+ *
+ * @return the next pseudorandom boolean
+ * @since 1.2
+ */
+ public boolean nextBoolean() {
+ return next(1) != 0;
+ }
+
+ /**
+ * Generates the next pseudorandom float uniformly distributed between 0.0f
+ * (inclusive) and 1.0f (exclusive). The implementation is as follows.
+ *
+ * <pre>
+ * public float nextFloat() {
+ * return next(24) / ((float) (1 << 24));
+ * }
+ * </pre>
+ *
+ * @return the next pseudorandom float
+ */
+ public float nextFloat() {
+ return next(24) / (float) (1 << 24);
+ }
+
+ /**
+ * Generates the next pseudorandom double uniformly distributed between 0.0
+ * (inclusive) and 1.0 (exclusive). The implementation is as follows.
+ *
+ * <pre>
+ * public double nextDouble() {
+ * return (((long) next(26) << 27) + next(27)) / (double) (1L << 53);
+ * }
+ * </pre>
+ *
+ * @return the next pseudorandom double
+ */
+ public double nextDouble() {
+ return (((long) next(26) << 27) + next(27)) / (double) (1L << 53);
+ }
+
+ /**
+ * Generates the next pseudorandom, Gaussian (normally) distributed double
+ * value, with mean 0.0 and standard deviation 1.0. The algorithm is as
+ * follows.
+ *
+ * <pre>
+ * public synchronized double nextGaussian() {
+ * if (haveNextNextGaussian) {
+ * haveNextNextGaussian = false;
+ * return nextNextGaussian;
+ * } else {
+ * double v1, v2, s;
+ * do {
+ * v1 = 2 * nextDouble() - 1; // between -1.0 and 1.0
+ * v2 = 2 * nextDouble() - 1; // between -1.0 and 1.0
+ * s = v1 * v1 + v2 * v2;
+ * } while (s >= 1);
+ *
+ * double norm = Math.sqrt(-2 * Math.log(s) / s);
+ * nextNextGaussian = v2 * norm;
+ * haveNextNextGaussian = true;
+ * return v1 * norm;
+ * }
+ * }
+ * </pre>
+ *
+ * <p>
+ * This is described in section 3.4.1 of <em>The Art of Computer
+ * Programming, Volume 2</em> by Donald Knuth.
+ *
+ * @return the next pseudorandom Gaussian distributed double
+ */
+ public synchronized double nextGaussian() {
+ if (haveNextNextGaussian) {
+ haveNextNextGaussian = false;
+ return nextNextGaussian;
+ }
+ double v1, v2, s;
+ do {
+ v1 = 2 * nextDouble() - 1; // Between -1.0 and 1.0.
+ v2 = 2 * nextDouble() - 1; // Between -1.0 and 1.0.
+ s = v1 * v1 + v2 * v2;
+ } while (s >= 1);
+ double norm = Math.sqrt(-2 * Math.log(s) / s);
+ nextNextGaussian = v2 * norm;
+ haveNextNextGaussian = true;
+ return v1 * norm;
+ }
+}
projects / IRC.git / commitdiff