From fa9b013eb88e66b08c20440d1e949f4b7f9e7fb0 Mon Sep 17 00:00:00 2001 From: Stu-PID Date: Thu, 29 Feb 2024 18:33:12 -0500 Subject: [PATCH] Remove Flywheel from Proto (Chassis) --- src/main/java/frc/robot/RobotContainer.java | 11 -- .../robot/subsystems/flywheel/Flywheel.java | 110 ------------------ .../robot/subsystems/flywheel/FlywheelIO.java | 41 ------- .../subsystems/flywheel/FlywheelIOSim.java | 68 ----------- .../flywheel/FlywheelIOSparkMax.java | 89 -------------- .../flywheel/FlywheelIOTalonFX.java | 98 ---------------- 6 files changed, 417 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/flywheel/Flywheel.java delete mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java delete mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java delete mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java delete mode 100644 src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 51f191e..3ac2b66 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -105,15 +105,6 @@ public RobotContainer() { m_mast = null; break; } - - // flywheel = new Flywheel(new FlywheelIOSparkMax()); - // drive = new Drive( - // new GyroIOPigeon2(true), - // new ModuleIOTalonFX(0), - // new ModuleIOTalonFX(1), - // new ModuleIOTalonFX(2), - // new ModuleIOTalonFX(3)); - // flywheel = new Flywheel(new FlywheelIOTalonFX()); break; case SIM: @@ -125,7 +116,6 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); - // flywheel = new Flywheel(new FlywheelIOSim()); m_intake = null; m_feeder = null; m_incrementer = null; @@ -142,7 +132,6 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - // flywheel = new Flywheel(new FlywheelIO() {}); m_intake = null; m_feeder = null; m_incrementer = null; diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java deleted file mode 100644 index 6056b24..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java +++ /dev/null @@ -1,110 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// 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 static edu.wpi.first.units.Units.*; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -public class Flywheel extends SubsystemBase { - private final FlywheelIO io; - private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); - private final SimpleMotorFeedforward ffModel; - private final SysIdRoutine sysId; - - /** Creates a new Flywheel. */ - public Flywheel(FlywheelIO io) { - this.io = io; - - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (Constants.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; - } - - // Configure SysId - sysId = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - null, - null, - (state) -> Logger.recordOutput("Flywheel/SysIdState", state.toString())), - new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Volts)), null, this)); - } - - @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 a command to run a quasistatic test in the specified direction. */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysId.quasistatic(direction); - } - - /** Returns a command to run a dynamic test in the specified direction. */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysId.dynamic(direction); - } - - /** 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; - } -} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java deleted file mode 100644 index 98f7624..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// 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 org.littletonrobotics.junction.AutoLog; - -public interface FlywheelIO { - @AutoLog - public static class FlywheelIOInputs { - 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(FlywheelIOInputs 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) {} -} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java deleted file mode 100644 index 32ffa6f..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// 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 edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; - -public class FlywheelIOSim implements FlywheelIO { - private FlywheelSim sim = new FlywheelSim(DCMotor.getNEO(1), 1.5, 0.004); - private PIDController pid = new PIDController(0.0, 0.0, 0.0); - - private boolean closedLoop = false; - private double ffVolts = 0.0; - private double appliedVolts = 0.0; - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - if (closedLoop) { - appliedVolts = - MathUtil.clamp(pid.calculate(sim.getAngularVelocityRadPerSec()) + ffVolts, -12.0, 12.0); - sim.setInputVoltage(appliedVolts); - } - - sim.update(0.02); - - inputs.positionRad = 0.0; - inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec(); - inputs.appliedVolts = appliedVolts; - inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()}; - } - - @Override - public void setVoltage(double volts) { - closedLoop = false; - appliedVolts = volts; - sim.setInputVoltage(volts); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - closedLoop = true; - pid.setSetpoint(velocityRadPerSec); - this.ffVolts = ffVolts; - } - - @Override - public void stop() { - setVoltage(0.0); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - pid.setPID(kP, kI, kD); - } -} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java deleted file mode 100644 index 787f369..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkMax.java +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// 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; - -/** - * 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 = 1.5; - - private final CANSparkMax leader = new CANSparkMax(0, MotorType.kBrushless); - private final CANSparkMax follower = new CANSparkMax(1, MotorType.kBrushless); - private final RelativeEncoder encoder = leader.getEncoder(); - private final SparkPIDController pid = leader.getPIDController(); - - public FlywheelIOSparkMax() { - leader.restoreFactoryDefaults(); - follower.restoreFactoryDefaults(); - - leader.setCANTimeout(250); - follower.setCANTimeout(250); - - leader.setInverted(false); - follower.follow(leader, false); - - leader.enableVoltageCompensation(12.0); - leader.setSmartCurrentLimit(30); - - leader.burnFlash(); - follower.burnFlash(); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / GEAR_RATIO); - inputs.velocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); - inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); - inputs.currentAmps = new double[] {leader.getOutputCurrent(), follower.getOutputCurrent()}; - } - - @Override - public void setVoltage(double volts) { - leader.setVoltage(volts); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - pid.setReference( - Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, - ControlType.kVelocity, - 0, - ffVolts, - ArbFFUnits.kVoltage); - } - - @Override - public void stop() { - leader.stopMotor(); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - pid.setP(kP, 0); - pid.setI(kI, 0); - pid.setD(kD, 0); - pid.setFF(0, 0); - } -} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java deleted file mode 100644 index 9c81daf..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// 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.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; - -public class FlywheelIOTalonFX implements FlywheelIO { - private static final double GEAR_RATIO = 1.5; - - private final TalonFX leader = new TalonFX(0); - private final TalonFX follower = new TalonFX(1); - - private final StatusSignal leaderPosition = leader.getPosition(); - private final StatusSignal leaderVelocity = leader.getVelocity(); - private final StatusSignal leaderAppliedVolts = leader.getMotorVoltage(); - private final StatusSignal leaderCurrent = leader.getStatorCurrent(); - private final StatusSignal followerCurrent = follower.getStatorCurrent(); - - public FlywheelIOTalonFX() { - var config = new TalonFXConfiguration(); - config.CurrentLimits.StatorCurrentLimit = 30.0; - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.MotorOutput.NeutralMode = NeutralModeValue.Coast; - leader.getConfigurator().apply(config); - follower.getConfigurator().apply(config); - follower.setControl(new Follower(leader.getDeviceID(), false)); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); - leader.optimizeBusUtilization(); - follower.optimizeBusUtilization(); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - BaseStatusSignal.refreshAll( - leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); - inputs.positionRad = Units.rotationsToRadians(leaderPosition.getValueAsDouble()) / GEAR_RATIO; - inputs.velocityRadPerSec = - Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / GEAR_RATIO; - inputs.appliedVolts = leaderAppliedVolts.getValueAsDouble(); - inputs.currentAmps = - new double[] {leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()}; - } - - @Override - public void setVoltage(double volts) { - leader.setControl(new VoltageOut(volts)); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - leader.setControl( - new VelocityVoltage( - Units.radiansToRotations(velocityRadPerSec), - 0.0, - true, - ffVolts, - 0, - false, - false, - false)); - } - - @Override - public void stop() { - leader.stopMotor(); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - var config = new Slot0Configs(); - config.kP = kP; - config.kI = kI; - config.kD = kD; - leader.getConfigurator().apply(config); - } -}