<filename>src/org/usfirst/frc841/CometFRC2014/subsystems/Catapult.java
// RobotBuilder Version: 1.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.frc841.CometFRC2014.subsystems;
import org.usfirst.frc841.CometFRC2014.RobotMap;
import org.usfirst.frc841.CometFRC2014.commands.*;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.command.Subsystem;
/**
 *Defined the subsystem that controls the Catapult mechanism
 * 
 * @author <EMAIL> (<NAME>)
 */
public class Catapult extends Subsystem {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    DoubleSolenoid rachetSolenoid = RobotMap.catapultRachetSolenoid;
    AnalogChannel potentiometer = RobotMap.catapultPotentiometer;
    SpeedController winchMotor = RobotMap.catapultWinchMotor;
    DigitalInput secondaryLimit = RobotMap.catapultSecondaryLimit;
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
   
    //Constants to set the limit for the release and cock positions
    double cockLimit = 0.8;
    double releaseLimit = 3.66;//3.33;
    
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    public void initDefaultCommand() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
	
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    //Sets the winch motor full power forward
    public void WinchIn(){
        winchMotor.set(1);
    }
    //sets the winch motor to stall power forward
    public void WinchStall(){
        winchMotor.set(0.16); //It has been tested to get the stall value 
    }
    //Sets the winch motor to no power
    public void WInchStop(){
        winchMotor.set(0);
    }
    //Sets solenoid forward to encage gear for winch to pull in catapult
    public void RachetEngage(){
        rachetSolenoid.set(DoubleSolenoid.Value.kForward);
    }
    //Sets solenoid reverse to disengage gear to neutral to realse catapult 
    public void RachetRelease(){
        rachetSolenoid.set(DoubleSolenoid.Value.kReverse);
    }
    //Turns off solenoid
    public void RachetOff(){
        rachetSolenoid.set(DoubleSolenoid.Value.kOff);
    }
    //Returns the current position of the Potentiometer to get the 
    //catapult angle.
    public double getPosition(){
        return potentiometer.getVoltage();
    }
    //Returns the cocklimit so it can be accessed by the command
    public double getCockLimit(){
        return cockLimit;
    }
    //Returns the releaselimit so it can be accessed by the command
    public double getReleaseLimit(){
        return releaseLimit;
    }
    public boolean isAtSafetyLimit(){
        return !secondaryLimit.get();
    }
     
}
