-
- public static DriveTrain getDriveTrain() {
- if (driveTrain == null)
- driveTrain = new DriveTrain();
- return driveTrain;
- }
-
- public void setMotorSpeeds(double left, double right) {
- robotDrive.tankDrive(left, right);
- }
-
- // ENCODER METHODS
-
- public double getLeftEncoder() {
- return leftEncoder.getDistance();
- }
-
- public double getRightEncoder() {
- return rightEncoder.getDistance();
- }
-
- public double getAvgEncoderDistance() {
- return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
- }
-
- public void resetEncoders() {
- leftEncoder.reset();
- rightEncoder.reset();
- }
-
- public double getLeftSpeed() {
- return leftEncoder.getRate();
- }
-
- public double getRightSpeed() {
- return rightEncoder.getRate();
- }
-
- public double getSpeed() {
- return (getLeftSpeed() + getRightSpeed()) / 2.0;
- }
-
- @Override
- protected void initDefaultCommand() {
- }
-
+ return driveTrain;
+ }
+
+ // DRIVE METHODS
+ public void setMotorValues(double left, double right) {
+ left = MathLib.restrictToRange(left, -1.0, 1.0);
+ right = MathLib.restrictToRange(right, -1.0, 1.0);
+
+ frontLeft.set(left);
+ rearLeft.set(left);
+
+ frontRight.set(-right);
+ rearRight.set(-right);
+ }
+
+ public void joystickDrive(final double thrust, final double twist) {
+ robotDrive.arcadeDrive(thrust, twist, true);
+ }
+
+ public void stop() {
+ setMotorValues(0, 0);
+ }
+
+ public double getLeftMotorVal() {
+ return (frontLeft.get() + rearLeft.get()) / 2;
+ }
+
+ public double getRightMotorVal() {
+ return (frontRight.get() + rearRight.get()) / 2;
+ }
+
+ // ENCODER METHODS
+
+ public double getLeftEncoderDistance() {
+ return leftEncoder.getDistance();
+ }
+
+ public double getRightEncoderDistance() {
+ return rightEncoder.getDistance();
+ }
+
+ public void printEncoderOutput() {
+ System.out.println("left: " + getLeftEncoderDistance());
+ System.out.println("right: " + getRightEncoderDistance());
+ // System.out.println(getAvgEncoderDistance());
+ }
+
+ public double getAvgEncoderDistance() {
+ return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
+ }
+
+ public void resetEncoders() {
+ leftEncoder.reset();
+ rightEncoder.reset();
+ }
+
+ public double getLeftSpeed() {
+ return leftEncoder.getRate();
+ }
+
+ public double getRightSpeed() {
+ return rightEncoder.getRate();
+ }
+
+ public double getSpeed() {
+ return (getLeftSpeed() + getRightSpeed()) / 2.0;
+ }
+
+ // ------Gyro------//
+ public double getAngle() {
+ return this.imu.getAngle();
+ }
+
+ public void resetGyro() {
+ this.imu.reset();
+ }
+
+ /*
+ * @return a value that is the current setpoint for the piston kReverse or
+ * KForward
+ */
+ public Value getLeftGearPistonValue() {
+ return leftGearPiston.get();
+ }
+
+ /*
+ * @return a value that is the current setpoint for the piston kReverse or
+ * KForward
+ */
+ public Value getRightGearPistonValue() {
+ return rightGearPiston.get();
+ }
+
+ /*
+ * Changes the ball shift gear assembly to high
+ */
+ public void setHighGear() {
+ changeGear(Constants.DriveTrain.HIGH_GEAR);
+ }
+
+ /*
+ * Changes the ball shift gear assembly to low
+ */
+ public void setLowGear() {
+ changeGear(Constants.DriveTrain.LOW_GEAR);
+ }
+
+ /*
+ * Changes the gear to a DoubleSolenoid.Value
+ */
+ private void changeGear(DoubleSolenoid.Value gear) {
+ System.out.println(gear);
+ leftGearPiston.set(gear);
+ rightGearPiston.set(gear);
+ }
+
+ @Override
+ protected void initDefaultCommand() {
+ setDefaultCommand(new JoystickDrive());
+ }