--- /dev/null
+
+public class MotorController {
+
+}
--- /dev/null
+/*\r
+ * PWMControl.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 the super class of all motor control classes. The class\r
+ * represent a generic interface to such class. In our case the motor controller\r
+ * require for 0 speed 1.5 ms out of 10-20ms cycle and a change between 1 to 2\r
+ * seconds for max reverse and forward speed. These values: 1, 1.5, 2 and 20 ms\r
+ * are thus this class specific\r
+ * \r
+ * @author Michael Gesundheit\r
+ * @version 1.0\r
+ */\r
+\r
+public abstract class PWMControl {\r
+\r
+ PWMManager pwmManager;\r
+ boolean DEBUG = false;\r
+ // boolean DEBUG = true;\r
+ static final byte PULSE_HIGH = 1;\r
+ static final byte PULSE_LOW = 0;\r
+ int highMask;\r
+ int m1HighMask;\r
+ int m2HighMask;\r
+ int lowMask;\r
+ int myOwnBit;\r
+ int myBitPos;\r
+ int m1BitPos;\r
+ int m2BitPos;\r
+ int speedFactor;\r
+ int agilityFactor;\r
+ boolean updateTime;\r
+ int pulseWidth;\r
+ Object obj;\r
+\r
+ int motorLeftUpTime = 150;\r
+ int motorRightUpTime = 150;\r
+ boolean manualMode = false;\r
+\r
+ PWMControl(PWMManager pwmMan, int motor1bit, int motor2bit) {\r
+ pwmManager = pwmMan; // This is papa\r
+ m1BitPos = 0x1 << motor1bit;\r
+ m2BitPos = 0x1 << motor2bit;\r
+ m1HighMask = 0x1 << motor1bit;\r
+ m2HighMask = 0x1 << motor2bit;\r
+ lowMask = 0x0;\r
+ obj = new Object();\r
+ }\r
+\r
+ public void setManualMode() {\r
+ if (DEBUG)\r
+ System.out.println("PWMContolr: setManualMode... ");\r
+// synchronized (obj) {\r
+ if (manualMode == false) {\r
+ manualMode = true;\r
+ }\r
+// }\r
+ }\r
+\r
+ public void setAutomatedMode() {\r
+ if (DEBUG)\r
+ System.out.println("PWMControl: setAutomatedMode... ");\r
+// synchronized (obj) {\r
+ if (manualMode == true) {\r
+ System.out.println("PWMControl: wake me up... ");\r
+ manualMode = false;\r
+ // obj.notify();\r
+ System.out.println("PWMControl: wake me up...... tried!!!");\r
+ }\r
+// }\r
+ }\r
+\r
+ /**\r
+ * OutPut value to motor control line\r
+ */\r
+ public void outToPortMLeft(byte value) {\r
+ /*\r
+ * System.out.println("PWMControl " + Integer.toString(myOwnBit) +\r
+ * //" bit position = 0x" + Integer.toHexString(myOwnBit) +\r
+ * " output value = 0x" + Integer.toHexString(value));\r
+ */\r
+ if (value == PULSE_HIGH) {\r
+ pwmManager.writeToPort(m1BitPos, m1HighMask);\r
+ } else {\r
+ pwmManager.writeToPort(m1BitPos, lowMask);\r
+ }\r
+ }\r
+\r
+ public void outToPortMRight(byte value) {\r
+ /*\r
+ * System.out.println("PWMControl " + Integer.toString(myOwnBit) +\r
+ * //" bit position = 0x" + Integer.toHexString(myOwnBit) +\r
+ * " output value = 0x" + Integer.toHexString(value));\r
+ */\r
+ if (value == PULSE_HIGH) {\r
+ pwmManager.writeToPort(m2BitPos, m2HighMask);\r
+ } else {\r
+ pwmManager.writeToPort(m2BitPos, lowMask);\r
+ }\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(int timePosition) {\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(int timePosition) {\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(int timePosition) {\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(int timePosition) {\r
+ }\r
+\r
+ /**\r
+ * setSpeedLeft - speed control for motor 1.\r
+ * \r
+ * @param uptime\r
+ * pulse width (time position)\r
+ */\r
+ public void setSpeedLeft(int timePosition) {\r
+ }\r
+\r
+ /**\r
+ * setSpeedRight - speed control for motor 1.\r
+ * \r
+ * @param uptime\r
+ * pulse width (time position)\r
+ */\r
+ public void setSpeedRight(int timePosition) {\r
+ }\r
+\r
+ public void setSpeedAgilityFactors(int speed, int agility) {\r
+// synchronized (this) {\r
+ speedFactor = speed;\r
+ agilityFactor = agility;\r
+// }\r
+ }\r
+\r
+ public void setUrgentReverse() {\r
+ }\r
+\r
+ public void setUrgentStraight() {\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
--- /dev/null
+/*\r
+ * PWMManager.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
+ * PWMManager - Interface between robot strategy code and the various motors\r
+ * PWMControl classes. Agility is a number between 0 to 100 and represent the\r
+ * precentage of max speed to be applied to the turn side wheel. On left turn\r
+ * the left wheel will receive less speed as % of "speed" value represented by\r
+ * agility.\r
+ * \r
+ * @author Michael Gesundheit\r
+ * @version 1.0\r
+ */\r
+\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 currentRegMask;\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 RCBridge rcb;\r
+ private int speedFactor;\r
+ private int agilityFactor;\r
+ private int zeroSpeed = 45;\r
+\r
+ /**\r
+ * Constractor\r
+ */\r
+ public PWMManager(String pwmSelection) {\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
+\r
+ // rcb = new RCBridge(this);\r
+ rcb = new RCBridge();\r
+\r
+ /* Enable input bits 0,1 Enable output for the rest */\r
+ // rawJEM.set(GPIO_E_OER, 0x00C0);\r
+ }\r
+\r
+ public void setManualMode() {\r
+ if (DEBUG)\r
+ System.out.println("PWMManager: 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
+ rcb.setAutomatedMode();\r
+ }\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
+ currentRegMask |= myValue;\r
+ /*\r
+ * // if (DEBUG){ // System.out.println("PWMM: writeToPort: myBit = 0x" +\r
+ * Integer.toHexString(myBit) + // " ~myBit = 0x" +\r
+ * Integer.toHexString(~myBit) + " myValue = 0x" + //\r
+ * Integer.toHexString(myValue) + " currentRegMask = 0x" + //\r
+ * Integer.toHexString(currentRegMask)); //}\r
+ */\r
+ // rawJEM.set(GPIO_E_OUT, currentRegMask);\r
+ }\r
+\r
+ /*\r
+ * public void stop(){ if(DEBUG) System.out.println("PWMManager: stop....");\r
+ * pwmControl.setSpeedLeft(zeroSpeed); pwmControl.setSpeedRight(zeroSpeed); }\r
+ * \r
+ * public void search(){ if(DEBUG)\r
+ * System.out.println("PWMManager: search...."); pwmControl.setSpeedLeft(70);\r
+ * pwmControl.setSpeedRight(50); }\r
+ * \r
+ * public void straight(){ if(DEBUG)\r
+ * System.out.println("PWMManager: strait...."); pwmControl.setSpeedLeft(100);\r
+ * pwmControl.setSpeedRight(100); }\r
+ * \r
+ * public void spinRight(){ if(DEBUG)\r
+ * System.out.println("PWMManager: spinRight....");\r
+ * pwmControl.setSpeedSpinLeft(100); pwmControl.setSpeedSpinRight(0); }\r
+ * \r
+ * public void spinLeft(){ if(DEBUG)\r
+ * System.out.println("PWMManager: spinLeft....");\r
+ * pwmControl.setSpeedSpinLeft(0); pwmControl.setSpeedSpinRight(100); }\r
+ * \r
+ * public void spin180(){ int mod = (rand.nextInt() % 2);\r
+ * \r
+ * if(DEBUG) System.out.println("PWMManager: spin180...."); if(mod == 1){\r
+ * pwmControl.setSpeedSpinLeft(0); pwmControl.setSpeedSpinRight(100); }else{\r
+ * pwmControl.setSpeedSpinLeft(100); pwmControl.setSpeedSpinRight(0); } }\r
+ * \r
+ * public void right(){ if(DEBUG) System.out.println("PWMManager: right....");\r
+ * pwmControl.setSpeedTurnLeft(100); pwmControl.setSpeedRight(zeroSpeed); }\r
+ * \r
+ * public void left(){ if(DEBUG) System.out.println("PWMManager: left....");\r
+ * pwmControl.setSpeedLeft(zeroSpeed); pwmControl.setSpeedTurnRight(100); }\r
+ * \r
+ * public void bearRight(){ if(DEBUG)\r
+ * System.out.println("PWMManager: bearRight....");\r
+ * pwmControl.setSpeedTurnLeft(100); pwmControl.setSpeedTurnRight(60); }\r
+ * \r
+ * public void bearLeft(){ if(DEBUG)\r
+ * System.out.println("PWMManager: bearLeft....");\r
+ * pwmControl.setSpeedTurnLeft(60); pwmControl.setSpeedTurnRight(100); }\r
+ * \r
+ * public void back(){ if(DEBUG) System.out.println("PWMManager: back....");\r
+ * pwmControl.setSpeedLeft(0); pwmControl.setSpeedRight(0); }\r
+ * \r
+ * public void backBearLeft(){ if(DEBUG)\r
+ * System.out.println("PWMManager: backBearLeft....");\r
+ * pwmControl.setSpeedLeft(30); pwmControl.setSpeedRight(0); }\r
+ * \r
+ * public void backBearRight(){ if(DEBUG)\r
+ * System.out.println("PWMManager: backBearRight....");\r
+ * pwmControl.setSpeedLeft(0); pwmControl.setSpeedRight(30); }\r
+ */\r
+ public void resume() {\r
+ if (DEBUG)\r
+ System.out.println("PWMManager: Reasume...........");\r
+ }\r
+\r
+ /**\r
+ * setSpeedFactor - set speed factor value\r
+ * \r
+ * @param speed\r
+ * factor\r
+ */\r
+ public synchronized void setSpeedFactor(int speedFactor) {\r
+ if (DEBUG)\r
+ System.out.println("PWMManager: setSpeedFactor....");\r
+ this.speedFactor = speedFactor;\r
+ pwmControl.setSpeedAgilityFactors(speedFactor, agilityFactor);\r
+ }\r
+\r
+ /**\r
+ * setAgilityFactor\r
+ * \r
+ * @param agility\r
+ * factor\r
+ */\r
+ public synchronized void setAgilityFactor(int agilityFactor) {\r
+ if (DEBUG)\r
+ System.out.println("PWMManager: setAgilityFactor....");\r
+ this.agilityFactor = agilityFactor;\r
+ pwmControl.setSpeedAgilityFactors(speedFactor, agilityFactor);\r
+ }\r
+\r
+ /**\r
+ * setSpeedAgilityFactors - set both speed and agility\r
+ * \r
+ * @param speed\r
+ * @param agility\r
+ */\r
+ public synchronized void setSpeedAgilityFactors(int speed, int agility) {\r
+ if (DEBUG)\r
+ System.out.println("PWMManager: setSpeedAgilityFactors....");\r
+ speedFactor = speed;\r
+ agilityFactor = agility;\r
+ pwmControl.setSpeedAgilityFactors(speedFactor, agilityFactor);\r
+ }\r
+\r
+ /**\r
+ * Control debug messaging. true - Activate debug messages false - deactivate\r
+ * debug messages\r
+ */\r
+ public void setDebug(boolean debug) {\r
+ DEBUG = debug;\r
+ }\r
+}
\ No newline at end of file
--- /dev/null
+/*\r
+ * PWMNative.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
+//public class PWMNative extends PWMControl implements Runnable, InterruptEventListener {\r
+public class PWMNative extends PWMControl {\r
+\r
+ private int PULSE_INTERVAL = 2000;\r
+ private int NATIVE_OFFSET = 100;\r
+ private Object obj;\r
+ private Object tc0Obj;\r
+ private Object tc1Obj;\r
+ private int pulseUpTime;\r
+\r
+ // private TimerCounter tc0;\r
+ // private TimerCounter tc1;\r
+ // private TimerCounter[] tcSet = new TimerCounter[2];\r
+\r
+ PWMNative(PWMManager pwmMan, int motor1bit, int motor2bit) {\r
+ super(pwmMan, motor1bit, motor2bit);\r
+\r
+ System.out.println("PWMNative constructor.....Start");\r
+ obj = new Object();\r
+ tc0Obj = new Object();\r
+ tc1Obj = new Object();\r
+\r
+ // TimerCounter.setPrescalerClockSource(TimerCounter.INTERNAL_PERIPHERAL_CLOCK);\r
+ // TimerCounter.setPrescalerReloadRegisterValue(375);\r
+ // TimerCounter.setPrescalerEnabled(true);\r
+\r
+ // tc0 = tcSet[0] = new TimerCounter(0);\r
+ // tc0.setMode_IO_Line_A(TimerCounter.TIMER_0_OUTPUT);\r
+ // // bring TIMER_0_OUTPUT_DIVIDE_BY_2 out via IO_Line_B\r
+ // tc0.setMode_IO_Line_B(TimerCounter.TIMER_0_OUTPUT_DIVIDE_BY_2);\r
+ // // In JemBuilder, go to Pin Setup 2 and allocate Port E bit 6 to\r
+ // // be Timer IO_Line_B.\r
+ //\r
+ // // Connect signal lead of servo (usually the white one) to IOE6 on\r
+ // // JStamp\r
+ // tc0.set_IO_Line_A_Polarity(TimerCounter.POLARITY_ACTIVE_STATE_HIGH);\r
+ // tc0.set_IO_Line_B_Polarity(TimerCounter.POLARITY_ACTIVE_STATE_HIGH);\r
+ // tc0.setExternalTimerEnableMode(TimerCounter.TIMER_ENABLED_ONLY_VIA_MTEN_AND_TRIGGER);\r
+ // tc0.setReloadRegisterValue(100);\r
+ // tc0.setExternalStartTriggerMode(TimerCounter.NO_EXTERNAL_START_TRIGGER);\r
+ // tc0.setMasterTimerEnabled(true);\r
+ // tc0.setGlobalInterruptEnabled(true);\r
+ // tc0.setTimeOutInterruptEnabled(true);\r
+ // tc0.setTimerTriggerRegister(false);\r
+ // System.out.println("PWMNative: Constructor completed 1....");\r
+ /*\r
+ * tc1 = tcSet[1] = new TimerCounter ( 1 ); tc1.setMode_IO_Line_A(\r
+ * TimerCounter.TIMER_1_OUTPUT ); //bring TIMER_0_OUTPUT_DIVIDE_BY_2 out via\r
+ * IO_Line_B tc1.setMode_IO_Line_B( TimerCounter.TIMER_1_OUTPUT_DIVIDE_BY_2\r
+ * ); //In JemBuilder, go to Pin Setup 2 and allocate Port E bit 6 to //be\r
+ * Timer IO_Line_B.\r
+ * tc1.set_IO_Line_A_Polarity(TimerCounter.POLARITY_ACTIVE_STATE_HIGH);\r
+ * tc1.set_IO_Line_B_Polarity(TimerCounter.POLARITY_ACTIVE_STATE_HIGH);\r
+ * tc1.setExternalTimerEnableMode\r
+ * (TimerCounter.TIMER_ENABLED_ONLY_VIA_MTEN_AND_TRIGGER );\r
+ * tc1.setReloadRegisterValue( 100 );\r
+ * tc1.setExternalStartTriggerMode(TimerCounter.NO_EXTERNAL_START_TRIGGER );\r
+ * tc1.setMasterTimerEnabled( true ); tc1.setGlobalInterruptEnabled( true);\r
+ * tc1.setTimeOutInterruptEnabled( true); tc1.setTimerTriggerRegister( false\r
+ * );\r
+ */\r
+ /*\r
+ * // Add interrupt event listener for GPTC\r
+ * InterruptController.addInterruptListener( this,\r
+ * InterruptController.GPTC_INTERRUPT );\r
+ * \r
+ * // Turn on interrupts InterruptController.enableInterrupt(\r
+ * InterruptController.GPTC_INTERRUPT );\r
+ * \r
+ * // start all prescaler based timers TimerCounter.setPrescalerEnabled(\r
+ * true );\r
+ */\r
+ // t2 = new Thread(this);\r
+ // t2.start();\r
+ // t2.setPriority(20);\r
+ System.out.println("PWMNative: Constructor return.....");\r
+ }\r
+\r
+ public void setUpTime(int upTime) {\r
+ // synchronized (obj) {\r
+ pulseUpTime = upTime;\r
+ // }\r
+ }\r
+\r
+ /*\r
+ * public void interruptEvent() { TimerCounter tc; int tcIntrState;\r
+ * \r
+ * System.out.println("PWMNative: InterruptEvent"); do { for ( int tcNum=0;\r
+ * tcNum<2; tcNum++ ) { tc = tcSet[ tcNum ];\r
+ * if(tc.readAndClear_TimeOutInterruptStatus()){ switch(tcNum){ case 0:\r
+ * System.out.println("PWMNative: Interrupt case 0"); synchronized(tc0Obj){\r
+ * System.out.println("PWMNative: Interrupt notify 0"); tc0Obj.notify(); }\r
+ * break; case 1: System.out.println("PWMNative: Interrupt case 1");\r
+ * synchronized(tc1Obj){ System.out.println("PWMNative: Interrupt notify 1");\r
+ * tc1Obj.notify(); } break; default:; } } } } while (\r
+ * TimerCounter.isGPTCInterruptPending() ); }\r
+ */\r
+ public void run() {\r
+ //\r
+ // System.out.println("PWMNative: run method........");\r
+ // int upTime0 = 150; // 1.5 milli seconds = 0 speed\r
+ // int upTime1 = 150; // 1.5 milli seconds = 0 speed\r
+ //\r
+ // while (true) {\r
+ // synchronized (obj) {\r
+ // /*\r
+ // * System.out.println("PWMNative: Updating Up Times......Left = " +\r
+ // * Integer.toString(motorLeftUpTime) + " Right = " +\r
+ // * Integer.toString(motorRightUpTime));\r
+ // */\r
+ // upTime0 = motorLeftUpTime;\r
+ // upTime1 = motorRightUpTime;\r
+ // }\r
+ //\r
+ // // Timer number 1\r
+ // tc0.setReloadRegisterValue(upTime0);\r
+ // outToPortMLeft(PULSE_HIGH);\r
+ // tc0.setTimerTriggerRegister(true);\r
+ // do {\r
+ // } while (tc0.didTimeOutInterruptOccur());\r
+ // outToPortMLeft(PULSE_LOW);\r
+ // tc0.setTimerTriggerRegister(false);\r
+ // tc0.resetTimeOutInterruptStatus();\r
+ //\r
+ // // System.out.println("PWMNative: Big loop long suspend1");\r
+ // try {\r
+ // tc0Obj.wait(18, 500000);\r
+ // } catch (Exception e) {\r
+ // }\r
+ // // System.out.println("PWMNative: Big loop long suspend2");\r
+ //\r
+ // tc0.setReloadRegisterValue(upTime1);\r
+ // outToPortMRight(PULSE_HIGH);\r
+ // tc0.setTimerTriggerRegister(true);\r
+ // do {\r
+ // } while (tc0.didTimeOutInterruptOccur());\r
+ // outToPortMRight(PULSE_LOW);\r
+ // tc0.setTimerTriggerRegister(false);\r
+ // tc0.resetTimeOutInterruptStatus();\r
+ //\r
+ // try {\r
+ // tc0Obj.wait(18, 500000);\r
+ // } catch (Exception e) {\r
+ // }\r
+\r
+ /*\r
+ * // Timer number 2 tc1.setReloadRegisterValue( upTime1 );\r
+ * tc1.setTimerTriggerRegister( true ); synchronized(tc1Obj){ try{\r
+ * System.out.println("PWMNative: Sleep 3"); tc1Obj.wait();\r
+ * System.out.println("PWMNative: Wake Up 3"); }catch(Exception e){ } }\r
+ * tc1.setTimerTriggerRegister( false ); tc1.resetTimeOutInterruptStatus();\r
+ * tc1.setReloadRegisterValue( PULSE_INTERVAL - upTime1 ); //this sets pulse\r
+ * interval tc1.setTimerTriggerRegister( true ); synchronized(tc1Obj){ try{\r
+ * System.out.println("PWMNative: Sleep 3"); tc1Obj.wait();\r
+ * System.out.println("PWMNative: Wake Up 3"); }catch(Exception e){ } }\r
+ * tc1.setTimerTriggerRegister( false ); tc1.resetTimeOutInterruptStatus();\r
+ */\r
+ // }\r
+ }\r
+\r
+ public void setManualMode() {\r
+ if (DEBUG)\r
+ System.out.println("PWMControl: setManualMode... ");\r
+ // synchronized (obj) {\r
+ if (manualMode == false) {\r
+ manualMode = true;\r
+ }\r
+ // }\r
+ }\r
+\r
+ public void setAutomatedMode() {\r
+ if (DEBUG)\r
+ System.out.println("PWMControl: setAutomatedMode... ");\r
+ /*\r
+ * synchronized(obj){ if(manualMode == true){\r
+ * System.out.println("PWMControl: wake me up... "); obj.notifyAll();\r
+ * manualMode = false; } }\r
+ */\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(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
+\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedSpinLeft: input-> " + Integer.toString(timePosition));\r
+ }\r
+ int timePos = timePosition + NATIVE_OFFSET;\r
+ int motorUpTime = (int) (timePos * agilityFactor * speedFactor) / 10000;\r
+\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedSpinLeft: output-> = " + Integer.toString(motorUpTime));\r
+ }\r
+ // synchronized (obj) {\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(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
+ int timePos = timePosition + NATIVE_OFFSET;\r
+ int motorUpTime = (int) ((timePos) * agilityFactor * speedFactor) / 10000;\r
+\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedSpinRight: output-> = " + Integer.toString(motorUpTime));\r
+ }\r
+ // synchronized (obj) {\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(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
+ int timePosLocal = timePosition + NATIVE_OFFSET;\r
+ int motorUpTime =\r
+ ((timePosLocal * 100 + ((100 - timePosLocal) * (100 - agilityFactor))) * speedFactor) / 10000;\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedTurnLeft: output-> = " + Integer.toString(motorUpTime));\r
+ }\r
+ // synchronized (obj) {\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(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
+ int timePos = timePosition + NATIVE_OFFSET;\r
+ int motorUpTime =\r
+ (((timePos * 100) + ((100 - timePos) * (100 - agilityFactor))) * speedFactor) / 10000;\r
+\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedTurnRight: output-> " + Integer.toString(motorUpTime));\r
+ }\r
+ // synchronized (obj) {\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(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
+ int timePos = timePosition + NATIVE_OFFSET;\r
+ int motorUpTime = (int) ((timePos * 100) * speedFactor) / 10000;\r
+\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedLeft: output-> " + Integer.toString(motorUpTime));\r
+ }\r
+ // synchronized (obj) {\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(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
+ int timePos = timePosition + NATIVE_OFFSET;\r
+ int motorUpTime = (int) (((timePos * 100) * speedFactor) / 10000);\r
+\r
+ if (DEBUG) {\r
+ System.out.println("setSpeedRight: output-> " + Integer.toString(motorUpTime));\r
+ }\r
+ // synchronized (obj) {\r
+ /* Factor in speedFactor */\r
+ motorRightUpTime = motorUpTime;\r
+ // }\r
+ }\r
+\r
+ public void setSpeedAgilityFactors(int speed, int agility) {\r
+ // synchronized (obj) {\r
+ speedFactor = speed;\r
+ agilityFactor = agility;\r
+ // }\r
+ }\r
+\r
+ public void setUrgentReverse() {\r
+ // synchronized (obj) {\r
+ motorLeftUpTime = 1;\r
+ motorRightUpTime = 1;\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
+/*\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 motor controller specific. In our case the motor controller\r
+ * require for 0 speed 1.5 ms out of 10-20ms cycle and a change between 1 to 2\r
+ * seconds for max reverse and forward speed. These values: 1, 1.5, 2 and 20 ms\r
+ * are thus this class specific\r
+ * \r
+ * @author Michael Gesundheit\r
+ * @version 1.0\r
+ */\r
+\r
+public class PWMRtsj extends PWMControl {\r
+\r
+ // RelativeTime zeroSpeedDuration;\r
+ // RelativeTime timeUpDuration;\r
+ // RelativeTime timeDownDuration;\r
+ // RelativeTime cycleTime;\r
+ boolean updateTime;\r
+ int sleepUpDurationNano = 500000;\r
+ long sleepUpDurationMilli = 1;\r
+ int sleepDownDurationNano = 500000;\r
+ long sleepDownDurationMilli = 18;\r
+ int pulseWidth;\r
+ // Object obj;\r
+\r
+ private static final int GPIO_A_OER = 0x09002000;\r
+ private static final int GPIO_A_OUT = GPIO_A_OER + 4;\r
+ private static final int GPIO_A_IN = GPIO_A_OER + 8;\r
+\r
+ /**\r
+ * Constructor - Start a standard Java thread. The thread is suspended and\r
+ * will wake up evey 18 ms. It will issue a 1-2ms pulse and suspends itself\r
+ * again to 18ms. This is to have a total of 20ms or less yet it's the maxim\r
+ * possible cycle so as to load the cpu as little as possible.\r
+ */\r
+ public PWMRtsj(PWMManager pwmMan, int motor1bit, int motor2bit) {\r
+\r
+ super(pwmMan, motor1bit, motor2bit); // This is papa\r
+\r
+ motorLeftUpTime = 450000; // Nano seconds\r
+ motorRightUpTime = 450000; // Nano seconds\r
+\r
+ if (DEBUG) {\r
+ System.out.println("PWMRtsj: About to start RoboThread...");\r
+ }\r
+\r
+ // t2 = new RoboThread();\r
+ // t2.start();\r
+ // t2.setPriority(10);\r
+ }\r
+\r
+ // A poor's man ajustimg for the 0 speed which found\r
+ // to be 450000 nano seconds.\r
+ private int normalizeTime(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(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
+ int timePos = normalizeTime(timePosition);\r
+\r
+ 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(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
+ int timePos = normalizeTime(timePosition);\r
+\r
+ 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(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
+ int timePosLocal = normalizeTime(timePosition);\r
+ 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(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
+ int timePos = normalizeTime(timePosition);\r
+ int motorUpTime = ((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(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
+ int timePos = normalizeTime(timePosition);\r
+ int motorUpTime = (int) (timePos * 100) * speedFactor;\r
+ /*\r
+ * System.out.println("motorUpTime = " + Integer.toString(motorUpTime) +\r
+ * " timePos = " + Integer.toString((int)timePos) + " timePosition = " +\r
+ * Integer.toString((int)timePosition) + " speedFactor = " +\r
+ * 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(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
+ int timePos = normalizeTime(timePosition);\r
+ 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 setSpeedAgilityFactors(int speed, int agility) {\r
+// synchronized (this) {\r
+ speedFactor = speed;\r
+ agilityFactor = agility;\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
+/*
+ * PWMControl.java
+ *
+ * Copyright (c) 1993-2002 Sun Microsystems, Inc. All Rights Reserved.
+ *
+ * This software is the confidential and proprietary information of Sun
+ * Microsystems, Inc. ("Confidential Information"). You shall not
+ * disclose such Confidential Information and shall use it only in
+ * accordance with the terms of the license agreement you entered into
+ * with Sun.
+ *
+ * SUN MAKES NO REPRESENTATIONS OR WARRANTIES ABOUT THE SUITABILITY OF
+ * THE SOFTWARE, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
+ * TO THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, OR NON-INFRINGEMENT. SUN SHALL NOT BE LIABLE FOR
+ * ANY DAMAGES SUFFERED BY LICENSEE AS A RESULT OF USING, MODIFYING OR
+ * DISTRIBUTING THIS SOFTWARE OR ITS DERIVATIVES.
+ */
+
+/**
+ * Manage the software link between two IO port lines.
+ * <p>
+ * Copyright: Copyright (c) 2002
+ * </p>
+ *
+ * @author Michael Gesundheit
+ * @version 1.0
+ */
+public class RCBridge {
+
+ public void setManualMode(){
+// rcLleft.activate();
+// rcLright.activate();
+}
+
+public void setAutomatedMode(){
+ // rcLleft.deActivate();
+ // rcLright.deActivate();
+}
+}
\ No newline at end of file
--- /dev/null
+/*\r
+ * RobotMain.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
+ * Robot's Main class - instantiates all required classes and resources.\r
+ * \r
+ * @author Michael Gesundheit\r
+ * @version 1.0\r
+ */\r
+public class RobotMain {\r
+\r
+ private static boolean DEBUG1 = true;\r
+ private static boolean DEBUG = false;\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 AUTONOMUS_MODE = 2;\r
+ private static final int ON_OFF = 128;// 0x80\r
+ private static final int MANUAL_AUTONOMUS = 32; // 0x20\r
+ public static final int LINE_SENSORS = 64; // 0x40\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
+\r
+ private static PWMManager pwmm;\r
+ private static StrategyMgr strategyMgr;\r
+\r
+ private static int onOffMode = ON_MODE;\r
+ private static int manualAutonomusMode = AUTONOMUS_MODE;\r
+ private static byte lineSensorsMask;\r
+ private static byte sonarSensors;\r
+ private static byte[] command;\r
+ private static byte currentCommand;\r
+ private static Object obj;\r
+ private static Object obj1;\r
+ private static byte privSonars;\r
+ private static byte privLineSensors;\r
+ private static byte currentSonars;\r
+ private static byte currentLineSensors;\r
+ public static String pwmSelection;\r
+\r
+ /**\r
+ * Constructor for the class for the robot main thread.\r
+ */\r
+ RobotMain() {\r
+ }\r
+\r
+ /**\r
+ * Processes sonar sensor input. This method is called each time new sonar\r
+ * sensor data arrives, and each time that a mode switch occurs between ON/OFF\r
+ * and Manual Override.\r
+ */\r
+ static void processSonars() {\r
+ strategyMgr.processSonars(sonarSensors);\r
+ }\r
+\r
+ /**\r
+ * Processes line sensor input. This method is called each time new line\r
+ * sensor data arrives, and each time that a mode switch occurs between ON/OFF\r
+ * and Manual Override.\r
+ */\r
+ static void processLineSensors() {\r
+ strategyMgr.processLineSensors(lineSensorsMask);\r
+ resume();\r
+ }\r
+\r
+ /**\r
+ * Resumes motors as per the last sonar command.\r
+ */\r
+ public static void resume() {\r
+ processSonars();\r
+ }\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
+ private static void processIOCommand() {\r
+\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
+ // }\r
+\r
+ if (DEBUG) {\r
+ // System.out.println("processIOCommand: Getting in opCode = "\r
+ // + Integer.toHexString((int) opCode) + " data = " +\r
+ // Integer.toHexString((int) data));\r
+ }\r
+ switch ((int) opCode) {\r
+ case ON_OFF:\r
+ if (DEBUG) {\r
+ System.out.println("processIO: ON_OFF....");\r
+ }\r
+ if ((data & 0x1) == 0x1) {\r
+ System.out.println("processIO: ON MODE........");\r
+ onOffMode = ON_MODE;\r
+ processLineSensors();\r
+ processSonars();\r
+ } else {\r
+ System.out.println("processIO: OFF MODE.......");\r
+ onOffMode = OFF_MODE;\r
+ strategyMgr.stop();\r
+ }\r
+ break;\r
+ case MANUAL_AUTONOMUS:\r
+ if (DEBUG) {\r
+ System.out.println("processIO: Setting manual_autonomus mode");\r
+ }\r
+ if ((data & 0x1) == 0x1) {\r
+ manualAutonomusMode = MANUAL_MODE;\r
+ pwmm.setManualMode();\r
+ } else {\r
+ manualAutonomusMode = AUTONOMUS_MODE;\r
+ pwmm.setAutomatedMode();\r
+ processLineSensors();\r
+ processSonars();\r
+ }\r
+ break;\r
+ case LINE_SENSORS:\r
+ if (DEBUG) {\r
+ // System.out.println("processIO: Line Sensors.................."\r
+ // + Integer.toBinaryString((int) (data & LS_ALL)));\r
+ }\r
+ lineSensorsMask = (byte) (data & LS_ALL);\r
+\r
+ /*\r
+ * Line sensors with '0' data means the robot moved off the edge line, and\r
+ * there is nothing to do.\r
+ */\r
+ if (((data & LS_ALL) == 0) || ((data & LS_ALL) == privLineSensors)) {\r
+ privLineSensors = (byte) (data & LS_ALL);\r
+ return;\r
+ }\r
+ if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {\r
+ if (DEBUG)\r
+ System.out.println("processIO: Line Sensors..Process...!!!");\r
+ processLineSensors();\r
+ }\r
+ break;\r
+ case SONAR_SENSORS:\r
+ if (DEBUG) {\r
+ // System.out.println("processIO: SONAR_SENORS: bits = ......"\r
+ // + Integer.toBinaryString((int) (currentCommand & ALL_SONARS)));\r
+ }\r
+ currentSonars = (byte) (data & ALL_SONARS);\r
+ /*\r
+ * No need to synchronized on this variable assignment since all referring\r
+ * code is on the same logical thread\r
+ */\r
+ sonarSensors = (byte) currentSonars;\r
+ if ((onOffMode == ON_MODE) && (manualAutonomusMode == AUTONOMUS_MODE)) {\r
+ processSonars();\r
+ }\r
+ break;\r
+ default:\r
+ System.out.println("processIOCommand: Default: opCode = " + Integer.toString((int) opCode)\r
+ + " data = " + Integer.toString((int) data));\r
+ }\r
+ }\r
+\r
+ /**\r
+ * Sets the simulation mode on in the IOManager.\r
+ */\r
+ // static public void setSimulation() {\r
+ // sm.setSimulation();\r
+ // }\r
+\r
+ /**\r
+ * Resets the simulation mode in the IOManager.\r
+ */\r
+ // static public void resetSimulation() {\r
+ // sm.resetSimulation();\r
+ // }\r
+\r
+ /**\r
+ * Saves the current IOManager command byte locally.\r
+ */\r
+ static public void setIOManagerCommand(byte[] cmd) {\r
+ // synchronized (obj1) {\r
+ currentCommand = cmd[0];\r
+ // }\r
+ // synchronized (obj) {\r
+ try {\r
+ // obj.notify();\r
+ } catch (IllegalMonitorStateException ie) {\r
+ System.out.println("IllegalMonitorStateException caught trying to notify");\r
+ }\r
+ // }\r
+ }\r
+\r
+ /**\r
+ * Sets debug messaging state: true - Activate debug messages false -\r
+ * deactivate debug messages\r
+ */\r
+ static public void setDebug(boolean debug) {\r
+ DEBUG = debug;\r
+ }\r
+\r
+ /**\r
+ * Runs the robot's main thread.\r
+ */\r
+ public static void main(String args[]) {\r
+\r
+ boolean active = true;\r
+ /**\r
+ * RealTime management of the robot behaviour based on sensors and commands\r
+ * input.\r
+ */\r
+\r
+ /**\r
+ * Set the low level PWM interface type, e.g. "rtsj" or "native" (ajile\r
+ * library-based).\r
+ */\r
+ pwmSelection = "rtsj";\r
+\r
+ System.out.println("PWM Selction is: " + pwmSelection);\r
+\r
+ pwmm = new PWMManager(pwmSelection);\r
+\r
+ // Pass in the PWMManager for callbacks.\r
+ // sm = new IOManager(pwmm);\r
+ // ram = new RemoteApplicationManager(pwmm);\r
+ strategyMgr = new StrategyMgr(pwmm);\r
+\r
+ /*\r
+ * Sets initial values for the speed and agility parameters. Speed and\r
+ * agility are arbitrary scale factors for the overall speed and speed of\r
+ * turns, respectively. These work with PWM via the native ajile libraries,\r
+ * but do not work well with the RTSJ implementation due to limits on the\r
+ * granularity of timing for the PWM pulse (quantization).\r
+ */\r
+ pwmm.setSpeedAgilityFactors(100, 100);\r
+\r
+ /*\r
+ * Robot's initial state is "ON" by default. Set this parameter to "OFF" if\r
+ * the robot is to be started over the TCP/IP link.\r
+ */\r
+ // issueCommand("OFF");\r
+\r
+ int count = 0;\r
+\r
+ while (active && count < 10000) {\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
+ processIOCommand();\r
+ // Nothing to do\r
+ } catch (Exception e) {\r
+ }\r
+ count++;\r
+ }\r
+ System.exit(0);\r
+ }\r
+\r
+}\r
--- /dev/null
+/*\r
+ * StragegyMgr.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
+ * StrategyMgr - an isolation class ment for programmers to modify and create\r
+ * thier own code for robot strategy.\r
+ * \r
+ * @author Michael Gesundheit\r
+ * @version 1.0\r
+ */\r
+public class StrategyMgr {\r
+\r
+ private PWMControl pwmControl;\r
+ private int zeroSpeed = 45;\r
+ private Random rand;\r
+ private boolean DEBUGL = true;\r
+ // private boolean DEBUGL = false;\r
+\r
+ // private boolean DEBUG = true;\r
+ private boolean DEBUG = false;\r
+\r
+ /**\r
+ * Constructor - Invoke communication to remote application thread\r
+ */\r
+ StrategyMgr(PWMManager pwmManager) {\r
+ this.pwmControl = pwmManager.getPWMControl();\r
+ rand = new Random();\r
+ }\r
+ \r
+ \r
+\r
+ void processSonars(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
+ if ((sonarSensors & 0x1f) == 0) {\r
+ // No sensor data may mean dead area between sensors.\r
+ // Continue with the last gesture until any sensor\r
+ // provide data.\r
+ if (DEBUG) {\r
+ System.out.println("sonar: NO SONARS!!!!!!!!");\r
+ }\r
+ } else if ((sonarSensors & (RobotMain.LF_RT_FRONT)) == (RobotMain.LF_RT_FRONT)) {\r
+ if (DEBUG) {\r
+ System.out.println("sonar: straight");\r
+ }\r
+ straight();\r
+ } else if ((sonarSensors & RobotMain.LF_FRONT) == RobotMain.LF_FRONT) {\r
+ if (DEBUG) {\r
+ System.out.println("sonar: bearLeft");\r
+ }\r
+ bearLeft();\r
+ } else if ((sonarSensors & RobotMain.RT_FRONT) == RobotMain.RT_FRONT) {\r
+ if (DEBUG) {\r
+ System.out.println("sonar: bearRight");\r
+ }\r
+ bearRight();\r
+ } else if ((sonarSensors & RobotMain.RT_SIDE) == RobotMain.RT_SIDE) {\r
+ if (DEBUG) {\r
+ System.out.println("sonar: right");\r
+ }\r
+ spinRight();\r
+ } else if ((sonarSensors & RobotMain.LF_SIDE) == RobotMain.LF_SIDE) {\r
+ if (DEBUG) {\r
+ System.out.println("sonar: left");\r
+ }\r
+ spinLeft();\r
+ } else if ((sonarSensors & RobotMain.BK_SIDE) == RobotMain.BK_SIDE) {\r
+ if (DEBUG) {\r
+ System.out.println("sonar: spin180");\r
+ }\r
+ spin180();\r
+ }\r
+ }\r
+\r
+ void processLineSensors(byte lineSensorsMask) {\r
+ while ((lineSensorsMask & RobotMain.LS_ALL) != 0) {\r
+ if ((lineSensorsMask & RobotMain.LS_ALL) == RobotMain.LS_ALL) {\r
+ if (DEBUGL)\r
+ System.out.println("Line Sensors - ALL");\r
+ stop();\r
+ } else if ((lineSensorsMask & RobotMain.LS_LEFT_RIGHT) == RobotMain.LS_LEFT_RIGHT) {\r
+ if (DEBUGL)\r
+ System.out.println("Line Sensors - Left & Right");\r
+ back();\r
+ try {\r
+ // Thread.sleep(1000);\r
+ } catch (InterruptedException ie) {\r
+ }\r
+ spin180();\r
+ try {\r
+ // Thread.sleep(1000);\r
+ } catch (InterruptedException ie) {\r
+ }\r
+ } else if ((lineSensorsMask & RobotMain.LS_LEFT_REAR) == RobotMain.LS_LEFT_REAR) {\r
+ if (DEBUGL)\r
+ System.out.println("Line Sensors - Left & Rear");\r
+ bearRight();\r
+ try {\r
+ // Thread.sleep(1000);\r
+ } catch (InterruptedException ie) {\r
+ }\r
+ } else if ((lineSensorsMask & RobotMain.LS_RIGHT_REAR) == RobotMain.LS_RIGHT_REAR) {\r
+ if (DEBUGL)\r
+ System.out.println("Line Sensors - Right & Rear");\r
+ bearLeft();\r
+ try {\r
+ // Thread.sleep(1000);\r
+ } catch (InterruptedException ie) {\r
+ }\r
+ } else if ((lineSensorsMask & RobotMain.LS_LEFT) == RobotMain.LS_LEFT) {\r
+ if (DEBUGL)\r
+ System.out.println("Line Sensors - Left");\r
+ backBearRight();\r
+ try {\r
+ // Thread.sleep(1000);\r
+ } catch (InterruptedException ie) {\r
+ }\r
+ } else if ((lineSensorsMask & RobotMain.LS_RIGHT) == RobotMain.LS_RIGHT) {\r
+ if (DEBUGL)\r
+ System.out.println("Line Sensors - Right");\r
+ backBearLeft();\r
+ try {\r
+ // Thread.sleep(1000);\r
+ } catch (InterruptedException ie) {\r
+ }\r
+ } else if ((lineSensorsMask & RobotMain.LS_REAR) == RobotMain.LS_REAR) {\r
+ if (DEBUGL)\r
+ System.out.println("Line Sensors - Rear");\r
+ straight();\r
+ try {\r
+ // Thread.sleep(1000);\r
+ } catch (InterruptedException ie) {\r
+ }\r
+ }\r
+\r
+ // TODO\r
+ // lineSensorsMask = sm.getLineSensorsState();\r
+\r
+ }// while loop\r
+ }\r
+\r
+ public void stop() {\r
+ if (DEBUG)\r
+ System.out.println("StrageyMgr: stop....");\r
+ pwmControl.setSpeedLeft(zeroSpeed);\r
+ pwmControl.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
+ }\r
+\r
+ public void straight() {\r
+ if (DEBUG)\r
+ System.out.println("StrategyMgr: strait....");\r
+ pwmControl.setSpeedLeft(100);\r
+ pwmControl.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
+ }\r
+\r
+ public void spinLeft() {\r
+ if (DEBUG)\r
+ System.out.println("StrategyMgr: spinLeft....");\r
+ pwmControl.setSpeedSpinLeft(0);\r
+ pwmControl.setSpeedSpinRight(100);\r
+ }\r
+\r
+ public void spin180() {\r
+ 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
+ } else {\r
+ pwmControl.setSpeedSpinLeft(100);\r
+ pwmControl.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
+ }\r
+\r
+ public void left() {\r
+ if (DEBUG)\r
+ System.out.println("StrategyMgr: left....");\r
+ pwmControl.setSpeedLeft(zeroSpeed);\r
+ pwmControl.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
+ }\r
+\r
+ public void bearLeft() {\r
+ if (DEBUG)\r
+ System.out.println("StrategyMgr: bearLeft....");\r
+ pwmControl.setSpeedTurnLeft(60);\r
+ pwmControl.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
+ }\r
+\r
+ public void backBearLeft() {\r
+ if (DEBUG)\r
+ System.out.println("StrategyMgr: backBearLeft....");\r
+ pwmControl.setSpeedLeft(30);\r
+ pwmControl.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
+ }\r
+}\r
--- /dev/null
+public class TestSensorInput {
+
+ FileInputStream inputFile;
+
+ public void init() {
+ inputFile = new FileInputStream("input.dat");
+ }
+
+ public static byte getCommand() {
+ // return Byte.parseInt(inputFile.readLine());
+ return 0;
+ }
+
+}
projects / IRC.git / commitdiff