diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java index de878eaf..0f480b3c 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java @@ -1,10 +1,15 @@ package org.jmhsrobotics.frc2024.subsystems.shooter; +import org.jmhsrobotics.warcore.rev.RevEncoderSimWrapper; + import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ShooterSubsystem extends SubsystemBase { @@ -17,6 +22,9 @@ public class ShooterSubsystem extends SubsystemBase { public ShooterSubsystem() { // Initializes motor(s) initializeMotors(); + if(RobotBase.isSimulation()){ + initSim(); + } } private void initializeMotors() { @@ -38,4 +46,19 @@ public double getRPM() { public void setSpeed(double speed) { motor1.set(speed); } + + FlywheelSim flywheelSim; + RevEncoderSimWrapper encSim; + public void initSim(){ + flywheelSim = new FlywheelSim(DCMotor.getNEO(1), 1, 1); + encSim = RevEncoderSimWrapper.create(motor1); + } + + @Override + public void simulationPeriodic() { + double motorVolts = motor1.get()*12; + flywheelSim.setInputVoltage(motorVolts); + flywheelSim.update(0.2); + encSim.setVelocity(flywheelSim.getAngularVelocityRPM()); + } }