This repository has been archived by the owner on Dec 2, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 2
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
12 changed files
with
117 additions
and
131 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 was deleted.
Oops, something went wrong.
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,82 @@ | ||
| // Copyright (c) FIRST and other WPILib contributors. | ||
| // Open Source Software; you can modify and/or share it under the terms of | ||
| // the WPILib BSD license file in the root directory of this project. | ||
|
|
||
| package frc.robot.subsystems.pivot2; | ||
|
|
||
| import java.util.function.DoubleSupplier; | ||
| import org.littletonrobotics.junction.Logger; | ||
|
|
||
| import edu.wpi.first.math.MathUtil; | ||
| import edu.wpi.first.math.controller.ArmFeedforward; | ||
| import edu.wpi.first.math.controller.PIDController; | ||
| import edu.wpi.first.math.controller.ProfiledPIDController; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.trajectory.TrapezoidProfile; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.wpilibj2.command.PIDCommand; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import frc.robot.Constants; | ||
| import frc.robot.Constants.ShooterConstants; | ||
| import frc.util.LoggedTunableNumber; | ||
|
|
||
| /** Add your docs here. */ | ||
| public class Pivot extends SubsystemBase { | ||
| public PivotIO io; | ||
| public PivotIOInputsAutoLogged inputs = new PivotIOInputsAutoLogged(); | ||
|
|
||
| private ArmFeedforward pivotFF = new ArmFeedforward(0.0, ShooterConstants.kGPivot, ShooterConstants.kVPivot, | ||
| ShooterConstants.kAPivot); | ||
| private ProfiledPIDController pivotPID = new ProfiledPIDController(0.0, 0.0, 0.0, | ||
| new TrapezoidProfile.Constraints(ShooterConstants.maxPivotVelocity, ShooterConstants.maxPivotAccel)); | ||
| private LoggedTunableNumber kPPivot = new LoggedTunableNumber("Shooter/kPPivot", ShooterConstants.kPPivot); | ||
|
|
||
| public Pivot(PivotIO io) { | ||
| this.io = io; | ||
|
|
||
| kPPivot.initDefault(ShooterConstants.kPPivot); | ||
| pivotPID.enableContinuousInput(0, Math.PI * 2); | ||
| pivotPID.setTolerance(ShooterConstants.pivotTolerance); | ||
| pivotPID.setP(kPPivot.getAsDouble()); | ||
| } | ||
|
|
||
| public void periodic() { | ||
| io.processInputs(inputs); | ||
| Logger.processInputs("Shooter/Pivot", inputs); | ||
| } | ||
|
|
||
| public Command setPivotTarget(DoubleSupplier radians) { | ||
| return this.run(() -> { | ||
| double volts = pivotPID.calculate(inputs.pivotPosition.getRadians(), radians.getAsDouble()) | ||
| + pivotFF.calculate(pivotPID.getSetpoint().position, | ||
| pivotPID.getSetpoint().velocity) | ||
| ; | ||
| io.setPivotVoltage(volts); | ||
| inputs.pivotAppliedVolts = volts; | ||
| inputs.pivotTargetPosition = Rotation2d.fromRadians(radians.getAsDouble()); | ||
|
|
||
| }); | ||
| } | ||
|
|
||
| public Command setPivotVoltage(DoubleSupplier volts) { | ||
| return this.run( | ||
| () -> { | ||
| io.setPivotVoltage(volts.getAsDouble()); | ||
| }); | ||
| } | ||
|
|
||
| public double getAngleRadians() { | ||
| return inputs.pivotPosition.getRadians() + ShooterConstants.simOffset; | ||
| } | ||
|
|
||
| public double getTargetRadians() { | ||
| return inputs.pivotTargetPosition.getRadians() + ShooterConstants.simOffset; | ||
| } | ||
|
|
||
| public Command resetEncoder() { | ||
| return this.run( | ||
| () -> { | ||
| io.resetEncoder(); | ||
| }); | ||
| } | ||
| } |
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