Test Climber and Reprogram to Fix Bugs

2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java

@@ -9,4 +9,8 @@ public interface IHandController {
 	public double getRightXAxis();
 	
 	public double getRightYAxis();
+
+	public double getLeftTriggerAxis();
+
+	public double getRightTriggerAxis();
 }

2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java

@@ -64,10 +64,10 @@ public class OI
 			JoystickButton climbUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_TRIGGER_AXIS);
 			JoystickButton climbDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_TRIGGER_AXIS);
 
-			double speed = m_driverXbox.getRightTriggerAxis() - m_driverXbox.getLeftTriggerAxis();
+			//double speed = m_driverXbox.getRightTriggerAxis() - m_driverXbox.getLeftTriggerAxis();
-			climbUp.whileActive(new InitiateClimber(speed));
+			//climbUp.whenActive(new InitiateClimber(m_driverXbox.getRightTriggerAxis()));
-			climbDown.whileActive(new InitiateClimber(speed));
+			//climbDown.whenActive(new InitiateClimber(-m_driverXbox.getLeftTriggerAxis()));
-			
+
 			JoystickButton ratchetFlip = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.Y_BUTTON);
 			ratchetFlip.whenPressed(new ratchetFlip(0.5));
 			ratchetFlip.whenReleased(new ratchetFlip(0));
@@ -82,7 +82,7 @@ public class OI
 	        
 	        
 	        //Operator Xbox
-/*
+			/*
 	        JoystickButton openIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
 	        openIntake.whenPressed(new IntakePosition(true));
 	        

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java

@@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj.command.Command;
 public class InitiateClimber extends Command
 {
 	static double SPEED;
-
 	public InitiateClimber(double speed) {
 		requires(Robot.climber);
 		this.SPEED = speed;

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/setClimberSafety.java

@@ -15,23 +15,18 @@ public class setClimberSafety extends Command {
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    Robot.climber.safetySwitch(safety);
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    if(safety){ //If saftey button is pressed
-      Robot.climber.safetySwitch(true);
-    }
-    else{
-      Robot.climber.safetySwitch(false);
-    }
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return false;
+    return true;
   }
 
   // Called once after isFinished returns true

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java

@@ -1,13 +1,10 @@
-
-
-
-
 package org.usfirst.frc4388.robot.subsystems;
 
 import java.util.ArrayList;
 
 import org.usfirst.frc4388.controller.XboxController;
 import org.usfirst.frc4388.robot.Constants;
+import org.usfirst.frc4388.robot.OI;
 import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.RobotMap;
 import org.usfirst.frc4388.robot.commands.*;
@@ -41,21 +38,25 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
  */
 public class Climber extends Subsystem{
 	
-		//Motors
+	//Motors
 	private WPI_TalonSRX climberBack;
 	private WPI_TalonSRX climberFront;
 	private WPI_TalonSRX climberFront2;
 	private WPI_TalonSRX flipOutMotor;
 
-		//Frequency Control
+	//Frequency Control
 	static float BACK_FREQ = 1;
 	static float FRONT_FREQ;
 	static float FREQ_RATIO = 0.2443744576F;
 
-		//Limit and Saftey vars
+	//Limit and Saftey vars
 	LimitSwitchSource limitSwitchSource;
 	SensorCollection isPressed;
 	boolean safetySwitch;
+
+	//Climb Trigger
+	double rightTriggerInput;
+	double leftTriggerInput;
 	
 	public Climber(){
 		
@@ -66,7 +67,8 @@ public class Climber extends Subsystem{
 			climberFront2.set(ControlMode.Follower, climberFront.getDeviceID());
 
 			climberBack.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
-    		climberBack.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+			climberBack.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+			climberBack.setNeutralMode(NeutralMode.Brake);
 			FRONT_FREQ = BACK_FREQ * FREQ_RATIO; // Sets the front motor speed to ~1/4 the back motor speed
 		} 
 		catch (Exception e) {
@@ -82,29 +84,51 @@ public class Climber extends Subsystem{
 
     @Override
     public void periodic() {
-        // Put code here to be run every loop
+
-    }
+		rightTriggerInput = OI.getInstance().getDriverController().getRightTriggerAxis();
-    
+		leftTriggerInput = OI.getInstance().getDriverController().getLeftTriggerAxis();
-	public void setClimbSpeed(double speed) {
+		double speed = rightTriggerInput - leftTriggerInput;
+
+		// Put code here to be run every loop
 		if (safetySwitch) {
+			/*
 			if (isPressed.isRevLimitSwitchClosed() && speed < 0){ //If leg at min height, and the input would retract the leg
 				climberBack.set(0);
 				climberFront.set(0);
 			}
-			if(isPressed.isFwdLimitSwitchClosed() && speed > 0){ //If leg at max height, and the input would extend the leg
+			else if(isPressed.isFwdLimitSwitchClosed() && speed > 0){ //If leg at max height, and the input would extend the leg
 				climberBack.set(0);
 				climberFront.set(FRONT_FREQ * speed);
 			}
-			else { //If leg not at max height
+			else { //If leg not at max height */
 				climberBack.set(BACK_FREQ * speed);
 				climberFront.set(FRONT_FREQ * speed);
-			}		
+			//}		
 		}
-		if (!safetySwitch) {
+		else {
 			climberBack.set(0);
 			climberFront.set(0);
 		}
 	}
+	
+	///DEPRICATED
+	public void setClimbSpeed(double speed) {
+		if (safetySwitch) {
+			/*
+			if (isPressed.isRevLimitSwitchClosed() && speed < 0){ //If leg at min height, and the input would retract the leg
+				climberBack.set(0);
+				climberFront.set(0);
+			}
+			else if(isPressed.isFwdLimitSwitchClosed() && speed > 0){ //If leg at max height, and the input would extend the leg
+				climberBack.set(0);
+				climberFront.set(FRONT_FREQ * speed);
+			}
+			else { //If leg not at max height */
+				climberBack.set(BACK_FREQ * speed);
+				climberFront.set(FRONT_FREQ * speed);
+			//}		
+		}
+	}
 
 	public void safetySwitch(boolean safetySwitch){
 		this.safetySwitch = safetySwitch;