import org.usfirst.frc.team3501.robot.Constants;
import edu.wpi.first.wpilibj.CANTalon;
+import edu.wpi.first.wpilibj.CANTalon.TalonControlMode;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.command.Subsystem;
public class DriveTrain extends Subsystem {
// Drivetrain related objects
private Encoder leftEncoder, rightEncoder;
private CANTalon frontLeft, frontRight, rearLeft, rearRight;
-
+ private PIDController frontLeftC, frontRightC, rearLeftC, rearRightC;
// Drivetrain specific constants that relate to the inches per pulse value for
// the encoders
private final static double WHEEL_DIAMETER = 6.0; // in inches
private final static double PULSES_PER_ROTATION = 256; // in pulses
private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches
private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches
-
public final static double INCHES_PER_PULSE = (((Math.PI)
* OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) / WHEEL_SPROCKET_DIAMETER)
* WHEEL_DIAMETER;
+ // Drivetrain specific constants that relate to the PID controllers
+ private final static double Kp = 1.0, Ki = 0.0, Kd = 0.0;
public DriveTrain() {
frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
+ this.configureTalons();
+
+ frontLeftC = new PIDController(Kp, Ki, Kd, frontLeft, frontLeft);
+ frontRightC = new PIDController(Kp, Ki, Kd, frontRight, frontRight);
+ rearLeftC = new PIDController(Kp, Ki, Kd, frontLeft, rearLeft);
+ rearRightC = new PIDController(Kp, Ki, Kd, frontRight, rearRight);
+ this.enablePIDControllers();
leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
protected void initDefaultCommand() {
}
+ public void configureTalons() {
+ frontLeft.changeControlMode(TalonControlMode.Speed);
+ frontRight.changeControlMode(TalonControlMode.Speed);
+ rearLeft.changeControlMode(TalonControlMode.Speed);
+ rearRight.changeControlMode(TalonControlMode.Speed);
+
+ frontLeft.configEncoderCodesPerRev(256);
+ frontRight.configEncoderCodesPerRev(256);
+ rearLeft.configEncoderCodesPerRev(256);
+ rearRight.configEncoderCodesPerRev(256);
+
+ frontLeft.enableControl();
+ frontRight.enableControl();
+ rearLeft.enableControl();
+ rearRight.enableControl();
+ }
+
+ public void enablePIDControllers() {
+ frontLeftC.enable();
+ frontRightC.enable();
+ rearLeftC.enable();
+ rearRightC.enable();
+ }
+
+ public void drive(double left, double right) {
+ frontLeftC.setSetpoint(left);
+ rearLeftC.setSetpoint(left);
+ frontRightC.setSetpoint(right);
+ rearRightC.setSetpoint(right);
+ }
+
public void resetEncoders() {
leftEncoder.reset();
rightEncoder.reset();
this.rearLeft.set(leftSpeed);
this.rearRight.set(-rightSpeed);
}
-
}