diff --git a/.gitignore b/.gitignore index 983678a..d4755c4 100644 --- a/.gitignore +++ b/.gitignore @@ -159,3 +159,5 @@ imgui.ini # End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode + +/.idea/ diff --git a/build.gradle b/build.gradle index 9eb9d8f..9720092 100644 --- a/build.gradle +++ b/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. diff --git a/src/main/java/frc/robot/lib/FancyLog.java b/src/main/java/frc/robot/lib/FancyLog.java index 8fb518e..aa71159 100644 --- a/src/main/java/frc/robot/lib/FancyLog.java +++ b/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) { diff --git a/src/main/java/frc/robot/lib/VirtualCANSparkMax.java b/src/main/java/frc/robot/lib/VirtualCANSparkMax.java new file mode 100644 index 0000000..e8622df --- /dev/null +++ b/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 + } + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 8967db7..f5631a7 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -2,7 +2,6 @@ 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; @@ -10,6 +9,7 @@ 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,24 +90,26 @@ 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 } /** @@ -117,13 +117,13 @@ public void periodic() { */ 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(); } /**