generated from StuyPulse/Phil
-
Notifications
You must be signed in to change notification settings - Fork 8
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'main' of https://github.com/StuyPulse/BigWang into se/swerve-commands
- Loading branch information
Showing
11 changed files
with
187 additions
and
33 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
39 changes: 39 additions & 0 deletions
39
src/main/java/com/stuypulse/robot/subsystems/climber/Climber.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,39 @@ | ||
| package com.stuypulse.robot.subsystems.climber; | ||
|
|
||
| import com.stuypulse.stuylib.network.SmartNumber; | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
|
|
||
| public abstract class Climber extends SubsystemBase { | ||
| private static final Climber instance; | ||
|
|
||
| private final SmartNumber targetHeight; | ||
|
|
||
| static { | ||
| instance = new ClimberImpl(); | ||
| } | ||
|
|
||
| public static Climber getInstance() { | ||
| return instance; | ||
| } | ||
|
|
||
| public Climber() { | ||
| targetHeight = new SmartNumber("Climber/Target Height", 0.0); | ||
| } | ||
|
|
||
| public void setTargetHeight(double height) { | ||
| targetHeight.set(height); | ||
| } | ||
|
|
||
| public double getTargetHeight() { | ||
| return targetHeight.get(); | ||
| } | ||
|
|
||
| public abstract double getHeight(); | ||
| public abstract double getVelocity(); | ||
|
|
||
| public abstract void setVoltage(double voltage); | ||
|
|
||
| public abstract boolean atTop(); | ||
| public abstract boolean atBottom(); | ||
| } |
105 changes: 105 additions & 0 deletions
105
src/main/java/com/stuypulse/robot/subsystems/climber/ClimberImpl.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,105 @@ | ||
| package com.stuypulse.robot.subsystems.climber; | ||
|
|
||
| import com.revrobotics.CANSparkMax; | ||
| import com.revrobotics.CANSparkLowLevel.MotorType; | ||
| import com.revrobotics.RelativeEncoder; | ||
|
|
||
| import com.stuypulse.robot.constants.Motors; | ||
| import com.stuypulse.robot.constants.Ports; | ||
| import com.stuypulse.robot.constants.Settings; | ||
|
|
||
| import edu.wpi.first.wpilibj.DigitalInput; | ||
| import edu.wpi.first.wpilibj.DriverStation; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
|
|
||
| public class ClimberImpl extends Climber { | ||
| private final CANSparkMax rightMotor; | ||
| private final CANSparkMax leftMotor; | ||
|
|
||
| private final RelativeEncoder rightEncoder; | ||
| private final RelativeEncoder leftEncoder; | ||
|
|
||
| private final DigitalInput topRightLimit; | ||
| private final DigitalInput topLeftLimit; | ||
| private final DigitalInput bottomRightLimit; | ||
| private final DigitalInput bottomLeftLimit; | ||
|
|
||
| protected ClimberImpl() { | ||
| rightMotor = new CANSparkMax(Ports.Climber.RIGHT_MOTOR, MotorType.kBrushless); | ||
| leftMotor = new CANSparkMax(Ports.Climber.LEFT_MOTOR, MotorType.kBrushless); | ||
|
|
||
| rightEncoder = rightMotor.getEncoder(); | ||
| leftEncoder = leftMotor.getEncoder(); | ||
|
|
||
| rightEncoder.setPositionConversionFactor(Settings.Climber.Encoder.POSITION_CONVERSION); | ||
| leftEncoder.setPositionConversionFactor(Settings.Climber.Encoder.POSITION_CONVERSION); | ||
|
|
||
| rightEncoder.setVelocityConversionFactor(Settings.Climber.Encoder.VELOCITY_CONVERSION); | ||
| leftEncoder.setVelocityConversionFactor(Settings.Climber.Encoder.VELOCITY_CONVERSION); | ||
|
|
||
| topRightLimit = new DigitalInput(Ports.Climber.TOP_RIGHT_LIMIT); | ||
| topLeftLimit = new DigitalInput(Ports.Climber.TOP_LEFT_LIMIT); | ||
| bottomRightLimit = new DigitalInput(Ports.Climber.BOTTOM_RIGHT_LIMIT); | ||
| bottomLeftLimit = new DigitalInput(Ports.Climber.BOTTOM_LEFT_LIMIT); | ||
|
|
||
| Motors.Climber.LEFT_MOTOR.configure(leftMotor); | ||
| Motors.Climber.RIGHT_MOTOR.configure(rightMotor); | ||
| } | ||
|
|
||
| @Override | ||
| public double getHeight() { | ||
| return (leftEncoder.getPosition() + rightEncoder.getPosition()) / 2; | ||
| } | ||
|
|
||
| @Override | ||
| public double getVelocity() { | ||
| return (leftEncoder.getVelocity() + rightEncoder.getVelocity()) / 2; | ||
| } | ||
|
|
||
| @Override | ||
| public boolean atTop() { | ||
| return !topRightLimit.get() || !topLeftLimit.get(); | ||
| } | ||
|
|
||
| @Override | ||
| public boolean atBottom() { | ||
| return !bottomRightLimit.get() || !bottomLeftLimit.get(); | ||
| } | ||
|
|
||
| @Override | ||
| public void setVoltage(double voltage) { | ||
| if (atTop() && voltage > 0) { | ||
| DriverStation.reportWarning("Climber Top Limit Reached", false); | ||
| voltage = 0.0; | ||
| } else if (atBottom() && voltage < 0) { | ||
| DriverStation.reportWarning("Climber Bottom Limit Reached", false); | ||
| voltage = 0.0; | ||
| } | ||
|
|
||
| rightMotor.setVoltage(voltage); | ||
| leftMotor.setVoltage(voltage); | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| super.periodic(); | ||
|
|
||
| if (Math.abs(getHeight() - getTargetHeight()) < Settings.Climber.AT_HEIGHT_THRESHOLD) { | ||
| setVoltage(0.0); | ||
| } else if (getHeight() > getTargetHeight()) { | ||
| setVoltage(-Settings.Climber.BANGBANG_VOLTAGE); | ||
| } else { | ||
| setVoltage(+Settings.Climber.BANGBANG_VOLTAGE); | ||
| } | ||
|
|
||
| SmartDashboard.putNumber("Climber/Target Height", getTargetHeight()); | ||
| SmartDashboard.putNumber("Climber/Height", getHeight()); | ||
|
|
||
| SmartDashboard.putNumber("Climber/Velocity", getVelocity()); | ||
|
|
||
| if (atBottom()) { | ||
| leftEncoder.setPosition(Settings.Climber.MIN_HEIGHT); | ||
| rightEncoder.setPosition(Settings.Climber.MIN_HEIGHT); | ||
| } | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters