-
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.
* added elevator lead and follow ports in constants * wrote elevator, elevatorio, and elevatoriotalonfx! yay! * feat: delete config loop * wrote elevator, elevatorio, and elevatoriotalonfx! yay! * Test commit from nmp * Test commit from nmp * started to add code for homing * added code for homing and current limits * added amber's config values * made requested edits: * added .withEnableFOC(true).withOverrideBrakeDurNeutral(true) to motion magic stuff * removed smart dashboard references * deleted pid values being set to 0 because they default to 0 * cleanup code --------- Co-authored-by: dlpond917 <[email protected]> Co-authored-by: amb2127 <[email protected]>
- Loading branch information
3 people
authored
Jan 26, 2024
1 parent
984097e
commit e11c2d7
Showing
5 changed files
with
224 additions
and
2 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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,56 @@ | ||
| package frc.subsystem.elevator; | ||
|
|
||
| import edu.wpi.first.wpilibj.DriverStation; | ||
| import static frc.robot.Constants.*; | ||
| import frc.subsystem.AbstractSubsystem; | ||
| import org.littletonrobotics.junction.Logger; | ||
|
|
||
|
|
||
| public class Elevator extends AbstractSubsystem { | ||
| final ElevatorIO elevatorIO; | ||
| private final ElevatorInputsAutoLogged elevatorInputs = new ElevatorInputsAutoLogged(); | ||
|
|
||
| private boolean homing = false; | ||
| private double homeTime = 0; | ||
| public Elevator(ElevatorIO elevatorIO){ | ||
| super(); | ||
| this.elevatorIO = elevatorIO; | ||
| } | ||
|
|
||
| public void setPosition(ElevatorPosition position) { | ||
| this.setPosition(position.positionLocationInches / ELEVATOR_INCHES_PER_ROTATION); | ||
| } | ||
|
|
||
| public void setPosition(double positionInRotations) { | ||
| double positionInInches = positionInRotations * ELEVATOR_INCHES_PER_ROTATION; | ||
| if (positionInInches < ELEVATOR_LOWER_LIMIT_INCHES) { | ||
| positionInInches = ELEVATOR_LOWER_LIMIT_INCHES; | ||
| } else if (positionInInches > ELEVATOR_UPPER_LIMIT_INCHES) { | ||
| positionInInches = ELEVATOR_UPPER_LIMIT_INCHES; | ||
| } | ||
| positionInRotations = positionInInches / ELEVATOR_INCHES_PER_ROTATION; | ||
| elevatorIO.setPosition(positionInRotations); | ||
| } | ||
|
|
||
| public void update() { | ||
| elevatorIO.updateInputs(elevatorInputs); | ||
|
|
||
| if (homing) { | ||
| if (DriverStation.isEnabled()) { | ||
| homeTime -= NOMINAL_DT; | ||
| elevatorIO.setElevatorVoltage(ELEVATOR_HOME_VOLTAGE); | ||
| double avgMotorVoltage = (elevatorInputs.leadMotorVoltage + elevatorInputs.followMotorVoltage) / 2.0; | ||
| if (homeTime <= 0 && avgMotorVoltage > ELEVATOR_STALLING_CURRENT) { | ||
| homing = false; | ||
| elevatorIO.setEncoderToZero(); | ||
| } | ||
| Logger.getInstance().recordOutput("Elevator/Home time", homeTime); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| public void home() { | ||
| homeTime = MIN_ELEVATOR_HOME_TIME; | ||
| homing = true; | ||
| } | ||
| } |
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,27 @@ | ||
| package frc.subsystem.elevator; | ||
|
|
||
| import org.littletonrobotics.junction.AutoLog; | ||
|
|
||
| public interface ElevatorIO { | ||
| @AutoLog | ||
| class ElevatorInputs { | ||
| public double leadMotorPosition; | ||
| public double leadMotorVelocity; | ||
| public double leadMotorVoltage; | ||
| public double leadMotorAmps; | ||
| public double leadMotorTemp; | ||
|
|
||
| public double followMotorPosition; | ||
| public double followMotorVelocity; | ||
| public double followMotorVoltage; | ||
| public double followMotorAmps; | ||
| public double followMotorTemp; | ||
| } | ||
|
|
||
| default void updateInputs(ElevatorInputs inputs) {} | ||
|
|
||
| default void setPosition(double targetPositionInRotations){} | ||
|
|
||
| default void setEncoderToZero() {} | ||
| default void setElevatorVoltage(double voltage) {} | ||
| } |
107 changes: 107 additions & 0 deletions
107
src/main/java/frc/subsystem/elevator/ElevatorIOTalonFX.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,107 @@ | ||
| package frc.subsystem.elevator; | ||
|
|
||
| import com.ctre.phoenix6.BaseStatusSignal; | ||
| import com.ctre.phoenix6.StatusSignal; | ||
| import com.ctre.phoenix6.configs.*; | ||
| import com.ctre.phoenix6.controls.Follower; | ||
| import com.ctre.phoenix6.controls.MotionMagicVoltage; | ||
| import com.ctre.phoenix6.controls.VoltageOut; | ||
| import com.ctre.phoenix6.hardware.TalonFX; | ||
| import com.ctre.phoenix6.signals.GravityTypeValue; | ||
| import frc.robot.Constants; | ||
|
|
||
| import static frc.robot.Constants.ELEVATOR_STALLING_CURRENT; | ||
|
|
||
| public class ElevatorIOTalonFX implements ElevatorIO { | ||
| private final StatusSignal<Double> leadMotorPosition; | ||
| private final StatusSignal<Double> leadMotorVelocity; | ||
| private final StatusSignal<Double> leadMotorVoltage; | ||
| private final StatusSignal<Double> leadMotorAmps; | ||
| private final StatusSignal<Double> leadMotorTemp; | ||
| private final StatusSignal<Double> followMotorPosition; | ||
| private final StatusSignal<Double> followMotorVelocity; | ||
| private final StatusSignal<Double> followMotorVoltage; | ||
| private final StatusSignal<Double> followMotorAmps; | ||
| private final StatusSignal<Double> followMotorTemp; | ||
|
|
||
| private final TalonFX leadMotor; | ||
| private final TalonFX followMotor; | ||
|
|
||
| public ElevatorIOTalonFX() { | ||
| leadMotor = new TalonFX(Constants.Ports.ELEVATOR_LEAD); | ||
| followMotor = new TalonFX(Constants.Ports.ELEVATOR_FOLLOW); | ||
| withVoltage = new VoltageOut(0); | ||
|
|
||
| leadMotorPosition = leadMotor.getPosition(); | ||
| leadMotorVelocity = leadMotor.getVelocity(); | ||
| leadMotorVoltage = leadMotor.getMotorVoltage(); | ||
| leadMotorAmps = leadMotor.getSupplyCurrent(); | ||
| leadMotorTemp = leadMotor.getDeviceTemp(); | ||
|
|
||
| followMotorPosition = followMotor.getPosition(); | ||
| followMotorVelocity = followMotor.getVelocity(); | ||
| followMotorVoltage = followMotor.getMotorVoltage(); | ||
| followMotorAmps = followMotor.getSupplyCurrent(); | ||
| followMotorTemp = followMotor.getDeviceTemp(); | ||
|
|
||
| BaseStatusSignal.setUpdateFrequencyForAll(50, leadMotorPosition, leadMotorVelocity, leadMotorVoltage, leadMotorAmps, leadMotorTemp, followMotorPosition, followMotorVelocity, followMotorVoltage, followMotorAmps, followMotorTemp); | ||
|
|
||
| leadMotor.optimizeBusUtilization(); | ||
| followMotor.optimizeBusUtilization(); | ||
|
|
||
| TalonFXConfiguration motorConfig = new TalonFXConfiguration(); | ||
| MotionMagicConfigs motionMagic = motorConfig.MotionMagic; | ||
|
|
||
| motionMagic.MotionMagicCruiseVelocity = 20; // rotations per second at cruise | ||
| motionMagic.MotionMagicAcceleration = 40; // time to reach max vel | ||
| motionMagic.MotionMagicJerk = 200; // time to reach max accel | ||
|
|
||
| Slot0Configs slot0 = motorConfig.Slot0; | ||
| slot0.kP = 1; | ||
| slot0.GravityType = GravityTypeValue.Elevator_Static; | ||
|
|
||
| FeedbackConfigs fdb = motorConfig.Feedback; | ||
| fdb.SensorToMechanismRatio = Constants.ELEVATOR_INCHES_PER_ROTATION; | ||
|
|
||
| motorConfig = motorConfig.withCurrentLimits(new CurrentLimitsConfigs() | ||
| .withSupplyCurrentLimit(ELEVATOR_STALLING_CURRENT) | ||
| .withSupplyCurrentLimitEnable(true) | ||
| .withStatorCurrentLimitEnable(false)); | ||
|
|
||
| leadMotor.getConfigurator().apply(motorConfig); | ||
| followMotor.getConfigurator().apply(motorConfig); | ||
|
|
||
| followMotor.setControl(new Follower(leadMotor.getDeviceID(), false)); | ||
| } | ||
| private final MotionMagicVoltage motionMagicRequest = new MotionMagicVoltage().withEnableFOC(true).withOverrideBrakeDurNeutral(true); | ||
| public void setPosition(double targetPositionInRotations) { | ||
| leadMotor.setControl(motionMagicRequest.withPosition(targetPositionInRotations)); | ||
| } | ||
|
|
||
| public void updateInputs(ElevatorInputs inputs) { | ||
| BaseStatusSignal.refreshAll(leadMotorPosition, leadMotorVelocity, leadMotorVoltage, leadMotorAmps, leadMotorTemp, followMotorPosition, followMotorVoltage, followMotorAmps, followMotorTemp); | ||
|
|
||
| inputs.leadMotorPosition = leadMotorPosition.getValue() * Constants.ELEVATOR_INCHES_PER_ROTATION; | ||
| inputs.leadMotorVelocity = leadMotorVelocity.getValue(); | ||
| inputs.leadMotorVoltage = leadMotorVoltage.getValue(); | ||
| inputs.leadMotorAmps = leadMotorAmps.getValue(); | ||
| inputs.leadMotorTemp = leadMotorTemp.getValue(); | ||
|
|
||
| inputs.followMotorPosition = followMotorPosition.getValue() * Constants.ELEVATOR_INCHES_PER_ROTATION; | ||
| inputs.followMotorVelocity = followMotorVelocity.getValue(); | ||
| inputs.followMotorVoltage = followMotorVoltage.getValue(); | ||
| inputs.followMotorAmps = followMotorAmps.getValue(); | ||
| inputs.followMotorTemp = followMotorTemp.getValue(); | ||
| } | ||
|
|
||
| public void setEncoderToZero() { | ||
| leadMotor.setPosition(0); | ||
| //elevatorFollower.getEncoder().setPosition(position); | ||
| } | ||
|
|
||
| private final VoltageOut withVoltage = new VoltageOut().withEnableFOC(true).withOverrideBrakeDurNeutral(true); | ||
|
|
||
| public void setElevatorVoltage(double voltage) { | ||
| leadMotor.setControl(withVoltage.withOutput(voltage)); | ||
| } | ||
| } |