package org.usfirst.frc.team3501.robot.subsystems;
import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
import com.ctre.CANTalon;
import edu.wpi.first.wpilibj.command.Subsystem;
public class DriveTrain extends Subsystem {
+ public static final double WHEEL_DIAMETER = 6; // inches
+ public static final int ENCODER_PULSES_PER_REVOLUTION = 256;
+ public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI
+ / ENCODER_PULSES_PER_REVOLUTION;
+
private static DriveTrain driveTrain;
private final CANTalon frontLeft, frontRight, rearLeft, rearRight;
private final RobotDrive robotDrive;
// ENCODERS
leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
- Constants.DriveTrain.ENCODER_LEFT_B);
+ Constants.DriveTrain.ENCODER_LEFT_B, false, Encoder.EncodingType.k4X);
rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
- Constants.DriveTrain.ENCODER_RIGHT_B);
+ Constants.DriveTrain.ENCODER_RIGHT_B, false, Encoder.EncodingType.k4X);
- leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
- rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
+ leftEncoder.setDistancePerPulse(INCHES_PER_PULSE);
+ rightEncoder.setDistancePerPulse(INCHES_PER_PULSE);
// ROBOT DRIVE
robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
// DRIVE METHODS
public void setMotorValues(final double left, final double right) {
- robotDrive.tankDrive(left, right);
+ frontLeft.set(left);
+ rearLeft.set(left);
+
+ frontRight.set(-right);
+ rearRight.set(-right);
}
- public void joystickDrive(final double left, final double right) {
- robotDrive.tankDrive(left, right);
+ public void joystickDrive(final double thrust, final double twist) {
+ robotDrive.arcadeDrive(thrust, twist, true);
}
public void stop() {
@Override
protected void initDefaultCommand() {
+ setDefaultCommand(new JoystickDrive());
}
}