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;
private static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3;
private Encoder leftEncoder, rightEncoder;
+
+ public static Lidar leftLidar;
+ public static Lidar rightLidar;
+
private CANTalon frontLeft, frontRight, rearLeft, rearRight;
private RobotDrive robotDrive;
// 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);
rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
+
+ leftLidar = new Lidar(I2C.Port.kOnboard);
+ rightLidar = new Lidar(I2C.Port.kOnboard); // TODO: find port for second
+ // lidar
leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
rightEncoder.reset();
}
- public Value getLeftGearPistonValue() {
- return leftGearPiston.get();
+ public double getLeftLidarDistance() {
+ return leftLidar.pidGet();
}
- public Value getRightGearPistonValue() {
- return rightGearPiston.get();
+ public double getRightLidarDistance() {
+ return rightLidar.pidGet();
}
public double getRightSpeed() {
output = Math.signum(output) * 0.3;
left = output;
right = output + drift * kp / 10;
- }
- else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
+ } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
left = output;
right = -output;
}
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);
+ }
}