Simulation

.gitignore

@@ -159,3 +159,5 @@ imgui.ini
 
 
 # End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode
+
+/.idea/

build.gradle

@@ -38,7 +38,7 @@ deploy {
 }
 
 // Set this to true to enable desktop support.
-def includeDesktopSupport = false
+def includeDesktopSupport = true
 
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 4.

src/main/java/frc/robot/lib/FancyLog.java

@@ -12,7 +12,7 @@ public static enum LogSystem {
    }
 
    public static final LogSystem enabledLogs[] = {
-      LogSystem.SUBSYSTEM_DRIVE, LogSystem.COMMAND_OI_DRIVE,
+      LogSystem.SUBSYSTEM_DRIVE, LogSystem.COMMAND_OI_DRIVE, LogSystem.SUBSYSTEM_SHOOT,
    };
 
    public static void Log(LogLevel level, LogSystem where, String message) {

src/main/java/frc/robot/lib/VirtualCANSparkMax.java

@@ -0,0 +1,129 @@
+package frc.robot.lib;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import com.revrobotics.*;
+import edu.wpi.first.wpilibj.RobotBase;
+
+public class VirtualCANSparkMax {
+   private final CANSparkMax physicalMotor;
+   private final CANEncoder physicalEncoder;
+   private final CANPIDController physicalController;
+
+   private SimDevice simDevice;
+   private SimDouble simMotorOutput;
+   private SimDouble simMotorVelocity;
+
+   private double PID_setPoint;
+   private double PID_lastValue;
+   private double PID_totalValue;
+   private double PID_P;
+   private double PID_I;
+   private double PID_D;
+   private double PID_FF;
+   private double PID_minOutput;
+   private double PID_maxOutput;
+   private double PID_velocity;
+
+   public VirtualCANSparkMax(int port) {
+      if (RobotBase.isReal()) {
+         physicalMotor = new CANSparkMax(port, CANSparkMaxLowLevel.MotorType.kBrushless);
+         physicalEncoder = new CANEncoder(physicalMotor);
+         physicalController = physicalMotor.getPIDController();
+         simDevice = null;
+         simMotorOutput = null;
+         simMotorVelocity = null;
+      } else {
+         simDevice = SimDevice.create("CAN Spark MAX", port);
+         if (simDevice != null) {
+           simMotorOutput = simDevice.createDouble("Motor Output", true, 0.0);
+           simMotorVelocity = simDevice.createDouble("Motor Velocity", true, 0.0);
+         }
+         physicalMotor = null;
+         physicalEncoder = null;
+         physicalController = null;
+      }
+   }
+
+   public void periodic() {
+      if (!RobotBase.isReal()) {
+         double encoderSpeed = getVelocity();
+         double pValue = (PID_setPoint - encoderSpeed) * PID_P;
+         PID_totalValue += pValue;
+         double iValue = PID_totalValue * PID_I;
+         double dValue = (pValue - PID_lastValue) * PID_D;
+         double ffValue = PID_setPoint * PID_FF;
+         PID_lastValue = pValue;
+         double speed = Math.min(Math.max(pValue + iValue + dValue + ffValue, PID_minOutput), PID_maxOutput);
+         simMotorOutput.set(speed);
+         simMotorVelocity.set(PID_velocity);
+         PID_velocity *= 0.9; // Very accurate motor simulation
+         PID_velocity += speed*1000;
+      }
+   }
+
+   public void setP(double kP) {
+      if (RobotBase.isReal()) {
+         physicalController.setP(kP);
+      } else {
+         PID_P = kP;
+      }
+   }
+   public void setI(double kI) {
+      if (RobotBase.isReal()) {
+         physicalController.setI(kI);
+      } else {
+         PID_I = kI;
+      }
+   }
+   public void setD(double kD) {
+      if (RobotBase.isReal()) {
+         physicalController.setD(kD);
+      } else {
+         PID_D = kD;
+      }
+   }
+   public void setIZone(double kIz) {
+      if (RobotBase.isReal()) {
+         physicalController.setIZone(kIz);
+      }
+   }
+   public void setFF(double kFF) {
+      if (RobotBase.isReal()) {
+         physicalController.setFF(kFF);
+      } else {
+         PID_FF = kFF;
+      }
+   }
+   public void setOutputRange(double kMinOutput, double kMaxOutput) {
+      if (RobotBase.isReal()) {
+         physicalController.setOutputRange(kMinOutput, kMaxOutput);
+      } else {
+         PID_minOutput = kMinOutput;
+         PID_maxOutput = kMaxOutput;
+      }
+   }
+   public void setReference(double targetSpeed) {
+      if (RobotBase.isReal()) {
+         physicalController.setReference(targetSpeed, ControlType.kVelocity);
+      } else {
+         PID_setPoint = targetSpeed;
+      }
+   }
+   public double getVelocity() {
+      if (RobotBase.isReal()) {
+         return physicalEncoder.getVelocity();
+      } else {
+         return PID_velocity;
+      }
+   }
+   public double getMotorTemperature() {
+      if (RobotBase.isReal()) {
+         return physicalMotor.getMotorTemperature();
+      } else {
+         return 500000; // Oh nO iTs toO Hot
+      }
+   }
+}

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -2,14 +2,14 @@
 
 import com.cuforge.libcu.Lasershark;
 import com.revrobotics.CANEncoder;
-import com.revrobotics.CANPIDController;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.ControlType;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Ports;
+import frc.robot.lib.VirtualCANSparkMax;
 
 /**
  * Ball shooter code
@@ -44,9 +44,7 @@ public static final class ShooterConstants {
 
    private double shooterSpeed = ShooterConstants.kDefaultShooterSpeed;
    private double manualShooterSpeed = ShooterConstants.kDefaultShooterSpeed;
-   private CANSparkMax shooterMotor = new CANSparkMax(Ports.kShooterMotorPort, MotorType.kBrushless);
-   private CANEncoder shooterEncoder = new CANEncoder(shooterMotor);
-   private CANPIDController shooterController = shooterMotor.getPIDController();
+   private VirtualCANSparkMax shooterMotor = new VirtualCANSparkMax(Ports.kShooterMotorPort);
    private Lasershark shootDetector = new Lasershark(Ports.kShooterLaserSharkPort);
 
    private double setPoint = 0;
@@ -64,12 +62,12 @@ public ShooterSubsystem() {
       kMaxOutput = ShooterConstants.kMaxOutput;
       kMinOutput = ShooterConstants.kMinOutput;
       maxRPM = ShooterConstants.kMaxRPM;
-      shooterController.setP(kP);
-      shooterController.setI(kI);
-      shooterController.setD(kD);
-      shooterController.setIZone(kIz);
-      shooterController.setFF(kFF);
-      shooterController.setOutputRange(kMinOutput, kMaxOutput);
+      shooterMotor.setP(kP);
+      shooterMotor.setI(kI);
+      shooterMotor.setD(kD);
+      shooterMotor.setIZone(kIz);
+      shooterMotor.setFF(kFF);
+      shooterMotor.setOutputRange(kMinOutput, kMaxOutput);
 
       // display PID coefficients on SmartDashboard
       SmartDashboard.putNumber("Shooting I Gain", kI);
@@ -92,38 +90,40 @@ public void periodic() {
       // if PID coefficients on SmartDashboard have changed, write new values to
       // controller
       if ((i != kI)) {
-         shooterController.setI(i);
+         shooterMotor.setI(i);
          kI = i;
       }
       if ((d != kD)) {
-         shooterController.setD(d);
+         shooterMotor.setD(d);
          kD = d;
       }
       if ((p != kP)) {
-         shooterController.setP(p);
+         shooterMotor.setP(p);
          kP = p;
       }
       if ((ff != kFF)) {
-         shooterController.setFF(ff);
+         shooterMotor.setFF(ff);
          kFF = ff;
       }
 
       SmartDashboard.putNumber("SetPoint", setPoint);
-      SmartDashboard.putNumber("ProcessVariable", shooterEncoder.getVelocity());
+      SmartDashboard.putNumber("ProcessVariable", shooterMotor.getVelocity());
+
+      shooterMotor.periodic(); // Updates PID for simulation
    }
 
    /**
     * Turns the shooter on or off
     */
    public void setShooter(boolean running) {
       if (running) {
-         shooterController.setOutputRange(kMinOutput, kMaxOutput);
+         shooterMotor.setOutputRange(kMinOutput, kMaxOutput);
          setPoint = shooterSpeed * maxRPM;
       } else {
          setPoint = 0 * maxRPM;
-         shooterController.setOutputRange(0, 0);
+         shooterMotor.setOutputRange(0, 0);
       }
-      shooterController.setReference(setPoint, ControlType.kVelocity);
+      shooterMotor.setReference(setPoint);
    }
 
    /**
@@ -159,7 +159,7 @@ public double getTemperature() {
     * Returns the speed of the shooter motor
     */
    public double getVelocity() {
-      return shooterEncoder.getVelocity();
+      return shooterMotor.getVelocity();
    }
 
    /**