generated from Frc5572/Java-Template
-
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 remote-tracking branch 'origin/main' into intake-indexer
- Loading branch information
Showing
6 changed files
with
200 additions
and
156 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
63 changes: 63 additions & 0 deletions
63
src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.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,63 @@ | ||
| package frc.robot.subsystems.elevator_wrist; | ||
|
|
||
| import org.littletonrobotics.junction.Logger; | ||
| import edu.wpi.first.math.controller.ArmFeedforward; | ||
| import edu.wpi.first.math.controller.ElevatorFeedforward; | ||
| import edu.wpi.first.math.controller.ProfiledPIDController; | ||
| import edu.wpi.first.math.trajectory.TrapezoidProfile; | ||
| import edu.wpi.first.wpilibj2.command.Subsystem; | ||
| import frc.robot.Constants; | ||
|
|
||
| /** | ||
| * Elevator and Wrist Subsystem | ||
| */ | ||
| public class ElevatorWrist implements Subsystem { | ||
| public ElevatorWristIO io; | ||
| public ElevatorWristInputsAutoLogged inputs = new ElevatorWristInputsAutoLogged(); | ||
|
|
||
|
|
||
| ProfiledPIDController elevatorPIDController = new ProfiledPIDController( | ||
| Constants.ElevatorWristConstants.ELEVATOR_KP, Constants.ElevatorWristConstants.ELEVATOR_KI, | ||
| Constants.ElevatorWristConstants.ELEVATOR_KD, | ||
| new TrapezoidProfile.Constraints(Constants.ElevatorWristConstants.ELEVATOR_MAX_VELOCITY, | ||
| Constants.ElevatorWristConstants.ELEVATOR_MAX_ACCELERATION)); | ||
|
|
||
| ProfiledPIDController wristPIDController = | ||
| new ProfiledPIDController(Constants.ElevatorWristConstants.WRIST_KP, | ||
| Constants.ElevatorWristConstants.WRIST_KI, Constants.ElevatorWristConstants.WRIST_KD, | ||
| new TrapezoidProfile.Constraints(Constants.ElevatorWristConstants.WRIST_MAX_VELOCITY, | ||
| Constants.ElevatorWristConstants.WRIST_MAX_ACCELERATION)); | ||
|
|
||
|
|
||
| private ElevatorFeedforward elevatorFeedForward = new ElevatorFeedforward( | ||
| Constants.ElevatorWristConstants.ELEVATOR_KS, Constants.ElevatorWristConstants.ELEVATOR_KG, | ||
| Constants.ElevatorWristConstants.ELEVATOR_KV); | ||
|
|
||
| private ArmFeedforward wristFeedForward = | ||
| new ArmFeedforward(Constants.ElevatorWristConstants.WRIST_KS, | ||
| Constants.ElevatorWristConstants.WRIST_KG, Constants.ElevatorWristConstants.WRIST_KV); | ||
|
|
||
| public ElevatorWrist(ElevatorWristIO io) { | ||
| this.io = io; | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
|
|
||
| io.updateInputs(inputs); | ||
| Logger.processInputs("ElevatorWrist", inputs); | ||
|
|
||
| double elevatorPIDValue = | ||
| elevatorPIDController.calculate(inputs.elevatorRelativeEncRawValue); | ||
| double wristPIDValue = wristPIDController.calculate(inputs.wristAbsoluteEncRawValue); | ||
|
|
||
| double elevatorFeedForwardValue = | ||
| elevatorFeedForward.calculate(0, 0, wristPIDController.getPeriod()); | ||
|
|
||
| double wristFeedForwardValue = | ||
| wristFeedForward.calculate(0, 0, wristPIDController.getPeriod()); | ||
|
|
||
| io.setElevatorVoltage(elevatorFeedForwardValue + elevatorPIDValue); | ||
| io.setWristVoltage(wristFeedForwardValue + wristPIDValue); | ||
| } | ||
| } |
26 changes: 26 additions & 0 deletions
26
src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWristIO.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,26 @@ | ||
| package frc.robot.subsystems.elevator_wrist; | ||
|
|
||
| import org.littletonrobotics.junction.AutoLog; | ||
|
|
||
| /** | ||
| * Elevator and wrist IO class | ||
| */ | ||
| public interface ElevatorWristIO { | ||
| /** | ||
| * Elevator and wrist inputs | ||
| */ | ||
| @AutoLog | ||
| public static class ElevatorWristInputs { | ||
| public double elevatorRelativeEncRawValue; | ||
| public boolean topLimitSwitch; | ||
| public boolean bottomLimitSwitch; | ||
| public double wristAbsoluteEncRawValue; | ||
| } | ||
|
|
||
| public default void updateInputs(ElevatorWristInputs inputs) {} | ||
|
|
||
| public default void setElevatorVoltage(double voltage) {} | ||
|
|
||
| public default void setWristVoltage(double voltage) {} | ||
|
|
||
| } |
70 changes: 70 additions & 0 deletions
70
src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWristReal.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,70 @@ | ||
| package frc.robot.subsystems.elevator_wrist; | ||
|
|
||
|
|
||
| import com.ctre.phoenix6.controls.VoltageOut; | ||
| import com.ctre.phoenix6.hardware.TalonFX; | ||
| import com.ctre.phoenix6.signals.NeutralModeValue; | ||
| import com.revrobotics.AbsoluteEncoder; | ||
| import com.revrobotics.CANSparkBase.IdleMode; | ||
| import com.revrobotics.CANSparkLowLevel.MotorType; | ||
| import com.revrobotics.CANSparkMax; | ||
| import com.revrobotics.SparkAbsoluteEncoder.Type; | ||
| import edu.wpi.first.wpilibj.DigitalInput; | ||
| import edu.wpi.first.wpilibj.Encoder; | ||
| import frc.robot.Constants; | ||
|
|
||
| /** | ||
| * Elevator and wrist real robot class | ||
| */ | ||
| public class ElevatorWristReal implements ElevatorWristIO { | ||
| public final TalonFX elevatorMotor = new TalonFX(Constants.Motors.ElevatorWrist.TALON_ID); | ||
|
|
||
| public final Encoder elevatorRelativeEnc = | ||
| new Encoder(Constants.ElevatorWristConstants.ELEVATOR_ENC_CHANNEL_A, | ||
| Constants.ElevatorWristConstants.ELEVATOR_ENC_CHANNEL_B); | ||
|
|
||
| public final CANSparkMax wristMotor = | ||
| new CANSparkMax(Constants.Motors.ElevatorWrist.NEO_ID, MotorType.kBrushless); | ||
| public final DigitalInput topLimitSwitch = | ||
| new DigitalInput(Constants.ElevatorWristConstants.TOP_LIMIT_SWITCH_PORT); | ||
| public final DigitalInput bottomLimitSwitch = | ||
| new DigitalInput(Constants.ElevatorWristConstants.BOTTOM_LIMIT_SWITCH_PORT); | ||
|
|
||
| public final AbsoluteEncoder wristAbsoluteEnc; | ||
|
|
||
| private VoltageOut voltage = new VoltageOut(0); | ||
|
|
||
| /** | ||
| * Constructor for elevator wrist real class | ||
| */ | ||
| public ElevatorWristReal() { | ||
| wristAbsoluteEnc = wristMotor.getAbsoluteEncoder(Type.kDutyCycle); | ||
|
|
||
| elevatorMotor.setNeutralMode(NeutralModeValue.Brake); | ||
| elevatorMotor.setInverted(false); | ||
|
|
||
| wristMotor.setIdleMode(IdleMode.kBrake); | ||
| wristMotor.setInverted(false); | ||
|
|
||
| } | ||
|
|
||
|
|
||
| @Override | ||
| public void updateInputs(ElevatorWristInputs inputs) { | ||
| inputs.topLimitSwitch = topLimitSwitch.get(); | ||
| inputs.bottomLimitSwitch = bottomLimitSwitch.get(); | ||
| inputs.elevatorRelativeEncRawValue = elevatorRelativeEnc.get(); | ||
| inputs.wristAbsoluteEncRawValue = wristAbsoluteEnc.getPosition(); | ||
| } | ||
|
|
||
| @Override | ||
| public void setElevatorVoltage(double v) { | ||
| elevatorMotor.setControl(voltage.withOutput(v)); | ||
| } | ||
|
|
||
| @Override | ||
| public void setWristVoltage(double v) { | ||
| wristMotor.setVoltage(v); | ||
| } | ||
|
|
||
| } |
This file was deleted.
Oops, something went wrong.