+
+ // ENCODERS
+ leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
+ Constants.DriveTrain.ENCODER_LEFT_B, false, Encoder.EncodingType.k4X);
+ rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
+ Constants.DriveTrain.ENCODER_RIGHT_B, false, Encoder.EncodingType.k4X);
+
+ leftEncoder.setDistancePerPulse(INCHES_PER_PULSE);
+ rightEncoder.setDistancePerPulse(INCHES_PER_PULSE);
+
+ // ROBOT DRIVE
+ robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
+
+ this.imu = new ADXRS450_Gyro(Constants.DriveTrain.GYRO_PORT);
+
+ // TODO: Not sure if MODULE_NUMBER should be the same for both
+ leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.PISTON_MODULE,
+ Constants.DriveTrain.LEFT_GEAR_PISTON_FORWARD,
+ Constants.DriveTrain.LEFT_GEAR_PISTON_REVERSE);
+ rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.PISTON_MODULE,
+ Constants.DriveTrain.RIGHT_GEAR_PISTON_FORWARD,
+ Constants.DriveTrain.RIGHT_GEAR_PISTON_REVERSE);
+ }
+
+ public static DriveTrain getDriveTrain() {
+ if (driveTrain == null) {
+ driveTrain = new DriveTrain();
+ }
+ 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;