assignment: sw08a

.gitignore

@@ -6,6 +6,12 @@
 # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider
 # Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839
 
+simgui.json
+simgui-ds.json
+simgui-window.json
+
+*.log
+
 #idea folder
 .idea/
 

src/main/java/frc/robot/Constants.java

@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.util.Units;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
  * constants. This class should not be used for any other purpose. All constants should be declared
@@ -13,4 +15,20 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+    public static class FlywheelConstants {
+        public static final int LEFT_CAN_ID = 10;
+        public static final int RIGHT_CAN_ID = 11;
+        public static final double FLYWHEEL_INERTIA =
+              0.5 * Units.lbsToKilograms(2) * Math.pow(Units.inchesToMeters(3), 2); // 1/2*M*R^2
+        public static final double FLYWHEEL_GEARING = 1.0;
+
+        public static final double KP = 0.01;
+        public static final double KI = 0.0;
+        public static final double KD = 0.0;
+        public static final double KFF = 0.0018698999;
+    }
+
+    public static class SimConstants {
+        public static final double VOLTAGE_NOISE_RANGE = 0.0;
+    }
 }

src/main/java/frc/robot/Robot.java

@@ -27,7 +27,7 @@ public class Robot extends TimedRobot {
     public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
-    robotContainer = new RobotContainer();
+        robotContainer = new RobotContainer();
     }
 
     /**
@@ -39,11 +39,11 @@ public void robotInit() {
     */
     @Override
     public void robotPeriodic() {
-    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
-    // commands, running already-scheduled commands, removing finished or interrupted commands,
-    // and running subsystem periodic() methods.  This must be called from the robot's periodic
-    // block in order for anything in the Command-based framework to work.
-    CommandScheduler.getInstance().run();
+        // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+        // commands, running already-scheduled commands, removing finished or interrupted commands,
+        // and running subsystem periodic() methods.  This must be called from the robot's periodic
+        // block in order for anything in the Command-based framework to work.
+        CommandScheduler.getInstance().run();
     }
 
     /** This function is called once each time the robot enters Disabled mode. */
@@ -56,12 +56,12 @@ public void disabledPeriodic() {}
     /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
     @Override
     public void autonomousInit() {
-    autonomousCommand = robotContainer.getAutonomousCommand();
+        autonomousCommand = robotContainer.getAutonomousCommand();
 
-    // schedule the autonomous command (example)
-    if (autonomousCommand != null) {
-      autonomousCommand.schedule();
-    }
+        // schedule the autonomous command (example)
+        if (autonomousCommand != null) {
+          autonomousCommand.schedule();
+        }
     }
 
     /** This function is called periodically during autonomous. */
@@ -70,13 +70,13 @@ public void autonomousPeriodic() {}
 
     @Override
     public void teleopInit() {
-    // This makes sure that the autonomous stops running when
-    // teleop starts running. If you want the autonomous to
-    // continue until interrupted by another command, remove
-    // this line or comment it out.
-    if (autonomousCommand != null) {
-      autonomousCommand.cancel();
-    }
+        // This makes sure that the autonomous stops running when
+        // teleop starts running. If you want the autonomous to
+        // continue until interrupted by another command, remove
+        // this line or comment it out.
+        if (autonomousCommand != null) {
+          autonomousCommand.cancel();
+        }
     }
 
     /** This function is called periodically during operator control. */
@@ -85,8 +85,8 @@ public void teleopPeriodic() {}
 
     @Override
     public void testInit() {
-    // Cancels all running commands at the start of test mode.
-    CommandScheduler.getInstance().cancelAll();
+        // Cancels all running commands at the start of test mode.
+        CommandScheduler.getInstance().cancelAll();
     }
 
     /** This function is called periodically during test mode. */

src/main/java/frc/robot/RobotContainer.java

@@ -4,12 +4,15 @@
 
 package frc.robot;
 
+import java.util.function.DoubleSupplier;
+
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.button.Button;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.flywheel.FlywheelSubsystem;
+import frc.robot.flywheel.commands.SetFlywheelFromPID;
+import frc.robot.flywheel.commands.SetFlywheelFromVoltage;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -18,13 +21,13 @@
  * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-    private final XboxController driverController = new XboxController(0);
-    private final XboxController operatorController = new XboxController(1);
+    private FlywheelSubsystem flywheelSubsystem;
 
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
     public RobotContainer() {
-    // Configure the button bindings
+        // Configure the button bindings
         configureButtonBindings();
+        configureFlywheel();
     }
 
     /**
@@ -34,7 +37,13 @@ public RobotContainer() {
     * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
     */
     private void configureButtonBindings() {}
