-
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.
feat: everything works :partyparrot:
- Loading branch information
1 parent
652f3aa
commit 27c8204
Showing
10 changed files
with
150 additions
and
24 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
36 changes: 36 additions & 0 deletions
36
src/main/java/org/team1540/advantagekitdemo/commands/IntakeCommand.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,36 @@ | ||
| package org.team1540.advantagekitdemo.commands; | ||
|
|
||
| import edu.wpi.first.math.controller.ArmFeedforward; | ||
| import edu.wpi.first.math.controller.PIDController; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import org.team1540.advantagekitdemo.subsystems.intake.Intake; | ||
|
|
||
| import static org.team1540.advantagekitdemo.Constants.IntakeConstants.*; | ||
|
|
||
| public class IntakeCommand extends Command { | ||
| private final Intake intake; | ||
| private final Rotation2d wristSetpoint; | ||
| private final double intakePercent; | ||
|
|
||
| private final PIDController pid = new PIDController(WRIST_KP, WRIST_KI, WRIST_KD); | ||
| private final ArmFeedforward ff = new ArmFeedforward(WRIST_KS, WRIST_KG, WRIST_KV); | ||
|
|
||
| public IntakeCommand(Intake intake, Rotation2d wristSetpoint, double intakePercent) { | ||
| this.intake = intake; | ||
| this.wristSetpoint = wristSetpoint; | ||
| this.intakePercent = intakePercent; | ||
| addRequirements(intake); | ||
| } | ||
|
|
||
| @Override | ||
| public void initialize() { | ||
| intake.setWristPosition(wristSetpoint); | ||
| intake.setIntakePercent(intakePercent); | ||
| } | ||
|
|
||
| @Override | ||
| public boolean isFinished() { | ||
| return intake.getProfilePosition() == wristSetpoint.getRotations(); | ||
| } | ||
| } |
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
55 changes: 55 additions & 0 deletions
55
src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIOReal.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,55 @@ | ||
| package org.team1540.advantagekitdemo.subsystems.elevator; | ||
|
|
||
| import com.ctre.phoenix6.BaseStatusSignal; | ||
| import com.ctre.phoenix6.StatusSignal; | ||
| import com.ctre.phoenix6.configs.TalonFXConfiguration; | ||
| import com.ctre.phoenix6.controls.Follower; | ||
| import com.ctre.phoenix6.controls.VoltageOut; | ||
| import com.ctre.phoenix6.hardware.TalonFX; | ||
| import com.ctre.phoenix6.signals.ForwardLimitValue; | ||
| import com.ctre.phoenix6.signals.ReverseLimitValue; | ||
| import org.team1540.advantagekitdemo.util.Conversions; | ||
|
|
||
| import static org.team1540.advantagekitdemo.Constants.ElevatorConstants.*; | ||
|
|
||
| public class ElevatorIOReal implements ElevatorIO { | ||
| private final TalonFX leader = new TalonFX(LEADER_ID); | ||
| private final TalonFX follower = new TalonFX(FOLLOWER_ID); | ||
|
|
||
| private final StatusSignal<Double> position = leader.getPosition(); | ||
| private final StatusSignal<Double> voltage = leader.getMotorVoltage(); | ||
| private final StatusSignal<Double> current = leader.getSupplyCurrent(); | ||
| private final StatusSignal<ReverseLimitValue> lowerLimit = leader.getReverseLimit(); | ||
| private final StatusSignal<ForwardLimitValue> upperLimit = leader.getForwardLimit(); | ||
|
|
||
| public ElevatorIOReal() { | ||
| TalonFXConfiguration config = new TalonFXConfiguration(); | ||
| config.CurrentLimits.SupplyCurrentLimit = 40; | ||
| config.CurrentLimits.SupplyCurrentThreshold = 60; | ||
| config.CurrentLimits.SupplyTimeThreshold = 0.1; | ||
| config.CurrentLimits.SupplyCurrentLimitEnable = true; | ||
|
|
||
| config.HardwareLimitSwitch.ForwardLimitEnable = true; | ||
| config.HardwareLimitSwitch.ForwardLimitAutosetPositionValue = 0; | ||
| config.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable = true; | ||
| config.HardwareLimitSwitch.ReverseLimitEnable = true; | ||
|
|
||
| follower.setControl(new Follower(LEADER_ID, true)); | ||
| } | ||
|
|
||
| @Override | ||
| public void updateInputs(ElevatorIOInputs inputs) { | ||
| BaseStatusSignal.refreshAll(position, voltage, current); | ||
|
|
||
| inputs.positionMeters = Conversions.motorRotsToMeters(position.getValue(), DRUM_RADIUS_METERS * 2, GEAR_RATIO); | ||
| inputs.appliedVolts = voltage.getValue(); | ||
| inputs.currentAmps = voltage.getValue(); | ||
| inputs.lowerLimit = lowerLimit.getValue() == ReverseLimitValue.ClosedToGround; | ||
| inputs.upperLimit = upperLimit.getValue() == ForwardLimitValue.ClosedToGround; | ||
| } | ||
|
|
||
| @Override | ||
| public void setVoltage(double volts) { | ||
| leader.setControl(new VoltageOut(volts)); | ||
| } | ||
| } |
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