Constants.DriveTrain.PID_ERROR);
double driveD = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_D_Val,
Constants.DriveTrain.PID_ERROR);
+
double gyroP = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_P_Val,
Constants.DriveTrain.PID_ERROR);
double gyroI = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_I_Val,
DriveTrain.getDriveTrain().getDriveController().setConstants(driveP, driveI,
driveD);
- DriveTrain.getDriveTrain().getGyroController().setConstants(driveP, driveI,
- driveD);
+ DriveTrain.getDriveTrain().getGyroController().setConstants(gyroP, gyroI,
+ gyroD);
// new DriveDistance(SETPOINT, SPEED).start();
- double Setpoint = SmartDashboard.getNumber(
- Constants.DriveTrain.DRIVE_TARGET_DIST, Constants.DriveTrain.PID_ERROR);
- double Speed = SmartDashboard.getNumber(
- Constants.DriveTrain.DRIVE_MOTOR_VAL, Constants.DriveTrain.PID_ERROR);
+ // new TurnForAngle(0, Direction.FORWARD, 5).start();
}
@Override
private Preferences prefs;
private PIDController driveController;
- private double driveP;
- private double driveI;
- private double driveD;
private double gyroP;
public DriveDistance(double distance, double maxTimeOut) {
this.maxTimeOut = maxTimeOut;
this.target = distance;
- this.driveP = driveTrain.driveP;
- this.driveI = driveTrain.driveI;
- this.driveD = driveTrain.driveD;
- this.gyroP = driveTrain.driveStraightGyroP;
this.driveController = driveTrain.getDriveController();
this.driveController.setDoneRange(0.5);
this.driveController.setMaxOutput(1.0);
private PIDController gyroController;
private double target;
- private double gyroP;
- private double gyroI;
- private double gyroD;
private double zeroAngle;
this.maxTimeOut = maxTimeOut;
this.target = Math.abs(angle);
- this.gyroP = driveTrain.turnP;
- this.gyroI = driveTrain.turnI;
- this.gyroD = driveTrain.turnD;
-
this.gyroController = Robot.getDriveTrain().getGyroController();
this.gyroController.setDoneRange(1);
this.gyroController.setMinDoneCycles(5);
public class DriveTrain extends Subsystem {
public static double driveP, driveI, driveD;
- public static double turnP = 0.004, turnI = 0.0013, turnD = -0.005;
+ public static double turnP, turnI, turnD;
public static double driveStraightGyroP = 0.01;
public static final double WHEEL_DIAMETER = 6; // inches
private DriveTrain() {
driveController = new PIDController(driveP, driveI, driveD);
+ gyroController = new PIDController(turnP, turnI, turnD);
// MOTOR CONTROLLERS
frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);