From f1cff88862d252495f111080c290f45cad5d15f9 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 16 Feb 2019 19:42:25 -0700 Subject: [PATCH] Test Climber and Reprogram to Fix Bugs --- .../frc4388/controller/IHandController.java | 4 ++ .../java/org/usfirst/frc4388/robot/OI.java | 10 ++-- .../robot/commands/InitiateClimber.java | 1 - .../robot/commands/setClimberSafety.java | 9 +-- .../frc4388/robot/subsystems/Climber.java | 56 +++++++++++++------ 5 files changed, 51 insertions(+), 29 deletions(-) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java b/2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java index d8f08dc..b32e726 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java +++ b/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(); } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java index c4b3411..e85ebbe 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/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(); - climbUp.whileActive(new InitiateClimber(speed)); - climbDown.whileActive(new InitiateClimber(speed)); - + //double speed = m_driverXbox.getRightTriggerAxis() - m_driverXbox.getLeftTriggerAxis(); + //climbUp.whenActive(new InitiateClimber(m_driverXbox.getRightTriggerAxis())); + //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)); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java index f7e4885..864ae5d 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java +++ b/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; diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/setClimberSafety.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/setClimberSafety.java index c0e459a..82f539c 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/setClimberSafety.java +++ b/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 diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java index a4728db..0c47e6a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java +++ b/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 - } - - public void setClimbSpeed(double speed) { + + rightTriggerInput = OI.getInstance().getDriverController().getRightTriggerAxis(); + leftTriggerInput = OI.getInstance().getDriverController().getLeftTriggerAxis(); + 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;