intake sim working

src/main/cpp/RobotContainer.cpp

@@ -28,7 +28,7 @@ void RobotContainer::ConfigureBindings() {
       },
       true));
 
-  controller.LeftTrigger().WhileTrue(swerveSubsystem.AlignToAmp());
+  //controller.LeftTrigger().WhileTrue(swerveSubsystem.AlignToAmp());
   controller.LeftBumper().WhileTrue(swerveSubsystem.XPattern());
   controller.Back().WhileTrue(
       swerveSubsystem.WheelRadius(frc2::sysid::Direction::kReverse));
@@ -52,6 +52,9 @@ void RobotContainer::ConfigureBindings() {
   controller.A().WhileTrue(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::AMP; }));
   controller.A().OnFalse(shooterSubsystem.RunShooter([] { return consts::shooter::PRESET_SPEEDS::OFF; }));
 
+  controller.LeftTrigger().WhileTrue(intakeSubsystem.IntakeNote());
+  controller.RightTrigger().WhileTrue(intakeSubsystem.PoopNote());
+
   // controller.A().WhileTrue(shooterSubsystem.TopWheelSysIdQuasistatic(frc2::sysid::Direction::kForward));
   // controller.B().WhileTrue(shooterSubsystem.TopWheelSysIdQuasistatic(frc2::sysid::Direction::kReverse));
   // controller.X().WhileTrue(shooterSubsystem.TopWheelSysIdDynamic(frc2::sysid::Direction::kForward));

src/main/cpp/subsystems/IntakeSubsystem.cpp

@@ -5,6 +5,7 @@
 #include "subsystems/IntakeSubsystem.h"
 #include <frc/smartdashboard/SmartDashboard.h>
 #include "constants/Constants.h"
+#include <frc2/command/Commands.h>
 
 IntakeSubsystem::IntakeSubsystem() {
     SetName("IntakeSubsystem");
@@ -13,8 +14,56 @@ IntakeSubsystem::IntakeSubsystem() {
     frc::SmartDashboard::PutData(this);
 }
 
+bool IntakeSubsystem::TouchingNote() {
+  return isTouchingNote;
+}
+
+frc2::CommandPtr IntakeSubsystem::IntakeNote() {
+  return frc2::cmd::RunEnd([this] {
+    intakeWheelVoltageSetpoint = consts::intake::gains::NOTE_INTAKE_VOLTAGE;
+  }, [this] {
+    intakeWheelVoltageSetpoint = 0_V;
+  }, {this});
+}
+
+frc2::CommandPtr IntakeSubsystem::PoopNote() {
+  return frc2::cmd::RunEnd([this] {
+    intakeWheelVoltageSetpoint = consts::intake::gains::NOTE_EJECT_VOLTAGE;
+  }, [this] {
+    intakeWheelVoltageSetpoint = 0_V;
+  }, {this});
+}
+
 // This method will be called once per scheduler run
-void IntakeSubsystem::Periodic() {}
+void IntakeSubsystem::Periodic() {
+  ctre::phoenix::StatusCode shooterWaitResult = ctre::phoenix6::BaseStatusSignal::RefreshAll({
+    &intakeMotorVoltageSig, 
+    &intakeMotorTorqueCurrentSig
+  });
+
+  currentIntakeWheelVoltage = intakeMotorVoltageSig.GetValue();
+  intakeWheelTorqueCurrent = intakeMotorTorqueCurrentSig.GetValue();
+
+  isTouchingNote = intakeSpikeDebouncer.Calculate(intakeWheelTorqueCurrent > consts::intake::gains::NOTE_SPIKE_THRESHOLD);
+
+  intakeMotor.SetControl(intakeMotorVoltageSetter.WithOutput(intakeWheelVoltageSetpoint));
+
+  UpdateNTEntries();
+}
+
+void IntakeSubsystem::UpdateNTEntries() {
+  intakeWheelMotorVoltagePub.Set(currentIntakeWheelVoltage.value());
+  intakeWheelMotorVoltageSetpointPub.Set(intakeWheelVoltageSetpoint.value());
+  intakeWheelTorqueCurrentPub.Set(intakeWheelTorqueCurrent.value());
+  touchingNotePub.Set(isTouchingNote);
+}
+
+void IntakeSubsystem::SimulationPeriodic() {
+    intakeMotorSim.SetSupplyVoltage(frc::RobotController::GetBatteryVoltage());
+    intakeSim.SetInputVoltage(intakeMotorSim.GetMotorVoltage());
+    intakeSim.Update(consts::LOOP_PERIOD);
+    intakeMotorSim.SetRotorVelocity(intakeSim.GetAngularVelocity());
+}
 
 bool IntakeSubsystem::ConfigureIntakeMotor(bool invert,
                             units::scalar_t intakeGearing,

src/main/deploy/commit.txt

@@ -1 +1 @@
-7857c51
+93bc473

src/main/include/constants/IntakeConstants.h

@@ -37,5 +37,11 @@ inline constexpr units::meter_t WHEEL_RADIUS = 1_in;
 //From onshape doc
 inline constexpr units::kilogram_square_meter_t INTAKE_MOI = 12.350445 * 1_in * 1_in * 1_lb;
 } // namespace physical
+
+namespace gains {
+inline constexpr units::ampere_t NOTE_SPIKE_THRESHOLD = 100_A;
+inline constexpr units::volt_t NOTE_INTAKE_VOLTAGE = 10_V;
+inline constexpr units::volt_t NOTE_EJECT_VOLTAGE = -5_V;
+}
 } // namespace intake
 } // namespace consts

