import edu.wpi.first.wpilibj.command.PIDSubsystem;
public class DriveTrain extends PIDSubsystem {
+ private static double kp = 0.013, ki = 0.000015, kd = -0.002;
+ private static double gp = 0.018, gi = 0.000015, gd = 0;
private static double pidOutput = 0;
private static double encoderTolerance = 8.0, gyroTolerance = 5.0;
private int DRIVE_MODE = 1;
// Drivetrain specific constants that relate to the PID controllers
private final static double Kp = 1.0, Ki = 0.0,
Kd = 0.0 * (OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
- / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
+ / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
public DriveTrain() {
super(kp, ki, kd);
rightEncoder.reset();
}
- public Value getLeftGearPistonValue() {
- return leftGearPiston.get();
- }
-
- public Value getRightGearPistonValue() {
- return rightGearPiston.get();
- }
-
public double getRightSpeed() {
return rightEncoder.getRate(); // in inches per second
}
rearRight.set(right);
}
- private static double kp = 0.013, ki = 0.000015, kd = -0.002;
- private static double gp = 0.018, gi = 0.000015, gd = 0;
+ public Value getLeftGearPistonValue() {
+ return leftGearPiston.get();
+ }
+
+ public Value getRightGearPistonValue() {
+ return rightGearPiston.get();
+ }
+
+ public void setHighGear() {
+ changeGear(Constants.DriveTrain.HIGH_GEAR);
+ }
+
+ public void setLowGear() {
+ changeGear(Constants.DriveTrain.LOW_GEAR);
+ }
+
+ public void changeGear(DoubleSolenoid.Value gear) {
+ leftGearPiston.set(gear);
+ rightGearPiston.set(gear);
+ }
}