package org.robockets.stronghold.robot.highgoalshooter;

import org.robockets.stronghold.robot.RobotMap;

import edu.wpi.first.wpilibj.command.Subsystem;

/**
 * Subsystem for the high goal shooter mechanism including the turn table, the rollers, the shooting rollers, and the hood.
 */
public class HighGoalShooter extends Subsystem {

    public void initDefaultCommand() {
    }
    
    
    /**
     * Roll the Jeff rollers (the motors that guide the ball into the firing mechanism) at a desired speed.
     * @param speed 	The speed to set both Jeff rollers at.
     */
    public void spinJeffRollers(double speed) {
    	RobotMap.jeffRoller1.set(speed);
    	RobotMap.jeffRoller2.set(speed);
    }
    
    /**
     * Roll the shooting wheel to fire the cannonball at a desired speed.
     * @param speed 	The speed to set the shooting mechanism at.
     */
    public void spinShootingWheel(double speed) {
    	RobotMap.shootingWheelMotor.set(speed);
    }
}


@@ -92,18 +92,116 @@
 bot.
-RobotMap;%0A
+DummyPIDOutput;%0Aimport org.robockets.stronghold.robot.RobotMap;%0A%0Aimport edu.wpi.first.wpilibj.PIDController;
 %0Aimp
@@ -427,16 +427,691 @@
 ystem %7B%0A
+%09public final PIDController turnTablePidController;%0A%09public final PIDController hoodPidController;%0A%09%0A%09public HighGoalShooter() %7B%0A%09%09turnTablePidController = new PIDController(1, 1, 0, RobotMap.turnTableEncoder, new DummyPIDOutput());%0A%09%09hoodPidController = new PIDController(1, 1, 0, RobotMap.turnTableEncoder, new DummyPIDOutput());%0A%09%09%0A%09%09turnTablePidController.disable();%0A%09%09hoodPidController.disable();%0A%09%09%0A%09%09turnTablePidController.setSetpoint(0);%0A%09%09turnTablePidController.setPercentTolerance(0.05);%0A%09%09turnTablePidController.setContinuous(true);%0A%09%09%0A%09%09hoodPidController.setSetpoint(0);%0A%09%09hoodPidController.setPercentTolerance(0.05);%0A%09%09hoodPidController.setContinuous(true);%0A%09%7D%0A%09
 %0A    pub
@@ -1153,21 +1153,16 @@
  %7D%0A    %0A
-    %0A
     /**%0A
@@ -1728,11 +1728,1040 @@
 ;%0A    %7D%0A
+    %0A    public void spinTurnTable(double speed) %7B%0A    %09RobotMap.turnTableMotor.set(speed);%0A    %7D%0A    %0A    public void spinTurnTableAssisted() %7B%0A    %09RobotMap.turnTableMotor.set(hoodPidController.get());%0A    %7D%0A    %0A    public void setTurnTableAngle(double angle) %7B%0A    %09turnTablePidController.setSetpoint(angle);%0A    %7D%0A    %0A    public void spinHood(double speed) %7B%0A    %09RobotMap.hoodMotor.set(speed);%0A    %7D%0A    %0A    public void spinHoodAssisted() %7B%0A    %09RobotMap.hoodMotor.set(hoodPidController.get());%0A    %7D%0A    %0A    public void setHoodAngle(double angle) %7B%0A    %09hoodPidController.setSetpoint(angle);%0A    %7D%0A    %0A    public void enableTurnTablePID() %7B%0A    %09turnTablePidController.enable();%0A    %09turnTablePidController.reset();%0A    %09turnTablePidController.setSetpoint(RobotMap.turnTableEncoder.get()); // Make sure setpoint starts as current position%0A    %7D%0A    %0A    public void enableHoodPID()%7B%0A    %09hoodPidController.enable();%0A    %09hoodPidController.reset();%0A    %09hoodPidController.setSetpoint(RobotMap.hoodEncoder.get());%0A    %7D%0A
 %7D%0A%0A

Add PID boilerplate for turntable and highgoal shooter hood.