-     Button driverLeftBumper = new JoystickButton(driverController, XboxController.Button.kLeftBumper.value);
+    private void configureFlywheel() {
+        flywheelSubsystem = new FlywheelSubsystem();
+
+        // flywheelSubsystem.setDefaultCommand(new SetFlywheelFromPID(flywheelSubsystem, Math.random() * 600 + 601));
+        //flywheelSubsystem.setDefaultCommand(new SetFlywheelFromPID(flywheelSubsystem));
+         flywheelSubsystem.setDefaultCommand(new SetFlywheelFromVoltage(flywheelSubsystem));
+    }
 
     /**
     * Use this to pass the autonomous command to the main {@link Robot} class.
@@ -43,6 +52,7 @@ private void configureButtonBindings() {}
     */
     public Command getAutonomousCommand() {
         // An ExampleCommand will run in autonomous
-        return new InstantCommand();
+        DoubleSupplier velocitySetpoint = () -> Math.sin(Timer.getFPGATimestamp()) * 200 + 1200;
+        return new SetFlywheelFromPID(flywheelSubsystem, velocitySetpoint);
     }
 }

src/main/java/frc/robot/flywheel/FlywheelSubsystem.java

@@ -0,0 +1,102 @@
+package frc.robot.flywheel;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.FlywheelConstants;
+
+public class FlywheelSubsystem extends SubsystemBase {
+    public class PeriodicIO {
+        // inputs
+        public double voltage = 0;
+        public double lastUpdateTime = -1;
+
+        // outputs
+        public double angularVelocity = 0;
+        public double currentDrawAmps = 0;
+
+        public double getDt(double currentTime) {
+            if (lastUpdateTime != -1) {
+                return currentTime - lastUpdateTime;
+            } else {
+                return 0.02; // Average loop rate, use if its first loop
+            }
+        }
+    }
+
+    private PeriodicIO periodicIO;
+
+    private TalonFX masterLeftShooterMotor;
+    private TalonFX followerRightShooterMotor;
+    private FlywheelSim flywheelSim;
+
+    public FlywheelSubsystem() {
+        periodicIO = new PeriodicIO();
+
+        if (RobotBase.isReal()) {
+            configureRealHardware();
+        } else {
+            configureSimHardware();
+        }
+    }
+
+    private void configureRealHardware() {
+        masterLeftShooterMotor = new TalonFX(FlywheelConstants.LEFT_CAN_ID);
+        masterLeftShooterMotor.enableVoltageCompensation(true);
+        followerRightShooterMotor = new TalonFX(FlywheelConstants.RIGHT_CAN_ID);
+
+        followerRightShooterMotor.follow(masterLeftShooterMotor);
+    }
+
+    private void configureSimHardware() {
+        flywheelSim =
+              new FlywheelSim(DCMotor.getFalcon500(2), FlywheelConstants.FLYWHEEL_GEARING, FlywheelConstants.FLYWHEEL_INERTIA);
+    }
+
+    public void stopFlywheel() {
+        masterLeftShooterMotor.neutralOutput();
+    }
+
+    public void setInputVoltage(double voltage) {
+        periodicIO.voltage = MathUtil.clamp(voltage, 0, 12);
+        if (RobotBase.isSimulation()) {
+            flywheelSim.setInputVoltage(periodicIO.voltage);
+        } else {
+            masterLeftShooterMotor.set(ControlMode.PercentOutput, periodicIO.voltage / 12);
+        }
+    }
+
+    public double getAngularVelocityRPM() {
+        return periodicIO.angularVelocity;
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        double currentTime = Timer.getFPGATimestamp();
+        flywheelSim.update(periodicIO.getDt(currentTime));
+
+        periodicIO.lastUpdateTime = currentTime;
+        periodicIO.angularVelocity = flywheelSim.getAngularVelocityRPM();
+        periodicIO.currentDrawAmps = flywheelSim.getCurrentDrawAmps();
+
+        RoboRioSim.setVInVoltage(
+            BatterySim.calculateDefaultBatteryLoadedVoltage(flywheelSim.getCurrentDrawAmps()));
+
+        simulationOutputToDashboard();
+    }
+
+    private void simulationOutputToDashboard() {
+        SmartDashboard.putNumber("Flywheel Angular Velocity", periodicIO.angularVelocity);
+        SmartDashboard.putNumber("Current Draw", periodicIO.currentDrawAmps);
+        SmartDashboard.putNumber("Flywheel Sim Voltage", periodicIO.voltage);
+    }
+}