4 * Copyright (c) 1993-2002 Sun Microsystems, Inc. All Rights Reserved.
\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
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
21 * This class is motor controller specific. In our case the motor controller
\r
22 * require for 0 speed 1.5 ms out of 10-20ms cycle and a change between 1 to 2
\r
23 * seconds for max reverse and forward speed. These values: 1, 1.5, 2 and 20 ms
\r
24 * are thus this class specific
\r
26 * @author Michael Gesundheit
\r
30 public class PWMRtsj extends PWMControl {
\r
32 // RelativeTime zeroSpeedDuration;
\r
33 // RelativeTime timeUpDuration;
\r
34 // RelativeTime timeDownDuration;
\r
35 // RelativeTime cycleTime;
\r
37 int sleepUpDurationNano = 500000;
\r
38 long sleepUpDurationMilli = 1;
\r
39 int sleepDownDurationNano = 500000;
\r
40 long sleepDownDurationMilli = 18;
\r
44 private static final int GPIO_A_OER = 0x09002000;
\r
45 private static final int GPIO_A_OUT = GPIO_A_OER + 4;
\r
46 private static final int GPIO_A_IN = GPIO_A_OER + 8;
\r
49 * Constructor - Start a standard Java thread. The thread is suspended and
\r
50 * will wake up evey 18 ms. It will issue a 1-2ms pulse and suspends itself
\r
51 * again to 18ms. This is to have a total of 20ms or less yet it's the maxim
\r
52 * possible cycle so as to load the cpu as little as possible.
\r
54 public PWMRtsj(PWMManager pwmMan, int motor1bit, int motor2bit) {
\r
56 super(pwmMan, motor1bit, motor2bit); // This is papa
\r
58 motorLeftUpTime = 450000; // Nano seconds
\r
59 motorRightUpTime = 450000; // Nano seconds
\r
62 System.out.println("PWMRtsj: About to start RoboThread...");
\r
65 // t2 = new RoboThread();
\r
67 // t2.setPriority(10);
\r
70 // A poor's man ajustimg for the 0 speed which found
\r
71 // to be 450000 nano seconds.
\r
72 private int normalizeTime(int timePosition) {
\r
73 if ((timePosition <= 50) && (timePosition >= 44)) {
\r
76 return timePosition;
\r
80 * setSpeedSpin - Set speed for the spin case motor 1.
\r
83 * pulse width (time position)
\r
85 public void setSpeedSpinLeft(int timePosition) {
\r
86 /* Speed comes in as a number between 0 - 100 */
\r
87 /* It represents values between 1 to 2 ms */
\r
88 /* 100-input to get reverse polarity for this motor */
\r
89 /* since it's mounted in reverse direction to the other motor */
\r
91 System.out.println("setSpeedSpinLeft: input-> " + Integer.toString(timePosition));
\r
93 int timePos = normalizeTime(timePosition);
\r
95 int motorUpTime = (int) (timePos * agilityFactor * speedFactor);
\r
96 // System.out.println("Left Motor UpTime1: " +
\r
97 // Integer.toString(motorUpTime));
\r
98 // Since the right motor is hanging in reverse position
\r
99 // the direction should be opposit
\r
100 // Bug in wait. Can't take 0 nanoseconds
\r
101 if (motorUpTime == 0) {
\r
103 // System.out.println("returning....");
\r
105 } else if (motorUpTime == 1000000) {
\r
110 System.out.println("setSpeedSpinLeft: output-> = " + Integer.toString(motorUpTime));
\r
112 // synchronized (this) {
\r
113 /* Factor in the speed and agility factors */
\r
114 motorLeftUpTime = motorUpTime;
\r
119 * setSpeedSpinRight - Set speed for the spin case right motor.
\r
122 * pulse width (time position)
\r
124 public void setSpeedSpinRight(int timePosition) {
\r
125 /* Speed comes in as a number between 0 - 100 */
\r
126 /* It represents values between 1 to 2 ms */
\r
127 /* An input of 50 should result in 0 speed. */
\r
128 /* 100 should result in full speed forward */
\r
129 /* while 0 should result in full speed backwords */
\r
131 System.out.println("setSpeedSpinRight: input-> " + Integer.toString(timePosition));
\r
133 int timePos = normalizeTime(timePosition);
\r
135 int motorUpTime = (int) ((timePos) * agilityFactor * speedFactor);
\r
136 // Bug in wait. Cant take 0 nanoseconds
\r
137 if (motorUpTime == 0) {
\r
139 // System.out.println("returning....");
\r
141 } else if (motorUpTime == 1000000) {
\r
146 System.out.println("setSpeedSpinRight: output-> = " + Integer.toString(motorUpTime));
\r
148 // synchronized (this) {
\r
149 /* Factor in the speed and agility factors */
\r
150 motorRightUpTime = motorUpTime;
\r
155 * setSpeedTurnM1 - set speed considering agility factor for motor 1
\r
158 * pulse width (time position)
\r
160 public void setSpeedTurnLeft(int timePosition) {
\r
161 /* Speed comes in as a number between 0 - 100 */
\r
162 /* It represents values between 1 to 2 ms */
\r
163 /* 100-input to get reverse polarity for this motor */
\r
164 /* since it's mounted in reverse direction to the other motor */
\r
166 System.out.println("setSpeedTurnLeft: input-> " + Integer.toString(timePosition));
\r
168 int timePosLocal = normalizeTime(timePosition);
\r
170 (timePosLocal * 100 + ((100 - timePosLocal) * (100 - agilityFactor))) * speedFactor;
\r
171 if (motorUpTime == 0) {
\r
173 // System.out.println("returning....");
\r
175 } else if (motorUpTime == 1000000) {
\r
179 System.out.println("setSpeedTurnLeft: output-> = " + Integer.toString(motorUpTime));
\r
181 // synchronized (this) {
\r
182 /* Factor in the speed and agility factors */
\r
183 motorLeftUpTime = motorUpTime;
\r
188 * setSpeedTurnM1 - set speed considering agility factor for motor 2
\r
191 * pulse width (time position)
\r
193 public void setSpeedTurnRight(int timePosition) {
\r
194 /* Speed comes in as a number between 0 - 100 */
\r
195 /* It represents values between 1 to 2 ms */
\r
197 System.out.println("setSpeedTurnRight: input-> " + Integer.toString(timePosition));
\r
199 int timePos = normalizeTime(timePosition);
\r
200 int motorUpTime = ((timePos * 100) + ((100 - timePos) * (100 - agilityFactor))) * speedFactor;
\r
201 if (motorUpTime == 0) {
\r
203 // System.out.println("returning....");
\r
205 } else if (motorUpTime == 1000000) {
\r
210 System.out.println("setSpeedTurnRight: output-> " + Integer.toString(motorUpTime));
\r
212 // synchronized (this) {
\r
213 /* Factor in the speed and agility factors */
\r
214 motorRightUpTime = motorUpTime;
\r
219 * setSpeedLeft - speed control for motor 1.
\r
222 * pulse width (time position)
\r
224 public void setSpeedLeft(int timePosition) {
\r
225 /* Speed comes in as a number between 0 - 100 */
\r
226 /* It represents values between 1 to 2 ms */
\r
227 /* 100-input to get reverse polarity for this motor */
\r
228 /* since it's mounted in reverse direction to the other motor */
\r
230 System.out.println("setSpeedLeft: input-> " + Integer.toString(timePosition));
\r
232 int timePos = normalizeTime(timePosition);
\r
233 int motorUpTime = (int) (timePos * 100) * speedFactor;
\r
235 * System.out.println("motorUpTime = " + Integer.toString(motorUpTime) +
\r
236 * " timePos = " + Integer.toString((int)timePos) + " timePosition = " +
\r
237 * Integer.toString((int)timePosition) + " speedFactor = " +
\r
238 * Integer.toString(speedFactor));
\r
240 if (motorUpTime == 0) {
\r
242 // System.out.println("returning....");
\r
244 } else if (motorUpTime == 1000000) {
\r
248 System.out.println("setSpeedLeft: output-> " + Integer.toString(motorUpTime));
\r
250 // synchronized (this) {
\r
251 /* Factor in speedFactor */
\r
252 motorLeftUpTime = motorUpTime;
\r
257 * setSpeedRight - speed control for motor 1.
\r
260 * pulse width (time position)
\r
262 public void setSpeedRight(int timePosition) {
\r
264 System.out.println("setSpeedRight: input-> " + Integer.toString(timePosition));
\r
266 /* Speed comes in as a number between 0 - 100 */
\r
267 /* It represents values between 1 to 2 ms */
\r
268 int timePos = normalizeTime(timePosition);
\r
269 int motorUpTime = (int) (timePos * 100) * speedFactor;
\r
270 if (motorUpTime == 0) {
\r
272 // System.out.println("returning....");
\r
274 } else if (motorUpTime == 1000000) {
\r
278 System.out.println("setSpeedRight: output-> " + Integer.toString(motorUpTime));
\r
280 // synchronized (this) {
\r
281 /* Factor in speedFactor */
\r
282 motorRightUpTime = motorUpTime;
\r
286 public void setSpeedAgilityFactors(int speed, int agility) {
\r
287 // synchronized (this) {
\r
288 speedFactor = speed;
\r
289 agilityFactor = agility;
\r
293 public void setUrgentReverse() {
\r
294 // synchronized (this) {
\r
295 motorLeftUpTime = 1;
\r
296 motorRightUpTime = 1;
\r
300 public void setUrgentStraight() {
\r
301 // synchronized (this) {
\r
302 motorLeftUpTime = 99;
\r
303 motorRightUpTime = 99;
\r
307 public void justSync() {
\r
308 // synchronized (this) {
\r
309 motorLeftUpTime = motorLeftUpTime;
\r
310 motorRightUpTime = motorRightUpTime;
\r
315 * Control debug messageing. true - Activate debug messages false - deactivate
\r
318 public void setDebug(boolean debug) {
\r