generated from Spikes-2212-Programming-Guild/Plain-Robot
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #29 from Spikes-2212-Programming-Guild/district-2-…
…structure fml
- Loading branch information
Showing
6 changed files
with
151 additions
and
21 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
22 changes: 22 additions & 0 deletions
22
src/main/java/frc/robot/commands/district2/District2PlaceOnReef.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,22 @@ | ||
| package frc.robot.commands.district2; | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; | ||
| import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; | ||
| import frc.robot.commands.MoveToHeight; | ||
| import frc.robot.commands.ReleaseCoral; | ||
| import frc.robot.commands.RotateStorage; | ||
| import frc.robot.subsystems.CoralJoint; | ||
| import frc.robot.subsystems.Elevator; | ||
| import frc.robot.subsystems.Storage; | ||
| import frc.robot.subsystems.district2.District2CoralJoint; | ||
|
|
||
| public class District2PlaceOnReef extends SequentialCommandGroup { | ||
|
|
||
| public District2PlaceOnReef(District2CoralJoint coralJoint, Storage storage, District2CoralJoint.StoragePose level) { | ||
| addCommands( | ||
| new District2RotateStorage(coralJoint, level), | ||
| new ReleaseCoral(storage), | ||
| new District2RotateStorage(coralJoint, District2CoralJoint.StoragePose.INTAKE) | ||
| ); | ||
| } | ||
| } |
38 changes: 38 additions & 0 deletions
38
src/main/java/frc/robot/commands/district2/District2RotateStorage.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,38 @@ | ||
| package frc.robot.commands.district2; | ||
|
|
||
| import com.spikes2212.command.genericsubsystem.commands.smartmotorcontrollergenericsubsystem.MoveSmartMotorControllerGenericSubsystem; | ||
| import com.spikes2212.control.FeedForwardController; | ||
| import com.spikes2212.control.FeedForwardSettings; | ||
| import com.spikes2212.control.PIDSettings; | ||
| import com.spikes2212.dashboard.RootNamespace; | ||
| import com.spikes2212.util.UnifiedControlMode; | ||
| import frc.robot.subsystems.CoralJoint; | ||
| import frc.robot.subsystems.district2.District2CoralJoint; | ||
|
|
||
| import java.util.function.Supplier; | ||
|
|
||
| public class District2RotateStorage extends MoveSmartMotorControllerGenericSubsystem { | ||
|
|
||
| private static final RootNamespace namespace = new RootNamespace("district 2 rotate storage"); | ||
| private static final PIDSettings pidSettings = namespace.addPIDNamespace("rotate storage"); | ||
| private static final FeedForwardSettings feedForwardSettings = namespace.addFeedForwardNamespace("rotate storage", | ||
| FeedForwardController.ControlMode.LINEAR_POSITION); | ||
|
|
||
| private final District2CoralJoint coralJoint; | ||
|
|
||
| public District2RotateStorage(District2CoralJoint coralJoint, Supplier<Double> setpoint) { | ||
| super(coralJoint, pidSettings, feedForwardSettings, UnifiedControlMode.POSITION, | ||
| setpoint, true); | ||
| this.coralJoint = coralJoint; | ||
| } | ||
|
|
||
| public District2RotateStorage(District2CoralJoint coralJoint, District2CoralJoint.StoragePose pose) { | ||
| this(coralJoint, () -> pose.neededPitch); | ||
| } | ||
|
|
||
| @Override | ||
| public boolean isFinished() { | ||
| return !(coralJoint.canMove(coralJoint.getSpeed())) || super.isFinished(); | ||
| } | ||
| } | ||
|
|
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
66 changes: 66 additions & 0 deletions
66
src/main/java/frc/robot/subsystems/district2/District2CoralJoint.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,66 @@ | ||
| package frc.robot.subsystems.district2; | ||
|
|
||
| import com.spikes2212.command.genericsubsystem.smartmotorcontrollersubsystem.SmartMotorControllerGenericSubsystem; | ||
| import com.spikes2212.util.smartmotorcontrollers.TalonFXWrapper; | ||
| import edu.wpi.first.wpilibj.DigitalInput; | ||
| import frc.robot.RobotMap; | ||
|
|
||
| public class District2CoralJoint extends SmartMotorControllerGenericSubsystem { | ||
|
|
||
| public enum StoragePose { | ||
|
|
||
| INTAKE(-1), L1(-1), L2(-1), L3(-1), RESTING(-1); | ||
|
|
||
| public final double neededPitch; | ||
|
|
||
| StoragePose(double neededPitch) { | ||
| this.neededPitch = neededPitch; | ||
| } | ||
| } | ||
|
|
||
| public static final double CORAL_JOINT_FORWARD_SPEED = 0.5; | ||
| public static final double CORAL_JOINT_BACKWARD_SPEED = 0.5; | ||
|
|
||
| private static final String NAMESPACE_NAME = "district 2 coral joint"; | ||
| private static final double GEAR_RATIO = 1; | ||
| private static final double DEGREES_IN_ROTATIONS = 360; | ||
| private static final double DISTANCE_PER_PULSE = GEAR_RATIO * DEGREES_IN_ROTATIONS; | ||
|
|
||
| private final TalonFXWrapper talonFX; | ||
| private final DigitalInput bottomLimit; | ||
|
|
||
| private static District2CoralJoint instance; | ||
|
|
||
| public static District2CoralJoint getInstance() { | ||
| if (instance == null) { | ||
| instance = new District2CoralJoint(NAMESPACE_NAME, | ||
| new TalonFXWrapper(RobotMap.CAN.CORAL_JOINT_TALON), | ||
| new DigitalInput(RobotMap.DIO.CORAL_JOINT_BOTTOM_LIMIT)); | ||
| } | ||
| return instance; | ||
| } | ||
|
|
||
| private District2CoralJoint(String namespaceName, TalonFXWrapper talonFX, DigitalInput bottomLimit) { | ||
| super(namespaceName, talonFX); | ||
| this.talonFX = talonFX; | ||
| this.bottomLimit = bottomLimit; | ||
| talonFX.setEncoderConversionFactor(DISTANCE_PER_PULSE); | ||
| configureDashboard(); | ||
| } | ||
|
|
||
| @Override | ||
| public boolean canMove(double speed) { | ||
| return !(bottomLimit.get() && speed < 0); | ||
| } | ||
|
|
||
| public void calibrateEncoderPosition() { | ||
| if (bottomLimit.get()) { | ||
| talonFX.setPosition(StoragePose.RESTING.neededPitch); | ||
| } | ||
| } | ||
|
|
||
| public void configureDashboard() { | ||
| namespace.putBoolean("bottom limit", bottomLimit::get); | ||
| namespace.putNumber("storage pose", talonFX::getPosition); | ||
| } | ||
| } |