-
Notifications
You must be signed in to change notification settings - Fork 1
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
1 parent
4c885a1
commit 33b7082
Showing
9 changed files
with
383 additions
and
32 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
91 changes: 91 additions & 0 deletions
91
src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.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,91 @@ | ||
| // Copyright 2021-2024 FRC 6328, FRC 5829 | ||
| // http://github.com/Mechanical-Advantage | ||
| // | ||
| // This program is free software; you can redistribute it and/or | ||
| // modify it under the terms of the GNU General Public License | ||
| // version 3 as published by the Free Software Foundation or | ||
| // available in the root directory of this project. | ||
| // | ||
| // This program is distributed in the hope that it will be useful, | ||
| // but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
| // GNU General Public License for more details. | ||
|
|
||
| package frc.robot.subsystems.flywheel; | ||
|
|
||
| import com.revrobotics.CANSparkBase.ControlType; | ||
| import com.revrobotics.CANSparkLowLevel.MotorType; | ||
| import com.revrobotics.CANSparkMax; | ||
| import com.revrobotics.RelativeEncoder; | ||
| import com.revrobotics.SparkPIDController; | ||
| import com.revrobotics.SparkPIDController.ArbFFUnits; | ||
| import edu.wpi.first.math.util.Units; | ||
| import frc.robot.Constants.FlywheelConstants; | ||
|
|
||
| /** | ||
| * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with | ||
| * "CANSparkFlex". | ||
| */ | ||
| public class FlywheelIOSparkMax implements FlywheelIO { | ||
| private static final double GEAR_RATIO = 52.0 / 34.0; // May be reciprocal | ||
|
|
||
| private final CANSparkMax topShooterMotor = | ||
| new CANSparkMax(FlywheelConstants.kTopFlywheelSparkMaxCanId, MotorType.kBrushless); | ||
| private final RelativeEncoder topShooterEncoder = topShooterMotor.getEncoder(); | ||
| private final SparkPIDController topShooterPID = topShooterMotor.getPIDController(); | ||
|
|
||
| private final CANSparkMax bottomShooterMotor = | ||
| new CANSparkMax(FlywheelConstants.kBottomFlywheelSparkMaxCanId, MotorType.kBrushless); | ||
| private final RelativeEncoder bottomShooterEncoder = bottomShooterMotor.getEncoder(); | ||
| private final SparkPIDController bottomShooterPID = bottomShooterMotor.getPIDController(); | ||
|
|
||
| public FlywheelIOSparkMax() { | ||
| topShooterMotor.restoreFactoryDefaults(); | ||
|
|
||
| topShooterMotor.setCANTimeout(250); | ||
|
|
||
| topShooterMotor.setInverted(false); | ||
|
|
||
| topShooterMotor.enableVoltageCompensation(12.0); | ||
| topShooterMotor.setSmartCurrentLimit(30); | ||
|
|
||
| topShooterMotor.burnFlash(); | ||
| } | ||
|
|
||
| @Override | ||
| public void updateInputs(FlywheelIOInputs inputs) { | ||
| inputs.positionRad = Units.rotationsToRadians(topShooterEncoder.getPosition() / GEAR_RATIO); | ||
| inputs.velocityRadPerSec = | ||
| Units.rotationsPerMinuteToRadiansPerSecond(topShooterEncoder.getVelocity() / GEAR_RATIO); | ||
| inputs.appliedVolts = topShooterMotor.getAppliedOutput() * topShooterMotor.getBusVoltage(); | ||
| inputs.currentAmps = new double[] {topShooterMotor.getOutputCurrent()}; | ||
| } | ||
|
|
||
| @Override | ||
| public void setVoltage(double volts) { | ||
| topShooterMotor.setVoltage(volts); | ||
| } | ||
|
|
||
| @Override | ||
| public void setVelocity(double velocityRadPerSec, double ffVolts) { | ||
| topShooterPID.setReference( | ||
| Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, | ||
| ControlType.kVelocity, | ||
| 0, | ||
| ffVolts, | ||
| ArbFFUnits.kVoltage); | ||
| } | ||
|
|
||
| @Override | ||
| public void stop() { | ||
| topShooterMotor.stopMotor(); | ||
| } | ||
|
|
||
| @Override | ||
| public void configurePID(double kP, double kI, double kD) { | ||
| topShooterPID.setP(kP, 0); | ||
| topShooterPID.setI(kI, 0); | ||
| topShooterPID.setD(kD, 0); | ||
| topShooterPID.setFF(0, 0); | ||
| } | ||
| } |
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,85 @@ | ||
| // Copyright 2021-2024 FRC 6328, FRC 5829 | ||
| // http://github.com/Mechanical-Advantage | ||
| // | ||
| // This program is free software; you can redistribute it and/or | ||
| // modify it under the terms of the GNU General Public License | ||
| // version 3 as published by the Free Software Foundation or | ||
| // available in the root directory of this project. | ||
| // | ||
| // This program is distributed in the hope that it will be useful, | ||
| // but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
| // GNU General Public License for more details. | ||
|
|
||
| package frc.robot.subsystems.intake; | ||
|
|
||
| import edu.wpi.first.math.controller.SimpleMotorFeedforward; | ||
| import edu.wpi.first.math.util.Units; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import frc.robot.Constants.EnvironmentalConstants; | ||
| import org.littletonrobotics.junction.AutoLogOutput; | ||
| import org.littletonrobotics.junction.Logger; | ||
|
|
||
| public class Intake extends SubsystemBase { | ||
| private final IntakeIO io; | ||
| private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); | ||
| private final SimpleMotorFeedforward ffModel; | ||
|
|
||
| /** Creates a new Flywheel. */ | ||
| public Intake(IntakeIO io) { | ||
| this.io = io; | ||
|
|
||
| // Switch constants based on mode (the physics simulator is treated as a | ||
| // separate robot with different tuning) | ||
| switch (EnvironmentalConstants.currentMode) { | ||
| case REAL: | ||
| case REPLAY: | ||
| ffModel = new SimpleMotorFeedforward(0.1, 0.05); | ||
| io.configurePID(1.0, 0.0, 0.0); | ||
| break; | ||
| case SIM: | ||
| ffModel = new SimpleMotorFeedforward(0.0, 0.03); | ||
| io.configurePID(0.5, 0.0, 0.0); | ||
| break; | ||
| default: | ||
| ffModel = new SimpleMotorFeedforward(0.0, 0.0); | ||
| break; | ||
| } | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| io.updateInputs(inputs); | ||
| Logger.processInputs("Flywheel", inputs); | ||
| } | ||
|
|
||
| /** Run open loop at the specified voltage. */ | ||
| public void runVolts(double volts) { | ||
| io.setVoltage(volts); | ||
| } | ||
|
|
||
| /** Run closed loop at the specified velocity. */ | ||
| public void runVelocity(double velocityRPM) { | ||
| var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); | ||
| io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec)); | ||
|
|
||
| // Log flywheel setpoint | ||
| Logger.recordOutput("Flywheel/SetpointRPM", velocityRPM); | ||
| } | ||
|
|
||
| /** Stops the flywheel. */ | ||
| public void stop() { | ||
| io.stop(); | ||
| } | ||
|
|
||
| /** Returns the current velocity in RPM. */ | ||
| @AutoLogOutput | ||
| public double getVelocityRPM() { | ||
| return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); | ||
| } | ||
|
|
||
| /** Returns the current velocity in radians per second. */ | ||
| public double getCharacterizationVelocity() { | ||
| return inputs.velocityRadPerSec; | ||
| } | ||
| } |
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,41 @@ | ||
| // Copyright 2021-2024 FRC 6328, FRC 5829 | ||
| // http://github.com/Mechanical-Advantage | ||
| // | ||
| // This program is free software; you can redistribute it and/or | ||
| // modify it under the terms of the GNU General Public License | ||
| // version 3 as published by the Free Software Foundation or | ||
| // available in the root directory of this project. | ||
| // | ||
| // This program is distributed in the hope that it will be useful, | ||
| // but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
| // GNU General Public License for more details. | ||
|
|
||
| package frc.robot.subsystems.intake; | ||
|
|
||
| import org.littletonrobotics.junction.AutoLog; | ||
|
|
||
| public interface IntakeIO { | ||
| @AutoLog | ||
| public static class IntakeIOInputs { | ||
| public double positionRad = 0.0; | ||
| public double velocityRadPerSec = 0.0; | ||
| public double appliedVolts = 0.0; | ||
| public double[] currentAmps = new double[] {}; | ||
| } | ||
|
|
||
| /** Updates the set of loggable inputs. */ | ||
| public default void updateInputs(IntakeIOInputs inputs) {} | ||
|
|
||
| /** Run open loop at the specified voltage. */ | ||
| public default void setVoltage(double volts) {} | ||
|
|
||
| /** Run closed loop at the specified velocity. */ | ||
| public default void setVelocity(double velocityRadPerSec, double ffVolts) {} | ||
|
|
||
| /** Stop in open loop. */ | ||
| public default void stop() {} | ||
|
|
||
| /** Set velocity PID constants. */ | ||
| public default void configurePID(double kP, double kI, double kD) {} | ||
| } |
Oops, something went wrong.