Add rough simulation support

FRC-Team-620 · Jan 27, 2024 · 479914e · 479914e
1 parent 7dd91e9
commit 479914e
Showing 1 changed file with 23 additions and 0 deletions.
diff --git 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());
+	}
 }