// Path: Robot/src/main/java/org/usfirst/frc/team2557/robot/commands/automation/Auto_LoadBall.java
// public class Auto_LoadBall extends CommandGroup {
// 
//     public Auto_LoadBall() {
//         // Move the arm to the correct position
//         this.addSequential(new MoveArmToAngleCommand(Arm.ARM_LOADBALL));
//         // Spin the intake for 2 seconds
//         this.addSequential(new IntakeInCommand(), 1.0);
//     }
// 
// }
// 
// Path: Robot/src/main/java/org/usfirst/frc/team2557/robot/commands/chassis/EncoderPosDriveCommand.java
// public class EncoderPosDriveCommand extends Command {
// 
//     private double _speed;
//     private double _encpos;
// 
//     public EncoderPosDriveCommand(double encpos, double speed) {
//         requires(Robot.chassis);
// 
//         this._speed = speed;
//         this._encpos = encpos;
//     }
// 
//     @Override
//     protected void initialize() {
//         Robot.chassis.resetDriveStraight();
//     }
// 
//     @Override
//     protected void execute() {
//         Robot.chassis.driveStraight(this._speed);
//     }
// 
//     @Override
//     protected boolean isFinished() {
//         return Math.abs(Robot.chassis.getLeftEncoderPos()) > this._encpos;
//     }
// 
//     @Override
//     protected void end() {
//         Robot.chassis.stop();
//     }
// 
//     @Override
//     protected void interrupted() {
//         this.end();
//     }
// }
// 
// Path: Robot/src/main/java/org/usfirst/frc/team2557/robot/commands/chassis/TurnByAngleCommand.java
// public class TurnByAngleCommand extends Command {
// 
//     private PIDController _controller;
//     private double _degrees;
// 
//     public TurnByAngleCommand(double degrees) {
//         this._degrees = degrees;
// 
//         // Kp, Ki, Kd, input (gyro), output (chassis)
//         this._controller = new PIDController(0.01, 0.05, 0,
//                 new PIDSource() {
//                     @Override
//                     public void setPIDSourceType(PIDSourceType pidSource) {
//                     }
// 
//                     @Override
//                     public PIDSourceType getPIDSourceType() {
//                         return PIDSourceType.kDisplacement;
//                     }
// 
//                     @Override
//                     public double pidGet() {
//                         return Robot.chassis.getGyroAngle();
//                     }
//                 }, new PIDOutput() {
//                     @Override
//                     public void pidWrite(double output) {
//                         Robot.chassis.set(output * 0.65, -output * 0.65);
//                     }
//                 });
//         requires(Robot.chassis);
// 
//         this._controller.setContinuous(true);
//         this._controller.setOutputRange(-1, 1);
//         this._controller.setAbsoluteTolerance(1); // 1 degree tolerance
//     }
// 
//     @Override
//     protected void initialize() {
//         // Reset the gyro
//         Robot.chassis.resetGyro();
//         // Reset the PID
//         this._controller.reset();
//         // Set the setpoint for the PID
//         this._controller.setSetpoint(this._degrees);
//     }
// 
//     @Override
//     protected void execute() {
//         this._controller.enable();
//     }
// 
//     @Override
//     protected boolean isFinished() {
//         return this._controller.onTarget();
//     }
// 
//     protected void end() {
//         this._controller.disable();
//     }
// 
//     @Override
//     protected void interrupted() {
//         this.end();
//     }
// 
// 
// 
// }


import edu.wpi.first.wpilibj.command.CommandGroup;
import org.usfirst.frc.team2557.robot.commands.automation.Auto_LoadBall;
import org.usfirst.frc.team2557.robot.commands.chassis.EncoderPosDriveCommand;
import org.usfirst.frc.team2557.robot.commands.chassis.TurnByAngleCommand;
package org.usfirst.frc.team2557.robot.commands.autonomous.sequences;


public class Auto_Pos1Center extends CommandGroup {