src/main/include/subsystems/IntakeSubsystem.h

@@ -13,6 +13,8 @@
 #include <ctre/phoenix6/TalonFX.hpp>
 #include <frc/system/plant/LinearSystemId.h>
 #include <frc/simulation/FlywheelSim.h>
+#include <frc/filter/Debouncer.h>
+#include <frc2/command/CommandPtr.h>
 
 class IntakeSubsystem : public frc2::SubsystemBase {
  public:
@@ -22,7 +24,11 @@ class IntakeSubsystem : public frc2::SubsystemBase {
    * Will be called periodically whenever the CommandScheduler runs.
    */
   void Periodic() override;
+  void SimulationPeriodic() override;
+  bool TouchingNote();
 
+  frc2::CommandPtr IntakeNote();
+  frc2::CommandPtr PoopNote();
 
  private:
   void UpdateNTEntries();
@@ -46,4 +52,23 @@ class IntakeSubsystem : public frc2::SubsystemBase {
 
   std::shared_ptr<nt::NetworkTable> nt{
       nt::NetworkTableInstance::GetDefault().GetTable("Intake")};
+  nt::DoublePublisher intakeWheelMotorVoltagePub{
+    nt->GetDoubleTopic("IntakeWheelMotorVoltage").Publish()
+  };
+  nt::DoublePublisher intakeWheelMotorVoltageSetpointPub{
+    nt->GetDoubleTopic("IntakeWheelMotorVoltageSetpoint").Publish()
+  };
+  nt::DoublePublisher intakeWheelTorqueCurrentPub{
+    nt->GetDoubleTopic("IntakeWheelMotorTorqueCurrent").Publish()
+  };
+  nt::DoublePublisher touchingNotePub{
+    nt->GetDoubleTopic("TouchingNote").Publish()
+  };
+
+  frc::Debouncer intakeSpikeDebouncer{.25_s, frc::Debouncer::kFalling};
+  bool isTouchingNote{false};
+
+  units::volt_t intakeWheelVoltageSetpoint{0_V};
+  units::volt_t currentIntakeWheelVoltage{0_V};
+  units::ampere_t intakeWheelTorqueCurrent{0_A};
 };