001 /*----------------------------------------------------------------------------*/
002 /* Copyright (c) FIRST 2008-2012. All Rights Reserved. */
003 /* Open Source Software - may be modified and shared by FRC teams. The code */
004 /* must be accompanied by the FIRST BSD license file in the root directory of */
005 /* the project. */
006 /*----------------------------------------------------------------------------*/
007 package edu.wpi.first.wpilibj;
008
009 import com.sun.squawk.util.MathUtils;
010 import edu.wpi.first.wpilibj.can.CANNotInitializedException;
011 import edu.wpi.first.wpilibj.can.CANTimeoutException;
012 import edu.wpi.first.wpilibj.communication.UsageReporting;
013 import edu.wpi.first.wpilibj.parsing.IUtility;
014
015 /**
016 * Utility class for handling Robot drive based on a definition of the motor configuration.
017 * The robot drive class handles basic driving for a robot. Currently, 2 and 4 motor standard
018 * drive trains are supported. In the future other drive types like swerve and meccanum might
019 * be implemented. Motor channel numbers are passed supplied on creation of the class. Those are
020 * used for either the drive function (intended for hand created drive code, such as autonomous)
021 * or with the Tank/Arcade functions intended to be used for Operator Control driving.
022 */
023 public class RobotDrive implements MotorSafety, IUtility {
024
025 protected MotorSafetyHelper m_safetyHelper;
026
027 /**
028 * The location of a motor on the robot for the purpose of driving
029 */
030 public static class MotorType {
031
032 /**
033 * The integer value representing this enumeration
034 */
035 public final int value;
036 static final int kFrontLeft_val = 0;
037 static final int kFrontRight_val = 1;
038 static final int kRearLeft_val = 2;
039 static final int kRearRight_val = 3;
040 /**
041 * motortype: front left
042 */
043 public static final MotorType kFrontLeft = new MotorType(kFrontLeft_val);
044 /**
045 * motortype: front right
046 */
047 public static final MotorType kFrontRight = new MotorType(kFrontRight_val);
048 /**
049 * motortype: rear left
050 */
051 public static final MotorType kRearLeft = new MotorType(kRearLeft_val);
052 /**
053 * motortype: rear right
054 */
055 public static final MotorType kRearRight = new MotorType(kRearRight_val);
056
057 private MotorType(int value) {
058 this.value = value;
059 }
060 }
061 public static final double kDefaultExpirationTime = 0.1;
062 public static final double kDefaultSensitivity = 0.5;
063 public static final double kDefaultMaxOutput = 1.0;
064 protected static final int kMaxNumberOfMotors = 4;
065 protected final int m_invertedMotors[] = new int[4];
066 protected double m_sensitivity;
067 protected double m_maxOutput;
068 protected SpeedController m_frontLeftMotor;
069 protected SpeedController m_frontRightMotor;
070 protected SpeedController m_rearLeftMotor;
071 protected SpeedController m_rearRightMotor;
072 protected boolean m_allocatedSpeedControllers;
073 protected boolean m_isCANInitialized = true;
074 protected static boolean kArcadeRatioCurve_Reported = false;
075 protected static boolean kTank_Reported = false;
076 protected static boolean kArcadeStandard_Reported = false;
077 protected static boolean kMecanumCartesian_Reported = false;
078 protected static boolean kMecanumPolar_Reported = false;
079
080 /** Constructor for RobotDrive with 2 motors specified with channel numbers.
081 * Set up parameters for a two wheel drive system where the
082 * left and right motor pwm channels are specified in the call.
083 * This call assumes Jaguars for controlling the motors.
084 * @param leftMotorChannel The PWM channel number on the default digital module that drives the left motor.
085 * @param rightMotorChannel The PWM channel number on the default digital module that drives the right motor.
086 */
087 public RobotDrive(final int leftMotorChannel, final int rightMotorChannel) {
088 m_sensitivity = kDefaultSensitivity;
089 m_maxOutput = kDefaultMaxOutput;
090 m_frontLeftMotor = null;
091 m_rearLeftMotor = new Jaguar(leftMotorChannel);
092 m_frontRightMotor = null;
093 m_rearRightMotor = new Jaguar(rightMotorChannel);
094 for (int i = 0; i < kMaxNumberOfMotors; i++) {
095 m_invertedMotors[i] = 1;
096 }
097 m_allocatedSpeedControllers = true;
098 setupMotorSafety();
099 drive(0, 0);
100 }
101
102 /**
103 * Constructor for RobotDrive with 4 motors specified with channel numbers.
104 * Set up parameters for a four wheel drive system where all four motor
105 * pwm channels are specified in the call.
106 * This call assumes Jaguars for controlling the motors.
107 * @param frontLeftMotor Front left motor channel number on the default digital module
108 * @param rearLeftMotor Rear Left motor channel number on the default digital module
109 * @param frontRightMotor Front right motor channel number on the default digital module
110 * @param rearRightMotor Rear Right motor channel number on the default digital module
111 */
112 public RobotDrive(final int frontLeftMotor, final int rearLeftMotor,
113 final int frontRightMotor, final int rearRightMotor) {
114 m_sensitivity = kDefaultSensitivity;
115 m_maxOutput = kDefaultMaxOutput;
116 m_rearLeftMotor = new Jaguar(rearLeftMotor);
117 m_rearRightMotor = new Jaguar(rearRightMotor);
118 m_frontLeftMotor = new Jaguar(frontLeftMotor);
119 m_frontRightMotor = new Jaguar(frontRightMotor);
120 for (int i = 0; i < kMaxNumberOfMotors; i++) {
121 m_invertedMotors[i] = 1;
122 }
123 m_allocatedSpeedControllers = true;
124 setupMotorSafety();
125 drive(0, 0);
126 }
127
128 /**
129 * Constructor for RobotDrive with 2 motors specified as SpeedController objects.
130 * The SpeedController version of the constructor enables programs to use the RobotDrive classes with
131 * subclasses of the SpeedController objects, for example, versions with ramping or reshaping of
132 * the curve to suit motor bias or dead-band elimination.
133 * @param leftMotor The left SpeedController object used to drive the robot.
134 * @param rightMotor the right SpeedController object used to drive the robot.
135 */
136 public RobotDrive(SpeedController leftMotor, SpeedController rightMotor) {
137 if (leftMotor == null || rightMotor == null) {
138 m_rearLeftMotor = m_rearRightMotor = null;
139 throw new NullPointerException("Null motor provided");
140 }
141 m_frontLeftMotor = null;
142 m_rearLeftMotor = leftMotor;
143 m_frontRightMotor = null;
144 m_rearRightMotor = rightMotor;
145 m_sensitivity = kDefaultSensitivity;
146 m_maxOutput = kDefaultMaxOutput;
147 for (int i = 0; i < kMaxNumberOfMotors; i++) {
148 m_invertedMotors[i] = 1;
149 }
150 m_allocatedSpeedControllers = false;
151 setupMotorSafety();
152 drive(0, 0);
153 }
154
155 /**
156 * Constructor for RobotDrive with 4 motors specified as SpeedController objects.
157 * Speed controller input version of RobotDrive (see previous comments).
158 * @param rearLeftMotor The back left SpeedController object used to drive the robot.
159 * @param frontLeftMotor The front left SpeedController object used to drive the robot
160 * @param rearRightMotor The back right SpeedController object used to drive the robot.
161 * @param frontRightMotor The front right SpeedController object used to drive the robot.
162 */
163 public RobotDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
164 SpeedController frontRightMotor, SpeedController rearRightMotor) {
165 if (frontLeftMotor == null || rearLeftMotor == null || frontRightMotor == null || rearRightMotor == null) {
166 m_frontLeftMotor = m_rearLeftMotor = m_frontRightMotor = m_rearRightMotor = null;
167 throw new NullPointerException("Null motor provided");
168 }
169 m_frontLeftMotor = frontLeftMotor;
170 m_rearLeftMotor = rearLeftMotor;
171 m_frontRightMotor = frontRightMotor;
172 m_rearRightMotor = rearRightMotor;
173 m_sensitivity = kDefaultSensitivity;
174 m_maxOutput = kDefaultMaxOutput;
175 for (int i = 0; i < kMaxNumberOfMotors; i++) {
176 m_invertedMotors[i] = 1;
177 }
178 m_allocatedSpeedControllers = false;
179 setupMotorSafety();
180 drive(0, 0);
181 }
182
183 /**
184 * Drive the motors at "speed" and "curve".
185 *
186 * The speed and curve are -1.0 to +1.0 values where 0.0 represents stopped and
187 * not turning. The algorithm for adding in the direction attempts to provide a constant
188 * turn radius for differing speeds.
189 *
190 * This function will most likely be used in an autonomous routine.
191 *
192 * @param outputMagnitude The forward component of the output magnitude to send to the motors.
193 * @param curve The rate of turn, constant for different forward speeds.
194 */
195 public void drive(double outputMagnitude, double curve) {
196 double leftOutput, rightOutput;
197
198 if(!kArcadeRatioCurve_Reported){
199 UsageReporting.report(UsageReporting.kResourceType_RobotDrive, getNumMotors(), UsageReporting.kRobotDrive_ArcadeRatioCurve);
200 kArcadeRatioCurve_Reported = true;
201 }
202 if (curve < 0) {
203 double value = MathUtils.log(-curve);
204 double ratio = (value - m_sensitivity) / (value + m_sensitivity);
205 if (ratio == 0) {
206 ratio = .0000000001;
207 }
208 leftOutput = outputMagnitude / ratio;
209 rightOutput = outputMagnitude;
210 } else if (curve > 0) {
211 double value = MathUtils.log(curve);
212 double ratio = (value - m_sensitivity) / (value + m_sensitivity);
213 if (ratio == 0) {
214 ratio = .0000000001;
215 }
216 leftOutput = outputMagnitude;
217 rightOutput = outputMagnitude / ratio;
218 } else {
219 leftOutput = outputMagnitude;
220 rightOutput = outputMagnitude;
221 }
222 setLeftRightMotorOutputs(leftOutput, rightOutput);
223 }
224
225 /**
226 * Provide tank steering using the stored robot configuration.
227 * drive the robot using two joystick inputs. The Y-axis will be selected from
228 * each Joystick object.
229 * @param leftStick The joystick to control the left side of the robot.
230 * @param rightStick The joystick to control the right side of the robot.
231 */
232 public void tankDrive(GenericHID leftStick, GenericHID rightStick) {
233 if (leftStick == null || rightStick == null) {
234 throw new NullPointerException("Null HID provided");
235 }
236 tankDrive(leftStick.getY(), rightStick.getY(), true);
237 }
238
239 /**
240 * Provide tank steering using the stored robot configuration.
241 * drive the robot using two joystick inputs. The Y-axis will be selected from
242 * each Joystick object.
243 * @param leftStick The joystick to control the left side of the robot.
244 * @param rightStick The joystick to control the right side of the robot.
245 * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
246 */
247 public void tankDrive(GenericHID leftStick, GenericHID rightStick, boolean squaredInputs) {
248 if (leftStick == null || rightStick == null) {
249 throw new NullPointerException("Null HID provided");
250 }
251 tankDrive(leftStick.getY(), rightStick.getY(), squaredInputs);
252 }
253
254 /**
255 * Provide tank steering using the stored robot configuration.
256 * This function lets you pick the axis to be used on each Joystick object for the left
257 * and right sides of the robot.
258 * @param leftStick The Joystick object to use for the left side of the robot.
259 * @param leftAxis The axis to select on the left side Joystick object.
260 * @param rightStick The Joystick object to use for the right side of the robot.
261 * @param rightAxis The axis to select on the right side Joystick object.
262 */
263 public void tankDrive(GenericHID leftStick, final int leftAxis,
264 GenericHID rightStick, final int rightAxis) {
265 if (leftStick == null || rightStick == null) {
266 throw new NullPointerException("Null HID provided");
267 }
268 tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), true);
269 }
270
271 /**
272 * Provide tank steering using the stored robot configuration.
273 * This function lets you pick the axis to be used on each Joystick object for the left
274 * and right sides of the robot.
275 * @param leftStick The Joystick object to use for the left side of the robot.
276 * @param leftAxis The axis to select on the left side Joystick object.
277 * @param rightStick The Joystick object to use for the right side of the robot.
278 * @param rightAxis The axis to select on the right side Joystick object.
279 * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
280 */
281 public void tankDrive(GenericHID leftStick, final int leftAxis,
282 GenericHID rightStick, final int rightAxis, boolean squaredInputs) {
283 if (leftStick == null || rightStick == null) {
284 throw new NullPointerException("Null HID provided");
285 }
286 tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), squaredInputs);
287 }
288
289 /**
290 * Provide tank steering using the stored robot configuration.
291 * This function lets you directly provide joystick values from any source.
292 * @param leftValue The value of the left stick.
293 * @param rightValue The value of the right stick.
294 * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
295 */
296 public void tankDrive(double leftValue, double rightValue, boolean squaredInputs) {
297
298 if(!kTank_Reported){
299 UsageReporting.report(UsageReporting.kResourceType_RobotDrive, getNumMotors(), UsageReporting.kRobotDrive_Tank);
300 kTank_Reported = true;
301 }
302
303 // square the inputs (while preserving the sign) to increase fine control while permitting full power
304 leftValue = limit(leftValue);
305 rightValue = limit(rightValue);
306 if(squaredInputs) {
307 if (leftValue >= 0.0) {
308 leftValue = (leftValue * leftValue);
309 } else {
310 leftValue = -(leftValue * leftValue);
311 }
312 if (rightValue >= 0.0) {
313 rightValue = (rightValue * rightValue);
314 } else {
315 rightValue = -(rightValue * rightValue);
316 }
317 }
318 setLeftRightMotorOutputs(leftValue, rightValue);
319 }
320
321 /**
322 * Provide tank steering using the stored robot configuration.
323 * This function lets you directly provide joystick values from any source.
324 * @param leftValue The value of the left stick.
325 * @param rightValue The value of the right stick.
326 */
327 public void tankDrive(double leftValue, double rightValue) {
328 tankDrive(leftValue, rightValue, true);
329 }
330
331 /**
332 * Arcade drive implements single stick driving.
333 * Given a single Joystick, the class assumes the Y axis for the move value and the X axis
334 * for the rotate value.
335 * (Should add more information here regarding the way that arcade drive works.)
336 * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
337 * for forwards/backwards and the X-axis will be selected for rotation rate.
338 * @param squaredInputs If true, the sensitivity will be decreased for small values
339 */
340 public void arcadeDrive(GenericHID stick, boolean squaredInputs) {
341 // simply call the full-featured arcadeDrive with the appropriate values
342 arcadeDrive(stick.getY(), stick.getX(), squaredInputs);
343 }
344
345 /**
346 * Arcade drive implements single stick driving.
347 * Given a single Joystick, the class assumes the Y axis for the move value and the X axis
348 * for the rotate value.
349 * (Should add more information here regarding the way that arcade drive works.)
350 * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
351 * for forwards/backwards and the X-axis will be selected for rotation rate.
352 */
353 public void arcadeDrive(GenericHID stick) {
354 this.arcadeDrive(stick, true);
355 }
356
357 /**
358 * Arcade drive implements single stick driving.
359 * Given two joystick instances and two axis, compute the values to send to either two
360 * or four motors.
361 * @param moveStick The Joystick object that represents the forward/backward direction
362 * @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically Y_AXIS)
363 * @param rotateStick The Joystick object that represents the rotation value
364 * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
365 * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
366 */
367 public void arcadeDrive(GenericHID moveStick, final int moveAxis,
368 GenericHID rotateStick, final int rotateAxis,
369 boolean squaredInputs) {
370 double moveValue = moveStick.getRawAxis(moveAxis);
371 double rotateValue = rotateStick.getRawAxis(rotateAxis);
372
373 arcadeDrive(moveValue, rotateValue, squaredInputs);
374 }
375
376 /**
377 * Arcade drive implements single stick driving.
378 * Given two joystick instances and two axis, compute the values to send to either two
379 * or four motors.
380 * @param moveStick The Joystick object that represents the forward/backward direction
381 * @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically Y_AXIS)
382 * @param rotateStick The Joystick object that represents the rotation value
383 * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
384 */
385 public void arcadeDrive(GenericHID moveStick, final int moveAxis,
386 GenericHID rotateStick, final int rotateAxis) {
387 this.arcadeDrive(moveStick, moveAxis, rotateStick, rotateAxis, true);
388 }
389
390 /**
391 * Arcade drive implements single stick driving.
392 * This function lets you directly provide joystick values from any source.
393 * @param moveValue The value to use for forwards/backwards
394 * @param rotateValue The value to use for the rotate right/left
395 * @param squaredInputs If set, decreases the sensitivity at low speeds
396 */
397 public void arcadeDrive(double moveValue, double rotateValue, boolean squaredInputs) {
398 // local variables to hold the computed PWM values for the motors
399 if(!kArcadeStandard_Reported){
400 UsageReporting.report(UsageReporting.kResourceType_RobotDrive, getNumMotors(), UsageReporting.kRobotDrive_ArcadeStandard);
401 kArcadeStandard_Reported = true;
402 }
403
404 double leftMotorSpeed;
405 double rightMotorSpeed;
406
407 moveValue = limit(moveValue);
408 rotateValue = limit(rotateValue);
409
410 if (squaredInputs) {
411 // square the inputs (while preserving the sign) to increase fine control while permitting full power
412 if (moveValue >= 0.0) {
413 moveValue = (moveValue * moveValue);
414 } else {
415 moveValue = -(moveValue * moveValue);
416 }
417 if (rotateValue >= 0.0) {
418 rotateValue = (rotateValue * rotateValue);
419 } else {
420 rotateValue = -(rotateValue * rotateValue);
421 }
422 }
423
424 if (moveValue > 0.0) {
425 if (rotateValue > 0.0) {
426 leftMotorSpeed = moveValue - rotateValue;
427 rightMotorSpeed = Math.max(moveValue, rotateValue);
428 } else {
429 leftMotorSpeed = Math.max(moveValue, -rotateValue);
430 rightMotorSpeed = moveValue + rotateValue;
431 }
432 } else {
433 if (rotateValue > 0.0) {
434 leftMotorSpeed = -Math.max(-moveValue, rotateValue);
435 rightMotorSpeed = moveValue + rotateValue;
436 } else {
437 leftMotorSpeed = moveValue - rotateValue;
438 rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
439 }
440 }
441
442 setLeftRightMotorOutputs(leftMotorSpeed, rightMotorSpeed);
443 }
444
445 /**
446 * Arcade drive implements single stick driving.
447 * This function lets you directly provide joystick values from any source.
448 * @param moveValue The value to use for fowards/backwards
449 * @param rotateValue The value to use for the rotate right/left
450 */
451 public void arcadeDrive(double moveValue, double rotateValue) {
452 this.arcadeDrive(moveValue, rotateValue, true);
453 }
454
455 /**
456 * Drive method for Mecanum wheeled robots.
457 *
458 * A method for driving with Mecanum wheeled robots. There are 4 wheels
459 * on the robot, arranged so that the front and back wheels are toed in 45 degrees.
460 * When looking at the wheels from the top, the roller axles should form an X across the robot.
461 *
462 * This is designed to be directly driven by joystick axes.
463 *
464 * @param x The speed that the robot should drive in the X direction. [-1.0..1.0]
465 * @param y The speed that the robot should drive in the Y direction.
466 * This input is inverted to match the forward == -1.0 that joysticks produce. [-1.0..1.0]
467 * @param rotation The rate of rotation for the robot that is completely independent of
468 * the translation. [-1.0..1.0]
469 * @param gyroAngle The current angle reading from the gyro. Use this to implement field-oriented controls.
470 */
471 public void mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle)
472 {
473 if(!kMecanumCartesian_Reported){
474 UsageReporting.report(UsageReporting.kResourceType_RobotDrive, getNumMotors(), UsageReporting.kRobotDrive_MecanumCartesian);
475 kMecanumCartesian_Reported = true;
476 }
477 double xIn = x;
478 double yIn = y;
479 // Negate y for the joystick.
480 yIn = -yIn;
481 // Compenstate for gyro angle.
482 double rotated[] = rotateVector(xIn, yIn, gyroAngle);
483 xIn = rotated[0];
484 yIn = rotated[1];
485
486 double wheelSpeeds[] = new double[kMaxNumberOfMotors];
487 wheelSpeeds[MotorType.kFrontLeft_val] = xIn + yIn + rotation;
488 wheelSpeeds[MotorType.kFrontRight_val] = -xIn + yIn - rotation;
489 wheelSpeeds[MotorType.kRearLeft_val] = -xIn + yIn + rotation;
490 wheelSpeeds[MotorType.kRearRight_val] = xIn + yIn - rotation;
491
492 normalize(wheelSpeeds);
493
494 byte syncGroup = (byte)0x80;
495
496 m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
497 m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
498 m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);
499 m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);
500
501 if (m_isCANInitialized) {
502 try {
503 CANJaguar.updateSyncGroup(syncGroup);
504 } catch (CANNotInitializedException e) {
505 m_isCANInitialized = false;
506 } catch (CANTimeoutException e) {}
507 }
508
509 if (m_safetyHelper != null) m_safetyHelper.feed();
510 }
511
512 /**
513 * Drive method for Mecanum wheeled robots.
514 *
515 * A method for driving with Mecanum wheeled robots. There are 4 wheels
516 * on the robot, arranged so that the front and back wheels are toed in 45 degrees.
517 * When looking at the wheels from the top, the roller axles should form an X across the robot.
518 *
519 * @param magnitude The speed that the robot should drive in a given direction.
520 * @param direction The direction the robot should drive in degrees. The direction and maginitute are
521 * independent of the rotation rate.
522 * @param rotation The rate of rotation for the robot that is completely independent of
523 * the magnitute or direction. [-1.0..1.0]
524 */
525 public void mecanumDrive_Polar(double magnitude, double direction, double rotation) {
526 if(!kMecanumPolar_Reported){
527 UsageReporting.report(UsageReporting.kResourceType_RobotDrive, getNumMotors(), UsageReporting.kRobotDrive_MecanumPolar);
528 kMecanumPolar_Reported = true;
529 }
530 double frontLeftSpeed, rearLeftSpeed, frontRightSpeed, rearRightSpeed;
531 // Normalized for full power along the Cartesian axes.
532 magnitude = limit(magnitude) * Math.sqrt(2.0);
533 // The rollers are at 45 degree angles.
534 double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
535 double cosD = Math.cos(dirInRad);
536 double sinD = Math.sin(dirInRad);
537
538 double wheelSpeeds[] = new double[kMaxNumberOfMotors];
539 wheelSpeeds[MotorType.kFrontLeft_val] = (sinD * magnitude + rotation);
540 wheelSpeeds[MotorType.kFrontRight_val] = (cosD * magnitude - rotation);
541 wheelSpeeds[MotorType.kRearLeft_val] = (cosD * magnitude + rotation);
542 wheelSpeeds[MotorType.kRearRight_val] = (sinD * magnitude - rotation);
543
544 normalize(wheelSpeeds);
545
546 byte syncGroup = (byte)0x80;
547
548 m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
549 m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
550 m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);
551 m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);
552
553 if (m_isCANInitialized) {
554 try {
555 CANJaguar.updateSyncGroup(syncGroup);
556 } catch (CANNotInitializedException e) {
557 m_isCANInitialized = false;
558 } catch (CANTimeoutException e) {}
559 }
560
561 if (m_safetyHelper != null) m_safetyHelper.feed();
562 }
563
564 /**
565 * Holonomic Drive method for Mecanum wheeled robots.
566 *
567 * This is an alias to mecanumDrive_Polar() for backward compatability
568 *
569 * @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
570 * @param direction The direction the robot should drive. The direction and maginitute are
571 * independent of the rotation rate.
572 * @param rotation The rate of rotation for the robot that is completely independent of
573 * the magnitute or direction. [-1.0..1.0]
574 */
575 void holonomicDrive(float magnitude, float direction, float rotation) {
576 mecanumDrive_Polar(magnitude, direction, rotation);
577 }
578
579 /** Set the speed of the right and left motors.
580 * This is used once an appropriate drive setup function is called such as
581 * twoWheelDrive(). The motors are set to "leftSpeed" and "rightSpeed"
582 * and includes flipping the direction of one side for opposing motors.
583 * @param leftOutput The speed to send to the left side of the robot.
584 * @param rightOutput The speed to send to the right side of the robot.
585 */
586 public void setLeftRightMotorOutputs(double leftOutput, double rightOutput) {
587 if (m_rearLeftMotor == null || m_rearRightMotor == null) {
588 throw new NullPointerException("Null motor provided");
589 }
590
591 byte syncGroup = (byte)0x80;
592
593 if (m_frontLeftMotor != null) {
594 m_frontLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
595 }
596 m_rearLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);
597
598 if (m_frontRightMotor != null) {
599 m_frontRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
600 }
601 m_rearRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);
602
603 if (m_isCANInitialized) {
604 try {
605 CANJaguar.updateSyncGroup(syncGroup);
606 } catch (CANNotInitializedException e) {
607 m_isCANInitialized = false;
608 } catch (CANTimeoutException e) {}
609 }
610
611 if (m_safetyHelper != null) m_safetyHelper.feed();
612 }
613
614 /**
615 * Limit motor values to the -1.0 to +1.0 range.
616 */
617 protected static double limit(double num) {
618 if (num > 1.0) {
619 return 1.0;
620 }
621 if (num < -1.0) {
622 return -1.0;
623 }
624 return num;
625 }
626
627 /**
628 * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
629 */
630 protected static void normalize(double wheelSpeeds[]) {
631 double maxMagnitude = Math.abs(wheelSpeeds[0]);
632 int i;
633 for (i=1; i<kMaxNumberOfMotors; i++) {
634 double temp = Math.abs(wheelSpeeds[i]);
635 if (maxMagnitude < temp) maxMagnitude = temp;
636 }
637 if (maxMagnitude > 1.0) {
638 for (i=0; i<kMaxNumberOfMotors; i++) {
639 wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
640 }
641 }
642 }
643
644 /**
645 * Rotate a vector in Cartesian space.
646 */
647 protected static double[] rotateVector(double x, double y, double angle) {
648 double cosA = Math.cos(angle * (3.14159 / 180.0));
649 double sinA = Math.sin(angle * (3.14159 / 180.0));
650 double out[] = new double[2];
651 out[0] = x * cosA - y * sinA;
652 out[1] = x * sinA + y * cosA;
653 return out;
654 }
655
656 /**
657 * Invert a motor direction.
658 * This is used when a motor should run in the opposite direction as the drive
659 * code would normally run it. Motors that are direct drive would be inverted, the
660 * drive code assumes that the motors are geared with one reversal.
661 * @param motor The motor index to invert.
662 * @param isInverted True if the motor should be inverted when operated.
663 */
664 public void setInvertedMotor(MotorType motor, boolean isInverted) {
665 m_invertedMotors[motor.value] = isInverted ? -1 : 1;
666 }
667
668 /**
669 * Set the turning sensitivity.
670 *
671 * This only impacts the drive() entry-point.
672 * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
673 */
674 public void setSensitivity(double sensitivity)
675 {
676 m_sensitivity = sensitivity;
677 }
678
679 /**
680 * Configure the scaling factor for using RobotDrive with motor controllers in a mode other than PercentVbus.
681 * @param maxOutput Multiplied with the output percentage computed by the drive functions.
682 */
683 public void setMaxOutput(double maxOutput)
684 {
685 m_maxOutput = maxOutput;
686 }
687
688
689 /**
690 * Free the speed controllers if they were allocated locally
691 */
692 public void free() {
693 if (m_allocatedSpeedControllers) {
694 if (m_frontLeftMotor != null) {
695 ((PWM) m_frontLeftMotor).free();
696 }
697 if (m_frontRightMotor != null) {
698 ((PWM) m_frontRightMotor).free();
699 }
700 if (m_rearLeftMotor != null) {
701 ((PWM) m_rearLeftMotor).free();
702 }
703 if (m_rearRightMotor != null) {
704 ((PWM) m_rearRightMotor).free();
705 }
706 }
707 }
708
709 public void setExpiration(double timeout) {
710 m_safetyHelper.setExpiration(timeout);
711 }
712
713 public double getExpiration() {
714 return m_safetyHelper.getExpiration();
715 }
716
717 public boolean isAlive() {
718 return m_safetyHelper.isAlive();
719 }
720
721 public boolean isSafetyEnabled() {
722 return m_safetyHelper.isSafetyEnabled();
723 }
724
725 public void setSafetyEnabled(boolean enabled) {
726 m_safetyHelper.setSafetyEnabled(enabled);
727 }
728
729 public String getDescription() {
730 return "Robot Drive";
731 }
732
733 public void stopMotor() {
734 if (m_frontLeftMotor != null) {
735 m_frontLeftMotor.set(0.0);
736 }
737 if (m_frontRightMotor != null) {
738 m_frontRightMotor.set(0.0);
739 }
740 if (m_rearLeftMotor != null) {
741 m_rearLeftMotor.set(0.0);
742 }
743 if (m_rearRightMotor != null) {
744 m_rearRightMotor.set(0.0);
745 }
746 }
747
748 private void setupMotorSafety() {
749 m_safetyHelper = new MotorSafetyHelper(this);
750 m_safetyHelper.setExpiration(kDefaultExpirationTime);
751 m_safetyHelper.setSafetyEnabled(true);
752 }
753
754 protected int getNumMotors()
755 {
756 int motors = 0;
757 if (m_frontLeftMotor != null) motors++;
758 if (m_frontRightMotor != null) motors++;
759 if (m_rearLeftMotor != null) motors++;
760 if (m_rearRightMotor != null) motors++;
761 return motors;
762 }
763 }