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