diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1904d8d..c3640d2 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -68,6 +68,10 @@ public final class Constants { public static final class IntakeConstants { public static final int INTAKE_SPARK_ID = 9; } + + public static final class ClimberConstants { + public static final int CLIMBER_SPARK_ID = 10; + } public static final class LevelerConstants { public static final int LEVELER_CAN_ID = -1; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5387b64..6fb19d4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -17,7 +17,9 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveWithJoystick; +import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunIntakeWithTriggers; +import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; @@ -40,6 +42,7 @@ public class RobotContainer { private final Drive m_robotDrive = new Drive(); private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); + private final Climber m_robotClimber = new Climber(); private final Leveler m_robotLeveler = new Leveler(); private final Storage m_robotStorage = new Storage(); @@ -58,6 +61,8 @@ public class RobotContainer { m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); // drives intake with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); + // drives climber with input from triggers on the opperator controller + m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // drives the leveler with an axis input from the driver controller diff --git a/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java b/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java new file mode 100644 index 0000000..9ab4285 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java @@ -0,0 +1,63 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Climber; +import frc4388.utility.controller.IHandController; + +public class RunClimberWithTriggers extends CommandBase { + private Climber m_climber; + private IHandController m_controller; + + /** + * Uses input from opperator triggers to control climber motor + * @param subsystem the climber subsystem + * @param controller the driver controller + */ + public RunClimberWithTriggers(Climber subsystem, IHandController controller) { + m_climber = subsystem; + m_controller = controller; + addRequirements(m_climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double rightTrigger = m_controller.getRightTriggerAxis(); + double leftTrigger = m_controller.getLeftTriggerAxis(); + double output = 0; + if (rightTrigger < .5) { + if(rightTrigger > leftTrigger) { + output = rightTrigger; + } + if (leftTrigger > rightTrigger) { + output = -leftTrigger; + } + } else { + output = rightTrigger; + } + m_climber.runClimber(output); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java new file mode 100644 index 0000000..6b35036 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.subsystems; + +import com.revrobotics.CANDigitalInput; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ClimberConstants; + +public class Climber extends SubsystemBase { + CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless); + CANDigitalInput m_forwardLimit, m_reverseLimit; + /** + * Creates a new Climber. + */ + public Climber() { + m_climberMotor.restoreFactoryDefaults(); + + m_forwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + m_reverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + + m_forwardLimit.enableLimitSwitch(false); + m_reverseLimit.enableLimitSwitch(false); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Runs climber motor + * @param input the voltage to run motor at + */ + public void runClimber(double input) { + m_climberMotor.set(input); + } +} diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index f8d42a4..a633555 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix", - "version": "5.17.3", + "version": "5.17.4", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "http://devsite.ctr-electronics.com/maven/release/" @@ -11,19 +11,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.17.3" + "version": "5.17.4" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.17.3" + "version": "5.17.4" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -35,7 +35,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -47,7 +47,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -69,7 +69,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.17.3", + "version": "5.17.4", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -83,7 +83,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": false, @@ -97,7 +97,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": false, @@ -111,7 +111,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": false, @@ -125,7 +125,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixDiagnostics", "headerClassifier": "headers", "sharedLibrary": false, @@ -139,7 +139,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixCanutils", "headerClassifier": "headers", "sharedLibrary": false, @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixPlatform", "headerClassifier": "headers", "sharedLibrary": false, @@ -165,7 +165,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.17.3", + "version": "5.17.4", "libName": "CTRE_PhoenixCore", "headerClassifier": "headers", "sharedLibrary": false,