private double gyroI;
private double gyroD;
+ private double zeroAngle;
+
public TurnForAngle(double angle, Direction direction, double maxTimeOut) {
requires(Robot.getDriveTrain());
this.direction = direction;
this.maxTimeOut = maxTimeOut;
- this.target = angle;
+ this.target = Math.abs(angle);
- this.gyroP = driveTrain.defaultGyroP;
- this.gyroI = driveTrain.defaultGyroI;
- this.gyroD = driveTrain.defaultGyroD;
+ this.gyroP = driveTrain.turnP;
+ this.gyroI = driveTrain.turnI;
+ this.gyroD = driveTrain.turnD;
this.gyroController = new PIDController(this.gyroP, this.gyroI, this.gyroD);
this.gyroController.setDoneRange(1);
@Override
protected void initialize() {
this.driveTrain.resetEncoders();
- this.driveTrain.resetGyro();
this.gyroController.setSetPoint(this.target);
+ this.zeroAngle = driveTrain.getAngle();
}
@Override
double xVal = 0;
xVal = this.gyroController
- .calcPID(this.driveTrain.getAngle() - this.driveTrain.getZeroAngle());
+ .calcPID(Math.abs(this.driveTrain.getAngle() - this.zeroAngle));
- double leftDrive;
- double rightDrive;
+ double leftDrive = 0;
+ double rightDrive = 0;
if (direction == Constants.Direction.RIGHT) {
leftDrive = xVal;
rightDrive = -xVal;
- } else {
+ } else if (direction == Constants.Direction.LEFT) {
leftDrive = -xVal;
rightDrive = xVal;
}
this.driveTrain.setMotorValues(leftDrive, rightDrive);
+ System.out.println(this.driveTrain.getAngle() - this.zeroAngle);
}
@Override