import com.ctre.CANTalon;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
public class DriveTrain extends Subsystem {
- public static double driveP = 0.008, driveI = 0.001, driveD = -0.002;
- public static double defaultGyroP = 0.006, defaultGyroI = 0.00000,
- defaultGyroD = -0.000;
+ public static double driveP = 0.006, driveI = 0.001, driveD = -0.002;
+ public static double defaultGyroP = 0.004, defaultGyroI = 0.0013,
+ defaultGyroD = -0.005;
private double gyroZero = 0;
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;
-
+ public static final int MAINTAIN_CLIMBED_POSITION = 0;
+ public static final int TIME_TO_CLIMB_FOR = 0;
private static DriveTrain driveTrain;
+
private final CANTalon frontLeft, frontRight, rearLeft, rearRight;
private final RobotDrive robotDrive;
private final Encoder leftEncoder, rightEncoder;
+ private final DoubleSolenoid leftGearPiston, rightGearPiston;
private ADXRS450_Gyro imu;
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.MODULE_NUMBER,
+ Constants.DriveTrain.LEFT_GEAR_PISTON_FORWARD,
+ Constants.DriveTrain.LEFT_GEAR_PISTON_REVERSE);
+ rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_NUMBER,
+ Constants.DriveTrain.RIGHT_GEAR_PISTON_FORWARD,
+ Constants.DriveTrain.RIGHT_GEAR_PISTON_REVERSE);
}
public static DriveTrain getDriveTrain() {
// System.out.println("left: " + getLeftEncoderDistance());
// System.out.println("right: " + getRightEncoderDistance());
System.out.println(getAvgEncoderDistance());
+ System.out.println("left: " + getLeftEncoderDistance());
+ System.out.println("right: " + getRightEncoderDistance());
}
public double getAvgEncoderDistance() {
}
public void resetGyro() {
- this.gyroZero = this.getAngle();
+ this.imu.reset();
}
public double getZeroAngle() {
return this.gyroZero;
}
+ /*
+ * @return a value that is the current setpoint for the piston kReverse or
+ * KForward
+ */
+ public Value getLeftGearPistonValue() {
+ return leftGearPiston.get();
+ }
+
+ /*
+ * @return a value that is the current setpoint for the piston kReverse or
+ * KForward
+ */
+ public Value getRightGearPistonValue() {
+ return rightGearPiston.get();
+ }
+
+ /*
+ * Changes the ball shift gear assembly to high
+ */
+ public void setHighGear() {
+ changeGear(Constants.DriveTrain.HIGH_GEAR);
+ }
+
+ /*
+ * Changes the ball shift gear assembly to low
+ */
+ public void setLowGear() {
+ changeGear(Constants.DriveTrain.LOW_GEAR);
+ }
+
+ /*
+ * Changes the gear to a DoubleSolenoid.Value
+ */
+ private void changeGear(DoubleSolenoid.Value gear) {
+ leftGearPiston.set(gear);
+ rightGearPiston.set(gear);
+ }
+
@Override
protected void initDefaultCommand() {
setDefaultCommand(new JoystickDrive());