// Path: src/edu/neu/nutrons/reboundrumble/commands/CommandBase.java
// public abstract class CommandBase extends Command {
// 
//     // Single static instance of OI and all subsystems.
//     public static OI oi;
//     public static DTPIDController dtPID;
//     public static DriveTrain dt = new DriveTrain();
//     public static Shifter shifter = new Shifter();
//     public static Camera cam = new Camera();
//     public static Shooter shooter = new Shooter();
//     public static Hood hood = new Hood();
//     public static Elevator elev = new Elevator();
//     public static Intake intake = new Intake();
// 
//     // Some commands to be used in multiple places.
//     public static Command prepareFenderCmd(boolean manual) {
//         if(manual) {
//             return new PrepareManualShootingCmd(Shooter.FENDER_POWER, false, Camera.FENDER_POS);
//         }
//         else {
//             return new PrepareShootingCmd(Shooter.FENDER_RATE, false, Camera.FENDER_POS);
//         }
//     }
// 
//     public static Command prepareFenderCmd() {
//         return prepareFenderCmd(false);
//     }
// 
//     public static Command prepareKeyCmd(boolean manual) {
//         if(manual) {
//             return new PrepareManualShootingCmd(Shooter.KEY_POWER, true, Camera.KEY_POS);
//         }
//         else {
//             return new PrepareShootingCmd(Shooter.KEY_RATE, true, Camera.KEY_POS);
//         }
//     }
// 
//     public static Command prepareKeyCmd() {
//         return prepareKeyCmd(false);
//     }
// 
//     public static void init() {
//         // This MUST be here. If the OI creates Commands (which it very likely
//         // will), constructing it during the construction of CommandBase (from
//         // which commands extend), subsystems are not guaranteed to be
//         // yet. Thus, their requires() statements may grab null pointers. Bad
//         // news. Don't move it.
//         oi = new OI();
//         dtPID = new DTPIDController();
// 
//         // Display subsystem data on SmartDashboard.
//         SmartDashboard.putData(dt);
//         SmartDashboard.putData(cam);
//         SmartDashboard.putData(shooter);
//         SmartDashboard.putData(hood);
//         // Drive train PID put on SmartDashboard in dtPID constructor.
//     }
// 
//     public CommandBase(String name) {
//         super(name);
//     }
// 
//     public CommandBase() {
//         super();
//     }
// }


import edu.neu.nutrons.reboundrumble.commands.CommandBase;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.smartdashboard.SendablePIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
    public DTPIDController() {
        yawPID.setInputRange(-180, 180);
        yawPID.setContinuous();
        SmartDashboard.putData("Distance PID", disPID);
        SmartDashboard.putData("Yaw PID", yawPID);
        SmartDashboard.putData("Pitch PID", pitchPID);
        SmartDashboard.putData("Cam PID", camPID);
    }

    public void enableDistance(double setpoint) {
        disPID.enable();
        disPID.setSetpoint(disOut.get() + setpoint);
    }

    public void enableYaw(double setpoint) {
        yawPID.enable();
        yawPID.setSetpoint(yawOut.get() + setpoint);
    }

    public void enablePitch(double setpoint) {
        pitchPID.enable();
        pitchPID.setSetpoint(setpoint);
    }

    public void enableCam() {
        camPID.enable();
        camPID.setSetpoint(0);
    }

    public void disableDistance() {
// Path: src/edu/neu/nutrons/reboundrumble/commands/CommandBase.java
// public abstract class CommandBase extends Command {
// 
//     // Single static instance of OI and all subsystems.
//     public static OI oi;
//     public static DTPIDController dtPID;
//     public static DriveTrain dt = new DriveTrain();
//     public static Shifter shifter = new Shifter();
//     public static Camera cam = new Camera();
//     public static Shooter shooter = new Shooter();
//     public static Hood hood = new Hood();
//     public static Elevator elev = new Elevator();
//     public static Intake intake = new Intake();
// 
//     // Some commands to be used in multiple places.
//     public static Command prepareFenderCmd(boolean manual) {
//         if(manual) {
//             return new PrepareManualShootingCmd(Shooter.FENDER_POWER, false, Camera.FENDER_POS);
//         }
//         else {
//             return new PrepareShootingCmd(Shooter.FENDER_RATE, false, Camera.FENDER_POS);
//         }
//     }
// 
//     public static Command prepareFenderCmd() {
//         return prepareFenderCmd(false);
//     }
// 
//     public static Command prepareKeyCmd(boolean manual) {
//         if(manual) {
//             return new PrepareManualShootingCmd(Shooter.KEY_POWER, true, Camera.KEY_POS);
//         }
//         else {
//             return new PrepareShootingCmd(Shooter.KEY_RATE, true, Camera.KEY_POS);
//         }
//     }
// 
//     public static Command prepareKeyCmd() {
//         return prepareKeyCmd(false);
//     }
// 
//     public static void init() {
//         // This MUST be here. If the OI creates Commands (which it very likely
//         // will), constructing it during the construction of CommandBase (from
//         // which commands extend), subsystems are not guaranteed to be
//         // yet. Thus, their requires() statements may grab null pointers. Bad
//         // news. Don't move it.
//         oi = new OI();
//         dtPID = new DTPIDController();
// 
//         // Display subsystem data on SmartDashboard.
//         SmartDashboard.putData(dt);
//         SmartDashboard.putData(cam);
//         SmartDashboard.putData(shooter);
//         SmartDashboard.putData(hood);
//         // Drive train PID put on SmartDashboard in dtPID constructor.
//     }
// 
//     public CommandBase(String name) {
//         super(name);
//     }
// 
//     public CommandBase() {
//         super();
//     }
// }


// Path: src/edu/neu/nutrons/reboundrumble/DTPIDController.java
import edu.neu.nutrons.reboundrumble.commands.CommandBase;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.smartdashboard.SendablePIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

    public DTPIDController() {
        yawPID.setInputRange(-180, 180);
        yawPID.setContinuous();
        SmartDashboard.putData("Distance PID", disPID);
        SmartDashboard.putData("Yaw PID", yawPID);
        SmartDashboard.putData("Pitch PID", pitchPID);
        SmartDashboard.putData("Cam PID", camPID);
    }

    public void enableDistance(double setpoint) {
        disPID.enable();
        disPID.setSetpoint(disOut.get() + setpoint);
    }

    public void enableYaw(double setpoint) {
        yawPID.enable();
        yawPID.setSetpoint(yawOut.get() + setpoint);
    }

    public void enablePitch(double setpoint) {
        pitchPID.enable();
        pitchPID.setSetpoint(setpoint);
    }

    public void enableCam() {
        camPID.enable();
        camPID.setSetpoint(0);
    }

    public void disableDistance() {
        disPID.disable();