// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.


package org.usfirst.frc6133.HaltomRobotics.subsystems;

import org.usfirst.frc6133.HaltomRobotics.Robot;
import org.usfirst.frc6133.HaltomRobotics.RobotMap;
import org.usfirst.frc6133.HaltomRobotics.commands.*;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.CANTalon.FeedbackDevice;
import edu.wpi.first.wpilibj.CANTalon.TalonControlMode;
import edu.wpi.first.wpilibj.command.Subsystem;


/**
 *
 */
public class Pitch extends Subsystem {

    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS

    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    private final CANTalon talonL = RobotMap.pitchTalonL;
    private final CANTalon talonR = RobotMap.pitchTalonR;
    private final RobotDrive pitchControl = RobotMap.pitchPitchControl;

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    public static final double STEP = 0.103478;
    public static final double MIN_COUNT = -450;
    public static final double MAX_COUNT = 460;
    public static boolean isUp = false;
    
    
    
    
	

    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    public void initDefaultCommand() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND

        setDefaultCommand(new AdjustPitch());

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
    	
    	talonR.setFeedbackDevice(FeedbackDevice.QuadEncoder);
    	talonR.reverseSensor(false);
    	talonR.configEncoderCodesPerRev(497);
    	talonR.configNominalOutputVoltage(+0.0f, -0.0f);
    	talonR.configPeakOutputVoltage(+12.0f, -12.0f);
    	talonR.setProfile(0);
    	talonR.setF(0);
    	talonR.setP(1.5);
    	talonR.setI(0.015);
    	talonR.setD(150);
    	//talonR.setEncPosition(0);
        // Set the default command for a subsystem here.
        // setDefaultCommand(new MySpecialCommand());
    }
    
    public void adjustPitch(GenericHID joystick) {
    	//The robot will now remain flat when not aiming.
    	//Go to LaunchBall.java to see where adjustPitchPID gets called for aiming and launching the ball.
    	//Go to Vision.java to change the values of the angle for aiming.
    	//if (!Robot.isAiming)
    		//adjustPitchPID(0);
    	/*This code is not used anymore.
    	 *It had been used for adjusting the pitch with the triggers.
    	double LT = joystick.getRawAxis(2);
    	double RT = joystick.getRawAxis(3);
    	talonL.changeControlMode(TalonControlMode.PercentVbus);
    	talonR.changeControlMode(TalonControlMode.PercentVbus);
    	//talonL.setInverted(false);
    	if (LT != 0.0)
    	{
    		//pitchControl.drive(LT, 0);
    		adjustPitchPID(0);
    		isUp = false;
    	}
    		
    	else if (RT != 0)
    	{
    		//pitchControl.drive(-RT, 0);
    		adjustPitchPID(1200);
    		isUp = true;
    	}
    	else
    	{
    		//pitchControl.drive(0, 0);
    		//if (talonR.getSetpoint() == 0)
    		//	adjustPitchPID(0);
    		//else if (talonR.getSetpoint() > 0)
    		//	adjustPitchPID(1500);
    		//else
    		//	stop();
    		if (Robot.vision.goalsFound == 0) 
    		{
    			if (isUp)
    				adjustPitchPID(1500);
    			else
    				adjustPitchPID(0);
    		}
    			
    	}
    	*/
    }
    
    public void stop() {
    	talonL.changeControlMode(TalonControlMode.PercentVbus);
    	talonR.changeControlMode(TalonControlMode.PercentVbus);
    	pitchControl.drive(0, 0);
    }
    
    public void adjustPitchPID(double target)	{   	
    	talonL.changeControlMode(TalonControlMode.PercentVbus);
        talonR.changeControlMode(TalonControlMode.Position);
        talonR.set(target/-1988);
        	talonL.set(0);
        else
        	talonL.set(talonR.getOutputVoltage()/-12.0);
        //System.out.println("EncPos: " + talonR.getEncPosition() + "::Sensor: " + talonR.get() + "::RawPos: " + talonR.getPosition() + "::Setpoint: " + talonR.getSetpoint());
        
        talonR.setAllowableClosedLoopErr(10);
        
    	
    	
    }
    
    public boolean isPitchReady()
    {
    	if (talonR.getClosedLoopError() <= 20)
    		return true;
    	//System.out.println("Pitch not ready");
    	return false;
    }
}


@@ -4262,46 +4262,8 @@
     
-%09talonL.set(0);%0A        else%0A        %09
 talo
@@ -4311,18 +4311,16 @@
         
-//
 System.o

Revert "Fix Jerk?"