package org.usfirst.frc.team3501.robot;
import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
/**
* The Constants stores constant values for all subsystems. This includes the
public static final int LEFT_FORWARD = 0, LEFT_REVERSE = 1,
RIGHT_FORWARD = 2, RIGHT_REVERSE = 3;
public static double time = 0;
+
+ // Gearing constants
+ public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward;
+ public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse;
+
}
public static class Scaler {
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);
+ }
}