// 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.frc3566.OfficialCode.subsystems;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.SpeedController;
import org.usfirst.frc3566.OfficialCode.RobotMap;
import org.usfirst.frc3566.OfficialCode.commands.*;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.usfirst.frc3566.OfficialCode.Robot;
import org.usfirst.frc3566.OfficialCode.RobotConstants;
/**
 *
 */
public class ElToro extends Subsystem {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    SpeedController leftArm = RobotMap.elToroLeftArm;
    SpeedController rightArm = RobotMap.elToroRightArm;
    SpeedController elevator = RobotMap.elToroElevator;
    DigitalInput upperLimit = RobotMap.elToroUpperLimit;
    DigitalInput lowerLimit = RobotMap.elToroLowerLimit;
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    public void initDefaultCommand() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
        setDefaultCommand(new JoystickElToro());
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
	
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    public void raise(double speed) {
        elevator.set(speed);
    }
    
    public void raise() {
        if (!atUpperLimit() && !Robot.catapult.isWinding()) {
            stopTwirling();
            raise(RobotConstants.EL_TORO_RAISE_SPEED);
        }
    }
    
    public void lower(double speed) {
        if (!atLowerLimit()) {
            elevator.set(speed);
        }
    }
    
    public void lower() {
        lower(RobotConstants.EL_TORO_LOWER_SPEED);
    }
    
    public void stop() {
        elevator.set(0);
    }
    
    public boolean atUpperLimit() {
        return upperLimit.get() == RobotConstants.EL_TORO_UPPER_LIMIT_CLOSED;
    }
    
    public boolean atLowerLimit() {
        return lowerLimit.get() == RobotConstants.EL_TORO_LOWER_LIMIT_CLOSED;
    }
    
    public void twirl(double speed) {
        leftArm.set(speed);
        rightArm.set(-speed);
    }
    
    public void twirlInward() {
        twirl(RobotConstants.EL_TORO_TWIRL_INWARD_SPEED);
    }
    
    public void twirlOutward() {
        twirl(RobotConstants.EL_TORO_TWIRL_OUTWARD_SPEED);
    }
    
    public void stopTwirling() {
        twirl(0);
    }
    
    public void monitor() {
        SmartDashboard.putData("El Toro Upper Limit", upperLimit);
        SmartDashboard.putData("El Toro Lower Limit", lowerLimit);
    }
}

@@ -541,16 +541,18 @@
 stems;%0D%0A
+%0D%0A
 import e
@@ -953,16 +953,18 @@
 tants;%0D%0A
+%0D%0A
 /**%0D%0A *%0D
@@ -1076,24 +1076,26 @@
 CLARATIONS%0D%0A
+%0D%0A
     SpeedCon
@@ -1435,20 +1435,16 @@
 ATIONS%0D%0A
-    
 %0D%0A    //
@@ -1691,24 +1691,28 @@
 ElToro());%0D%0A
+    
     // END A
@@ -1774,9 +1774,8 @@
 ND%0D%0A
-%09
 %0D%0A  
@@ -1883,36 +1883,32 @@
 and());%0D%0A    %7D%0D%0A
-    
 %0D%0A    public voi
@@ -1936,78 +1936,8 @@
  %7B%0D%0A
-        elevator.set(speed);%0D%0A    %7D%0D%0A    %0D%0A    public void raise() %7B%0D%0A
     
@@ -2024,24 +2024,101 @@
 ing();%0D%0A    
+        elevator.set(speed);%0D%0A        %7D%0D%0A    %7D%0D%0A%0D%0A    public void raise() %7B%0D%0A
         rais
@@ -2161,34 +2161,19 @@
 );%0D%0A    
-    %7D%0D%0A    %7D%0D%0A    
+%7D%0D%0A
 %0D%0A    pu
@@ -2281,36 +2281,32 @@
       %7D%0D%0A    %7D%0D%0A
-    
 %0D%0A    public voi
@@ -2369,36 +2369,32 @@
 SPEED);%0D%0A    %7D%0D%0A
-    
 %0D%0A    public voi
@@ -2430,36 +2430,32 @@
 set(0);%0D%0A    %7D%0D%0A
-    
 %0D%0A    public boo
@@ -2555,36 +2555,32 @@
 CLOSED;%0D%0A    %7D%0D%0A
-    
 %0D%0A    public boo
@@ -2680,36 +2680,32 @@
 CLOSED;%0D%0A    %7D%0D%0A
-    
 %0D%0A    public voi
@@ -2788,36 +2788,32 @@
 speed);%0D%0A    %7D%0D%0A
-    
 %0D%0A    public voi
@@ -2889,36 +2889,32 @@
 SPEED);%0D%0A    %7D%0D%0A
-    
 %0D%0A    public voi
@@ -2992,36 +2992,32 @@
 SPEED);%0D%0A    %7D%0D%0A
-    
 %0D%0A    public voi
@@ -3062,20 +3062,16 @@
 %0A    %7D%0D%0A
-    
 %0D%0A    pu

Check Raise Right