Merge in Climber Code

Co-Authored-By: ryan123rudder <ryan123rudder@users.noreply.github.com>

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

@@ -51,9 +51,6 @@ public class OI
 			*/
 			//JoystickButton endEfector = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
 			//endEfector.toggleWhenActive(new WristPlacement(true));
-
-
-
 		
 			JoystickButton liftHatchIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
 			liftHatchIntake.whenPressed(new LiftHatchDropBall());
@@ -64,10 +61,14 @@ public class OI
 			liftBallIntake.whenPressed(new LiftBallDropHatch());
 
 
-
-
-	       // climbUp.whenPressed(new InitiateClimber(true));
-	        //climbUp.whenReleased(new InitiateClimber(false));
+			JoystickButton climbUp = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_TRIGGER_AXIS);
+			double speed = m_driverXbox.getRightTriggerAxis();
+			climbUp.whenPressed(new InitiateClimber(true, speed));
+			climbUp.whenReleased(new InitiateClimber(false, speed));
+			
+			JoystickButton ratchetFlip = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_TRIGGER_AXIS);
+			ratchetFlip.whenPressed(new ratchetFlip(true, speed));
+			ratchetFlip.whenReleased(new ratchetFlip(false, speed));
 	        
 	        JoystickButton shiftUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
 	        shiftUp.whenPressed(new DriveSpeedShift(true));
@@ -84,10 +85,13 @@ public class OI
 	        openIntake.whenPressed(new IntakePosition(true));
 	        
 	        JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
-	        CloseIntake.whenPressed(new IntakePosition(false));
+			CloseIntake.whenPressed(new IntakePosition(false));
+			*/
+			JoystickButton safteySwitch = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.START_BUTTON);
+			safteySwitch.whenPressed(new setClimberSafety(true));
+			safteySwitch.whenReleased(new setClimberSafety(false));
 	     	
-	        SmartDashboard.putData("PRE GAME!!!!", new PreGame());
-	       */
+	        //SmartDashboard.putData("PRE GAME!!!!", new PreGame());
 		  } catch (Exception e) {
 			  System.err.println("An error occurred in the OI constructor");
 		  }

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

@@ -24,7 +24,12 @@ public class RobotMap {
 	//Elevator Motors
 	public static final int ELEVATOR_MOTOR1_ID = 6;
 	public static final int ELEVATOR_MOTOR2_ID = 7;
-	public static final int CLIMBER_CAN_ID = 10;
+
+	
+	//Climber Motors
+	public static final int CLIMBER_CAN_ID = 11;
+	public static final int CLIMBER_WHEEL1_ID = 12;
+	public static final int CLIMBER_WHEEL2_ID = 13;
 	
 	
 	// Pneumatics

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

@@ -1,28 +1,36 @@
 package org.usfirst.frc4388.robot.commands;
 
 import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.Constants;
 
 import edu.wpi.first.wpilibj.command.Command;
 
 public class InitiateClimber extends Command
 {
 	boolean climb;
-	
-	public InitiateClimber(boolean climb) {
-		this.climb=climb;
+	double speed;
+
+	public InitiateClimber(boolean climb, double speed) {
 		requires(Robot.climber);
+		this.climb = climb;
+		this.speed = speed;
 	}
 
 	@Override
 	protected void initialize() {
-		Robot.climber.setClimbSpeed(climb);
+		
 	}
 
 	@Override
 	protected void execute() {
-    }
-
-    // Make this return true when this Command no longer needs to run execute()
+		if(climb){ //If climb button is pressed
+			Robot.climber.setClimbSpeed(climb, speed);
+		}
+		else{
+			Robot.climber.setClimbSpeed(false, 0);
+		}
+	}
+	   // Make this return true when this Command no longer needs to run execute()
     protected boolean isFinished() {
         return true;
     }
@@ -30,9 +38,9 @@ public class InitiateClimber extends Command
     // Called once after isFinished returns true
     protected void end() {
     }
-    
+
 	@Override
 	protected void interrupted() {
-			
+
 	}
-}
+}

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

@@ -0,0 +1,49 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class ratchetFlip extends Command {
+  boolean flip;
+  double speed;
+  public ratchetFlip(boolean flip, double speed) {
+    requires(Robot.climber);
+    this.flip = flip;
+    this.speed = speed;
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    if(flip){
+      Robot.climber.flipRatchet(true, speed);
+    }
+    else{
+      Robot.climber.flipRatchet(false, speed);
+    }
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+  }
+}

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

@@ -0,0 +1,47 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class setClimberSafety extends Command {
+  boolean safety;
+  public setClimberSafety(boolean safety) {
+    requires(Robot.climber);
+		this.safety = safety;
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+  }
+
+  // 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;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+  }
+}