From 979a957f1fcf7c0434bac877a96b065226a5577b Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 16 Feb 2019 13:16:51 -0700 Subject: [PATCH 1/3] Merge in Climber Code Co-Authored-By: ryan123rudder --- .../java/org/usfirst/frc4388/robot/OI.java | 24 +++++---- .../org/usfirst/frc4388/robot/RobotMap.java | 7 ++- .../robot/commands/InitiateClimber.java | 28 +++++++---- .../frc4388/robot/commands/ratchetFlip.java | 49 +++++++++++++++++++ .../robot/commands/setClimberSafety.java | 47 ++++++++++++++++++ 5 files changed, 134 insertions(+), 21 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ratchetFlip.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/setClimberSafety.java 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 b0da466..9d4c26b 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/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"); } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java index 64e015f..075c9f6 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/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 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 8a6e37d..619fc50 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 @@ -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() { - + } -} \ No newline at end of file +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ratchetFlip.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ratchetFlip.java new file mode 100644 index 0000000..8b6dfb9 --- /dev/null +++ b/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() { + } +} 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 new file mode 100644 index 0000000..c0e459a --- /dev/null +++ b/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() { + } +} From 259b7495e70907a503fca9a610525329e246a54b Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 16 Feb 2019 13:26:23 -0700 Subject: [PATCH 2/3] Add in missed Climber.java from last commit Co-Authored-By: ryan123rudder --- .../frc4388/robot/subsystems/Climber.java | 93 ++++++++++++++----- 1 file changed, 70 insertions(+), 23 deletions(-) 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 3cacf76..0c95756 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 @@ -4,14 +4,35 @@ 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.Robot; import org.usfirst.frc4388.robot.RobotMap; import org.usfirst.frc4388.robot.commands.*; +import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; +import org.usfirst.frc4388.utility.CANTalonEncoder; +import org.usfirst.frc4388.utility.ControlLoopable; +import org.usfirst.frc4388.utility.PIDParams; +import org.usfirst.frc4388.utility.SoftwarePIDPositionController; -import edu.wpi.first.wpilibj.DoubleSolenoid; +import com.ctre.phoenix.motorcontrol.ControlMode; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; @@ -20,20 +41,36 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; */ public class Climber extends Subsystem{ - private WPI_TalonSRX Climber; + //Motors + private WPI_TalonSRX climberBack; + private WPI_TalonSRX climberFront; + private WPI_TalonSRX climberFront2; + private WPI_TalonSRX flipOutMotor; - public boolean on; + //Frequency Control + static float BACK_FREQ = 1; + static float FRONT_FREQ; + static float FREQ_RATIO = 0.2443744576F; + + //Limit and Saftey vars + LimitSwitchSource limitSwitchSource; + SensorCollection isPressed; + boolean safetySwitch; public Climber(){ try{ - - Climber = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID); - - } catch (Exception e) { - - System.err.println("An error occurred in the climbing constructor"); - + climberBack = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID); + climberFront = new WPI_TalonSRX(RobotMap.CLIMBER_WHEEL1_ID); + climberFront2 = new WPI_TalonSRX(RobotMap.CLIMBER_WHEEL2_ID); + climberFront2.set(ControlMode.Follower, climberFront.getDeviceID()); + + climberBack.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + climberBack.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + FRONT_FREQ = BACK_FREQ * FREQ_RATIO; // Sets the front motor speed to ~1/4 the back motor speed + } + catch (Exception e) { + System.err.println("The code broke before the guard did. An error occurred in the climbing constructor"); } } @@ -49,20 +86,30 @@ public class Climber extends Subsystem{ } - public void setClimbSpeed(boolean Climb) { - if (Climb==true) { - Climber.set(1.0); + public void setClimbSpeed(boolean Climb, double speed) { + if (Climb && safetySwitch) { + if(isPressed.isFwdLimitSwitchClosed()){ //If leg at max height + climberBack.set(0); + climberFront.set(FRONT_FREQ * speed); + } + else if (!isPressed.isFwdLimitSwitchClosed()){ //If leg not at max height + climberBack.set(BACK_FREQ * speed); + climberFront.set(FRONT_FREQ * speed); + } } - if (Climb==false) { - Climber.set(0); - } -} - // Put methods for controlling this subsystem - // here. Call these from Commands. - { - // TODO Auto-generated method stub - + if (!Climb || !safetySwitch) { + climberBack.set(0); + climberFront.set(0); } + } } + public void safetySwitch(boolean safetySwitch){ + this.safetySwitch = safetySwitch; + } + + public void flipRatchet(boolean flip, double speed){ + climberFront.set(FRONT_FREQ * speed); + } +} From fac3d58db91649d52722d96de1c15718e217a417 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 16 Feb 2019 14:54:55 -0700 Subject: [PATCH 3/3] Fix Syntax Error in Climber.java --- .../main/java/org/usfirst/frc4388/robot/subsystems/Climber.java | 2 -- 1 file changed, 2 deletions(-) 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 0c95756..2b1eda0 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 @@ -103,8 +103,6 @@ public class Climber extends Subsystem{ } } -} - public void safetySwitch(boolean safetySwitch){ this.safetySwitch = safetySwitch; }