    public Auto_Pos1Center() {

        this.addParallel(new Auto_LoadBall());

        this.addSequential(new TurnByAngleCommand(77));

        this.addSequential(new EncoderPosDriveCommand(11406, 0.9));

        this.addSequential(new TurnByAngleCommand(-77));

// Path: Robot/src/main/java/org/usfirst/frc/team2557/robot/commands/automation/Auto_LoadBall.java
// public class Auto_LoadBall extends CommandGroup {
// 
//     public Auto_LoadBall() {
//         // Move the arm to the correct position
//         this.addSequential(new MoveArmToAngleCommand(Arm.ARM_LOADBALL));
//         // Spin the intake for 2 seconds
//         this.addSequential(new IntakeInCommand(), 1.0);
//     }
// 
// }
// 
// Path: Robot/src/main/java/org/usfirst/frc/team2557/robot/commands/chassis/EncoderPosDriveCommand.java
// public class EncoderPosDriveCommand extends Command {
// 
//     private double _speed;
//     private double _encpos;
// 
//     public EncoderPosDriveCommand(double encpos, double speed) {
//         requires(Robot.chassis);
// 
//         this._speed = speed;
//         this._encpos = encpos;
//     }
// 
//     @Override
//     protected void initialize() {
//         Robot.chassis.resetDriveStraight();
//     }
// 
//     @Override
//     protected void execute() {
//         Robot.chassis.driveStraight(this._speed);
//     }
// 
//     @Override
//     protected boolean isFinished() {
//         return Math.abs(Robot.chassis.getLeftEncoderPos()) > this._encpos;
//     }
// 
//     @Override
//     protected void end() {
//         Robot.chassis.stop();
//     }
// 
//     @Override
//     protected void interrupted() {
//         this.end();
//     }
// }
// 
// Path: Robot/src/main/java/org/usfirst/frc/team2557/robot/commands/chassis/TurnByAngleCommand.java
// public class TurnByAngleCommand extends Command {
// 
//     private PIDController _controller;
//     private double _degrees;
// 
//     public TurnByAngleCommand(double degrees) {
//         this._degrees = degrees;
// 
//         // Kp, Ki, Kd, input (gyro), output (chassis)
//         this._controller = new PIDController(0.01, 0.05, 0,
//                 new PIDSource() {
//                     @Override
//                     public void setPIDSourceType(PIDSourceType pidSource) {
//                     }
// 
//                     @Override
//                     public PIDSourceType getPIDSourceType() {
//                         return PIDSourceType.kDisplacement;
//                     }
// 
//                     @Override
//                     public double pidGet() {
//                         return Robot.chassis.getGyroAngle();
//                     }
//                 }, new PIDOutput() {
//                     @Override
//                     public void pidWrite(double output) {
//                         Robot.chassis.set(output * 0.65, -output * 0.65);
//                     }
//                 });
//         requires(Robot.chassis);
// 
//         this._controller.setContinuous(true);
//         this._controller.setOutputRange(-1, 1);
//         this._controller.setAbsoluteTolerance(1); // 1 degree tolerance
//     }
// 
//     @Override
//     protected void initialize() {
//         // Reset the gyro
//         Robot.chassis.resetGyro();
//         // Reset the PID
//         this._controller.reset();
//         // Set the setpoint for the PID
//         this._controller.setSetpoint(this._degrees);
//     }
// 
//     @Override
//     protected void execute() {
//         this._controller.enable();
//     }
// 
//     @Override
//     protected boolean isFinished() {
//         return this._controller.onTarget();
//     }
// 
//     protected void end() {
//         this._controller.disable();
//     }
// 
//     @Override
//     protected void interrupted() {
//         this.end();
//     }
// 
// 
// 
// }


// Path: Robot/src/main/java/org/usfirst/frc/team2557/robot/commands/autonomous/sequences/Auto_Pos1Center.java
import edu.wpi.first.wpilibj.command.CommandGroup;
import org.usfirst.frc.team2557.robot.commands.automation.Auto_LoadBall;
import org.usfirst.frc.team2557.robot.commands.chassis.EncoderPosDriveCommand;
import org.usfirst.frc.team2557.robot.commands.chassis.TurnByAngleCommand;

package org.usfirst.frc.team2557.robot.commands.autonomous.sequences;


public class Auto_Pos1Center extends CommandGroup {

    public Auto_Pos1Center() {

        this.addParallel(new Auto_LoadBall());

        this.addSequential(new TurnByAngleCommand(77));

        this.addSequential(new EncoderPosDriveCommand(11406, 0.9));

        this.addSequential(new TurnByAngleCommand(-77));

        this.addSequential(new EncoderPosDriveCommand(2320, 0.75));