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.zeroAngle = driveTrain.getAngle();
this.gyroP = driveTrain.defaultGyroP;
this.gyroI = driveTrain.defaultGyroI;
@Override
protected void initialize() {
this.driveTrain.resetEncoders();
- this.driveTrain.resetGyro();
this.gyroController.setSetPoint(this.target);
}
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;
}
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 void resetGyro() {
- this.gyroZero = this.getAngle();
+ this.imu.reset();
}
public double getZeroAngle() {