-
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.
This was the biggest merge of my life
- Loading branch information
1 parent
fcc2a9d
commit abbdc6d
Showing
16 changed files
with
353 additions
and
8 deletions.
There are no files selected for viewing
Binary file not shown.
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 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,68 @@ | ||
| package frc.robot.subsystems.intake; | ||
|
|
||
| import edu.wpi.first.math.controller.ProfiledPIDController; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.util.Units; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.wpilibj2.command.Commands; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import frc.robot.subsystems.intake.pivot.IntakePivotIO; | ||
| import frc.robot.subsystems.intake.pivot.IntakePivotInputsAutoLogged; | ||
| import frc.robot.subsystems.intake.roller.RollerIO; | ||
| import frc.robot.subsystems.intake.roller.RollerInputsAutoLogged; | ||
| import org.littletonrobotics.junction.Logger; | ||
|
|
||
| public class Intake extends SubsystemBase { | ||
| private RollerIO m_rollerIO; | ||
| private IntakePivotIO m_pivotIO; | ||
|
|
||
| public final RollerInputsAutoLogged m_rollerInputs; | ||
| public final IntakePivotInputsAutoLogged m_pivotInputs; | ||
|
|
||
| private ProfiledPIDController m_pivotController; | ||
|
|
||
| public Intake(RollerIO io, IntakePivotIO pivotIO, ProfiledPIDController pivotController) { | ||
| m_rollerIO = io; | ||
| m_pivotIO = pivotIO; | ||
| m_pivotController = pivotController; | ||
|
|
||
| m_rollerInputs = new RollerInputsAutoLogged(); | ||
| m_pivotInputs = new IntakePivotInputsAutoLogged(); | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| m_pivotIO.updateInputs(m_pivotInputs); | ||
| m_rollerIO.updateInputs(m_rollerInputs); | ||
|
|
||
| double pivotPidVoltage = m_pivotController.calculate(m_pivotInputs.curAngle); | ||
| m_pivotIO.setVoltage(pivotPidVoltage); | ||
|
|
||
| Logger.processInputs("Intake/Pivot", m_pivotInputs); | ||
| Logger.processInputs("Intake/Roller", m_rollerInputs); | ||
|
|
||
| Logger.recordOutput("Intake/Pivot/PIDVoltage", pivotPidVoltage); | ||
| Logger.recordOutput("Intake/Pivot/DesiredAngle", m_pivotController.getSetpoint().position); | ||
| Logger.recordOutput("Intake/CurrentAngle", Units.radiansToDegrees(pivotPidVoltage)); | ||
| } | ||
|
|
||
| public void setRollerVoltage(double voltage) { | ||
| m_rollerIO.setVoltage(voltage); | ||
| } | ||
|
|
||
| public void setPivotAngle(Rotation2d angle) { | ||
| m_pivotController.setGoal(angle.getRadians()); | ||
| } | ||
|
|
||
| public Command runRollerCommand(double voltage) { | ||
| return Commands.runOnce(() -> m_rollerIO.setVoltage(voltage)); | ||
| } | ||
|
|
||
| public Command setPivotAngleCommand(Rotation2d angle) { | ||
| return Commands.runOnce(() -> m_pivotController.setGoal(angle.getRadians())); | ||
| } | ||
|
|
||
| public Rotation2d getPivotAngle() { | ||
| return Rotation2d.fromRadians(m_pivotInputs.curAngle); | ||
| } | ||
| } |
19 changes: 19 additions & 0 deletions
19
src/main/java/frc/robot/subsystems/intake/pivot/IntakePivotIO.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,19 @@ | ||
| package frc.robot.subsystems.intake.pivot; | ||
|
|
||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import frc.lib.advantagekit.LoggedIO; | ||
| import org.littletonrobotics.junction.AutoLog; | ||
|
|
||
| public interface IntakePivotIO extends LoggedIO<IntakePivotIO.IntakePivotInputs> { | ||
| @AutoLog | ||
| public static class IntakePivotInputs { | ||
| public double curAngle; | ||
| public double curVelocity; | ||
| public double curVoltage; | ||
| public double curAmps; | ||
| } | ||
|
|
||
| public void setVoltage(double voltage); | ||
|
|
||
| public Rotation2d getCurrentAngle(); | ||
| } |
37 changes: 37 additions & 0 deletions
37
src/main/java/frc/robot/subsystems/intake/pivot/IntakePivotIONeo.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,37 @@ | ||
| package frc.robot.subsystems.intake.pivot; | ||
|
|
||
| import com.revrobotics.AbsoluteEncoder; | ||
| import com.revrobotics.CANSparkMax; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.util.Units; | ||
| import frc.robot.Constants.IntakeConstants; | ||
|
|
||
| public class IntakePivotIONeo implements IntakePivotIO { | ||
| private CANSparkMax m_motor; | ||
| private AbsoluteEncoder m_encoder; | ||
|
|
||
| public IntakePivotIONeo(int port) { | ||
| m_motor = new CANSparkMax(port, CANSparkMax.MotorType.kBrushless); | ||
| m_encoder = m_motor.getAbsoluteEncoder(); | ||
| m_encoder.setPositionConversionFactor(IntakeConstants.kPivotGearRatio); | ||
| } | ||
|
|
||
| @Override | ||
| public void updateInputs(IntakePivotInputs inputs) { | ||
| inputs.curAngle = Units.rotationsToRadians(m_encoder.getPosition()); | ||
| inputs.curVelocity = | ||
| Units.rotationsPerMinuteToRadiansPerSecond(m_motor.getEncoder().getVelocity()); | ||
| inputs.curVoltage = m_motor.getAppliedOutput(); | ||
| inputs.curAmps = m_motor.getOutputCurrent(); | ||
| } | ||
|
|
||
| @Override | ||
| public void setVoltage(double voltage) { | ||
| m_motor.setVoltage(voltage); | ||
| } | ||
|
|
||
| @Override | ||
| public Rotation2d getCurrentAngle() { | ||
| return Rotation2d.fromRotations(m_encoder.getPosition()); | ||
| } | ||
| } |
55 changes: 55 additions & 0 deletions
55
src/main/java/frc/robot/subsystems/intake/pivot/IntakePivotIOSim.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 frc.robot.subsystems.intake.pivot; | ||
|
|
||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.system.plant.DCMotor; | ||
| import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; | ||
| import frc.robot.Constants.IntakeConstants; | ||
|
|
||
| public class IntakePivotIOSim implements IntakePivotIO { | ||
| private SingleJointedArmSim m_sim; | ||
| private double m_voltage; | ||
|
|
||
| public IntakePivotIOSim() { | ||
| DCMotor gearbox = DCMotor.getNeo550(1); | ||
| double gearing = 24.2391; | ||
| double armLength = 0.21; | ||
| double jKgMetersSquared = SingleJointedArmSim.estimateMOI(armLength, 4); | ||
| double minAngle = IntakeConstants.kMinAngle.getDegrees(); | ||
| double maxAngle = IntakeConstants.kMaxAngle.getDegrees(); | ||
| double startingAngle = IntakeConstants.kHomeAngle.getDegrees(); | ||
|
|
||
| m_sim = | ||
| new SingleJointedArmSim( | ||
| gearbox, | ||
| gearing, | ||
| jKgMetersSquared, | ||
| armLength, | ||
| minAngle, | ||
| maxAngle, | ||
| false, | ||
| startingAngle); | ||
|
|
||
| m_voltage = 0.0; | ||
| } | ||
|
|
||
| @Override | ||
| public void updateInputs(IntakePivotInputs inputs) { | ||
| m_sim.update(0.02); | ||
|
|
||
| inputs.curAngle = getCurrentAngle().getRadians(); | ||
| inputs.curVoltage = m_voltage; | ||
| inputs.curVelocity = m_sim.getVelocityRadPerSec(); | ||
| inputs.curAmps = m_sim.getCurrentDrawAmps(); | ||
| } | ||
|
|
||
| @Override | ||
| public void setVoltage(double voltage) { | ||
| m_sim.setInputVoltage(voltage); | ||
| m_voltage = voltage; | ||
| } | ||
|
|
||
| @Override | ||
| public Rotation2d getCurrentAngle() { | ||
| return Rotation2d.fromRadians(m_sim.getAngleRads()); | ||
| } | ||
| } |
15 changes: 15 additions & 0 deletions
15
src/main/java/frc/robot/subsystems/intake/roller/RollerIO.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,15 @@ | ||
| package frc.robot.subsystems.intake.roller; | ||
|
|
||
| import frc.lib.advantagekit.LoggedIO; | ||
| import org.littletonrobotics.junction.AutoLog; | ||
|
|
||
| public interface RollerIO extends LoggedIO<RollerIO.RollerInputs> { | ||
| @AutoLog | ||
| public static class RollerInputs { | ||
| public double curVelocity; | ||
| public double curVoltage; | ||
| public double curAmps; | ||
| } | ||
|
|
||
| public void setVoltage(double voltage); | ||
| } |
Oops, something went wrong.