RightPeg.java 文件源码

public RightPeg() {
    Robot.driveTrain.resetEnc();
    table = NetworkTable.getTable("Vision");

    addSequential(new DFDSpeed(-200, -200, 1.60, 1.60));
    addSequential(new WaitCommand(0.2));
    addSequential(new GyroTurn(-150, -50), 2); //CHECK + AND -
    addSequential(new WaitCommand(0.2));
    addSequential(new VisionGyroAlign(), 1);
    addSequential(new WaitCommand(0.2));
    addSequential(new MovingVisionAlignment(), 5); //removed timeout
    addSequential(new WaitCommand(0.2));

    //Move back retry
    addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
    addSequential(new WaitCommand(0.1));
    addSequential(new MovingVisionAlignment(), 5); //removed timeout
    addSequential(new WaitCommand(0.1));
    addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
    addSequential(new WaitCommand(0.1));
    addSequential(new MovingVisionAlignment(), 5); //removed timeout
    addSequential(new WaitCommand(0.1));
    addSequential(new DFDSMove(200, 200, 1.0, 1.0), 2);
    addSequential(new WaitCommand(0.1));
    addSequential(new MovingVisionAlignment()); //removed timeout
    addSequential(new WaitCommand(0.1));
    addSequential(new GearOn(false, false), 1);
    addSequential(new WaitCommand(0.5));
    addSequential(new DFDSpeed(200, 200, 1.4, 1.4));

}