import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;
import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
/**
*
* again to cross the baseline.
*/
public class AutonMiddleGear extends CommandGroup {
- private static final double DISTANCE_TO_PEG = 91.3;
- private static final double DISTANCE_TO_BACK_OUT = 29.75;
+ private static final double DISTANCE_TO_PEG = 91.3 - 32;
+ private static final double DISTANCE_TO_BACK_OUT = -29.75;
private static final double THIRD_DISTANCE_TO_TRAVEL = 70;
private static final double DISTANCE_TO_BASELINE = 50.5;
*/
public AutonMiddleGear(Direction direction) {
addSequential(new DriveDistance(DISTANCE_TO_PEG, maxTimeOut));
+ addSequential(new WaitCommand(3));
addSequential(new DriveDistance(DISTANCE_TO_BACK_OUT, maxTimeOut));
addSequential(new TurnForAngle(ANGLE_TO_TURN, direction, maxTimeOut));
addSequential(new DriveDistance(THIRD_DISTANCE_TO_TRAVEL, maxTimeOut));
requires(driveTrain);
this.maxTimeOut = maxTimeOut;
this.target = distance;
- this.zeroAngle = driveTrain.getAngle();
this.driveP = driveTrain.driveP;
this.driveI = driveTrain.driveI;
this.driveD = driveTrain.driveD;
this.gyroP = driveTrain.driveStraightGyroP;
this.driveController = new PIDController(driveP, driveI, driveD);
- this.driveController.setDoneRange(0.5);
+ this.driveController.setDoneRange(1.0);
this.driveController.setMaxOutput(1.0);
this.driveController.setMinDoneCycles(5);
}
protected void initialize() {
this.driveTrain.resetEncoders();
this.driveController.setSetPoint(this.target);
+ this.zeroAngle = driveTrain.getAngle();
}
@Override
this.direction = direction;
this.maxTimeOut = maxTimeOut;
this.target = Math.abs(angle);
- this.zeroAngle = driveTrain.getAngle();
this.gyroP = driveTrain.turnP;
this.gyroI = driveTrain.turnI;
protected void initialize() {
this.driveTrain.resetEncoders();
this.gyroController.setSetPoint(this.target);
+ this.zeroAngle = driveTrain.getAngle();
}
@Override
import edu.wpi.first.wpilibj.command.Subsystem;
public class DriveTrain extends Subsystem {
- public static double driveP = 0.006, driveI = 0.001, driveD = -0.002;
+ public static double driveP = 0.012, driveI = 0.0011, driveD = -0.002;
public static double turnP = 0.004, turnI = 0.0013, turnD = -0.005;
public static double driveStraightGyroP = 0.01;