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() {
return frontLeft.get();
}
- public CANTalon getFrontLeft() {
- return frontLeft;
- }
-
- public CANTalon getFrontRight() {
- return frontRight;
- }
-
- public CANTalon getRearLeft() {
- return rearLeft;
- }
-
- public CANTalon getRearRight() {
- return rearRight;
- }
-
// ENCODER METHODS
- public double getLeftEncoder() {
+ public double getLeftEncoderDistance() {
return leftEncoder.getDistance();
}
- public double getRightEncoder() {
+ public double getRightEncoderDistance() {
return rightEncoder.getDistance();
}
@Override
protected void initDefaultCommand() {
+ setDefaultCommand(new JoystickDrive());
}
}