diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index ab14d19..3ab38f4 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -11,53 +11,32 @@ RobotContainer::RobotContainer() { ConfigureBindings(); } void RobotContainer::ConfigureBindings() { + // DEFAULT DRIVE COMMAND swerveSubsystem.SetDefaultCommand(swerveSubsystem.Drive( [this] { return str::NegateIfRed( - frc::ApplyDeadband(-controller.GetLeftY(), .1) * + frc::ApplyDeadband(-driverController.GetLeftY(), .1) * consts::swerve::physical::DRIVE_MAX_SPEED); }, [this] { return str::NegateIfRed( - frc::ApplyDeadband(-controller.GetLeftX(), .1) * + frc::ApplyDeadband(-driverController.GetLeftX(), .1) * consts::swerve::physical::DRIVE_MAX_SPEED); }, [this] { - return frc::ApplyDeadband(-controller.GetRightX(), .1) * + return frc::ApplyDeadband(-driverController.GetRightX(), .1) * consts::swerve::physical::DRIVE_MAX_ROT_SPEED; }, true, true)); - controller.LeftTrigger().WhileTrue(swerveSubsystem.AlignToAmp()); - controller.LeftBumper().WhileTrue( - swerveSubsystem.NoteAssist( - [this] { - return str::NegateIfRed( - frc::ApplyDeadband(-controller.GetLeftY(), .1) * - consts::swerve::physical::DRIVE_MAX_SPEED); - }, - [this] { - return str::NegateIfRed( - frc::ApplyDeadband(-controller.GetLeftX(), .1) * - consts::swerve::physical::DRIVE_MAX_SPEED); - }, - [this] { - return swerveSubsystem.GetFoundNotePose(); - }) - ); - // controller.Back().WhileTrue( - // swerveSubsystem.WheelRadius(frc2::sysid::Direction::kReverse)); - // controller.Start().WhileTrue( - // swerveSubsystem.WheelRadius(frc2::sysid::Direction::kForward)); + driverController.Start().WhileTrue(intakeSubsystem.FakeNote()); + + driverController.LeftTrigger().OnTrue(IntakeNote()); + (!driverController.LeftTrigger() && !intakeSubsystem.TouchedNote()).OnTrue(frc2::cmd::Sequence(intakeSubsystem.Stop(), frc2::cmd::Print("Cancelled"))); + intakeSubsystem.TouchedNote().OnTrue(RumbleDriver([]{ return .1_s; })); + feederSubsystem.GotNote().OnTrue(frc2::cmd::Parallel(intakeSubsystem.Stop(), feederSubsystem.Stop(), RumbleDriver([] { return .5_s; }), RumbleOperator([] { return .5_s; }))); - // controller.A().OnTrue(frc2::cmd::RunOnce([this] { - // noteVisualizer.LaunchNote( - // frc::Pose3d{swerveSubsystem.GetRobotPose()}, - // swerveSubsystem.GetRobotRelativeSpeed(), - // frc::Transform3d{frc::Translation3d{-4_in, 0_in, 13_in}, - // frc::Rotation3d{0_deg, -50_deg, 0_deg}}, - // 41.71_fps); - // })); + driverController.A().WhileTrue(swerveSubsystem.AlignToAmp()); // controller.A().WhileTrue(swerveSubsystem.SysIdDriveQuasistaticTorque(frc2::sysid::Direction::kForward)); // controller.B().WhileTrue(swerveSubsystem.SysIdDriveQuasistaticTorque(frc2::sysid::Direction::kReverse)); @@ -69,23 +48,27 @@ void RobotContainer::ConfigureBindings() { // controller.POVUp().OnTrue(swerveSubsystem.TuneDrivePID([this] { return // controller.Start().Get(); })); - controller.A().WhileTrue(shooterSubsystem.RunShooter( + // controller.Back().WhileTrue( + // swerveSubsystem.WheelRadius(frc2::sysid::Direction::kReverse)); + // controller.Start().WhileTrue( + // swerveSubsystem.WheelRadius(frc2::sysid::Direction::kForward)); + + operatorController.A().WhileTrue(shooterSubsystem.RunShooter( [] { return consts::shooter::PRESET_SPEEDS::AMP; })); - controller.A().OnFalse(shooterSubsystem.RunShooter( + operatorController.A().OnFalse(shooterSubsystem.RunShooter( [] { return consts::shooter::PRESET_SPEEDS::OFF; })); - controller.B().WhileTrue(shooterSubsystem.RunShooter( + operatorController.Y().WhileTrue(shooterSubsystem.RunShooter( [] { return consts::shooter::PRESET_SPEEDS::PASS; })); - controller.B().OnFalse(shooterSubsystem.RunShooter( + operatorController.Y().OnFalse(shooterSubsystem.RunShooter( [] { return consts::shooter::PRESET_SPEEDS::OFF; })); - controller.Y().WhileTrue(shooterSubsystem.RunShooter( + operatorController.B().WhileTrue(shooterSubsystem.RunShooter( [] { return consts::shooter::PRESET_SPEEDS::SUBWOOFER; })); - controller.Y().OnFalse(shooterSubsystem.RunShooter( + operatorController.B().OnFalse(shooterSubsystem.RunShooter( [] { return consts::shooter::PRESET_SPEEDS::SUBWOOFER; })); - controller.LeftTrigger().WhileTrue(intakeSubsystem.IntakeNote()); - controller.RightTrigger().WhileTrue(intakeSubsystem.PoopNote()); + operatorController.Back().WhileTrue(frc2::cmd::Parallel(intakeSubsystem.PoopNote(), feederSubsystem.Eject())); // controller.A().WhileTrue(shooterSubsystem.TopWheelSysIdQuasistatic(frc2::sysid::Direction::kForward)); // controller.B().WhileTrue(shooterSubsystem.TopWheelSysIdQuasistatic(frc2::sysid::Direction::kReverse)); @@ -124,6 +107,16 @@ void RobotContainer::ConfigureBindings() { // { return 180_deg; })); } +frc2::CommandPtr RobotContainer::IntakeNote() { + return frc2::cmd::Parallel( + frc2::cmd::Print("IntakeNote"), + intakeSubsystem.IntakeNote(), + feederSubsystem.Feed() + ).Until([this] { + return feederSubsystem.HasNote(); + }); +} + frc2::Command *RobotContainer::GetAutonomousCommand() { //return autos.GetSelectedCommand(); return nullptr; @@ -147,6 +140,38 @@ FeederSubsystem &RobotContainer::GetFeederSubsystem() { // str::Vision &RobotContainer::GetVision() { return vision; } -// str::NoteVisualizer &RobotContainer::GetNoteVisualizer() { -// return noteVisualizer; -// } +str::NoteVisualizer &RobotContainer::GetNoteVisualizer() { + return noteVisualizer; +} + +frc2::CommandPtr RobotContainer::RumbleDriver(std::function timeToRumble) { + return frc2::cmd::Sequence( + frc2::cmd::RunOnce([this] { + driverController.SetRumble( + frc::GenericHID::RumbleType::kBothRumble, 1.0); + }), + frc2::cmd::Wait(timeToRumble()), frc2::cmd::RunOnce([this] { + driverController.SetRumble( + frc::GenericHID::RumbleType::kBothRumble, 0.0); + })) + .FinallyDo([this] { + driverController.SetRumble(frc::GenericHID::RumbleType::kBothRumble, + 0.0); + }); +} + +frc2::CommandPtr RobotContainer::RumbleOperator(std::function timeToRumble) { + return frc2::cmd::Sequence( + frc2::cmd::RunOnce([this] { + operatorController.SetRumble( + frc::GenericHID::RumbleType::kBothRumble, 1.0); + }), + frc2::cmd::Wait(timeToRumble()), frc2::cmd::RunOnce([this] { + operatorController.SetRumble( + frc::GenericHID::RumbleType::kBothRumble, 0.0); + })) + .FinallyDo([this] { + operatorController.SetRumble(frc::GenericHID::RumbleType::kBothRumble, + 0.0); + }); +} \ No newline at end of file diff --git a/src/main/cpp/subsystems/FeederSubsystem.cpp b/src/main/cpp/subsystems/FeederSubsystem.cpp index bf08080..d89f195 100644 --- a/src/main/cpp/subsystems/FeederSubsystem.cpp +++ b/src/main/cpp/subsystems/FeederSubsystem.cpp @@ -6,6 +6,8 @@ #include "constants/Constants.h" #include #include +#include +#include FeederSubsystem::FeederSubsystem() { SetName("FeederSubsystem"); @@ -17,8 +19,41 @@ FeederSubsystem::FeederSubsystem() { frc::SmartDashboard::PutData(this); } +frc2::CommandPtr FeederSubsystem::Feed() { + return frc2::cmd::RunEnd([this] { + feederWheelVoltageSetpoint = consts::feeder::gains::NOTE_FEED_VOLTAGE; + }, [this] { + feederWheelVoltageSetpoint = 0_V; + }, {this}).WithName("FeedUntilNote"); +} + +frc2::CommandPtr FeederSubsystem::Stop() { + return frc2::cmd::RunOnce([this] { + feederWheelVoltageSetpoint = 0_V; + }, {this}).WithName("Stop"); +} + +frc2::CommandPtr FeederSubsystem::Eject() { + return frc2::cmd::RunEnd([this] { + feederWheelVoltageSetpoint = consts::feeder::gains::NOTE_EJECT_VOLTAGE; + }, [this] { + feederWheelVoltageSetpoint = 0_V; + }, {this}).WithName("Eject"); +} + // This method will be called once per scheduler run void FeederSubsystem::Periodic() { + + ctre::phoenix::StatusCode feederWaitResult = ctre::phoenix6::BaseStatusSignal::RefreshAll({ + &feederMotorVoltageSig, + }); + + if(!feederWaitResult.IsOK()) { + frc::DataLogManager::Log(fmt::format("Error grabbing feeder signals! Details: {}\n", feederWaitResult.GetName())); + } + + currentFeederWheelVoltage = feederMotorVoltageSig.GetValue(); + noteSensorRawVal = noteSensor.Get(); noteSensorDebouced = noteSensorDebouncer.Calculate(noteSensorRawVal); if (noteSensorDebouced) { @@ -26,6 +61,9 @@ void FeederSubsystem::Periodic() { } else { hasNote = false; } + + feederMotor.SetControl(feederMotorVoltageSetter.WithOutput(feederWheelVoltageSetpoint)); + UpdateNTEntries(); } diff --git a/src/main/cpp/subsystems/IntakeSubsystem.cpp b/src/main/cpp/subsystems/IntakeSubsystem.cpp index 96589ba..f565fdb 100644 --- a/src/main/cpp/subsystems/IntakeSubsystem.cpp +++ b/src/main/cpp/subsystems/IntakeSubsystem.cpp @@ -24,14 +24,32 @@ frc2::CommandPtr IntakeSubsystem::IntakeNote() { intakeWheelVoltageSetpoint = consts::intake::gains::NOTE_INTAKE_VOLTAGE; }, [this] { intakeWheelVoltageSetpoint = 0_V; - }, {this}); + stateTimer.Restart(); + }, {this}).WithName("IntakeNote").BeforeStarting([this] { stateTimer.Restart(); }); +} + +frc2::CommandPtr IntakeSubsystem::Stop() { + return frc2::cmd::RunOnce([this] { + intakeWheelVoltageSetpoint = 0_V; + stateTimer.Restart(); + }, {this}).WithName("Stop"); +} + +frc2::CommandPtr IntakeSubsystem::FakeNote() { + return frc2::cmd::Run([this] { + simOverrideTorque = true; + }).FinallyDo([this] { + simOverrideTorque = false; + }); } frc2::CommandPtr IntakeSubsystem::PoopNote() { return frc2::cmd::RunEnd([this] { intakeWheelVoltageSetpoint = consts::intake::gains::NOTE_EJECT_VOLTAGE; + stateTimer.Restart(); }, [this] { intakeWheelVoltageSetpoint = 0_V; + stateTimer.Restart(); }, {this}); } @@ -43,9 +61,14 @@ void IntakeSubsystem::Periodic() { }); currentIntakeWheelVoltage = intakeMotorVoltageSig.GetValue(); - intakeWheelTorqueCurrent = intakeMotorTorqueCurrentSig.GetValue(); + if(simOverrideTorque) { + intakeWheelTorqueCurrent = 300_A; + } + else { + intakeWheelTorqueCurrent = intakeMotorTorqueCurrentSig.GetValue(); + } - isTouchingNote = intakeSpikeDebouncer.Calculate(intakeWheelTorqueCurrent > consts::intake::gains::NOTE_SPIKE_THRESHOLD); + isTouchingNote = stateTimer.HasElapsed(.25_s) && intakeSpikeDebouncer.Calculate(intakeWheelTorqueCurrent > consts::intake::gains::NOTE_SPIKE_THRESHOLD); intakeMotor.SetControl(intakeMotorVoltageSetter.WithOutput(intakeWheelVoltageSetpoint)); diff --git a/src/main/cpp/subsystems/SwerveSubsystem.cpp b/src/main/cpp/subsystems/SwerveSubsystem.cpp index 133d236..925670f 100644 --- a/src/main/cpp/subsystems/SwerveSubsystem.cpp +++ b/src/main/cpp/subsystems/SwerveSubsystem.cpp @@ -177,7 +177,7 @@ bool SwerveSubsystem::IsNearAmp() { consts::yearSpecific::closeToAmpDistance; } -frc2::CommandPtr SwerveSubsystem::NoteAssist(std::function xVel, std::function yVel, std::function notePose) { +frc2::CommandPtr SwerveSubsystem::NoteAssist(std::function xVel, std::function yVel, std::function rotOverride, std::function notePose) { return frc2::cmd::Sequence( frc2::cmd::RunOnce( [this, notePose] { @@ -197,7 +197,7 @@ frc2::CommandPtr SwerveSubsystem::NoteAssist(std::function timeToRumble); + frc2::CommandPtr RumbleOperator(std::function timeToRumble); SwerveSubsystem swerveSubsystem; ShooterSubsystem shooterSubsystem; IntakeSubsystem intakeSubsystem; FeederSubsystem feederSubsystem; - // str::Vision vision; - // str::NoteVisualizer noteVisualizer; + //str::Vision vision; + str::NoteVisualizer noteVisualizer; // Autos autos{swerveSubsystem, shooterSubsystem, intakeSubsystem, // feederSubsystem}; diff --git a/src/main/include/constants/FeederConstants.h b/src/main/include/constants/FeederConstants.h index 137603c..1c04502 100644 --- a/src/main/include/constants/FeederConstants.h +++ b/src/main/include/constants/FeederConstants.h @@ -45,7 +45,7 @@ inline constexpr units::kilogram_square_meter_t FEEDER_MOI = namespace gains { inline constexpr units::volt_t NOTE_FEED_VOLTAGE = 10_V; -inline constexpr units::volt_t NOTE_EJECT_VOLTAGE = -5_V; +inline constexpr units::volt_t NOTE_EJECT_VOLTAGE = -10_V; inline constexpr units::second_t NOTE_SENSOR_DEBOUNCE_TIME = 0.1_s; } // namespace gains } // namespace feeder diff --git a/src/main/include/constants/IntakeConstants.h b/src/main/include/constants/IntakeConstants.h index 9470903..382c99c 100644 --- a/src/main/include/constants/IntakeConstants.h +++ b/src/main/include/constants/IntakeConstants.h @@ -35,13 +35,13 @@ inline constexpr frc::DCMotor INTAKE_MOTOR = frc::DCMotor::Falcon500(1); 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; +inline constexpr units::kilogram_square_meter_t INTAKE_MOI = 5.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; +inline constexpr units::volt_t NOTE_EJECT_VOLTAGE = -10_V; } } // namespace intake } // namespace consts diff --git a/src/main/include/str/Vision.h b/src/main/include/str/Vision.h index 7fbf23e..3b2d651 100644 --- a/src/main/include/str/Vision.h +++ b/src/main/include/str/Vision.h @@ -29,7 +29,7 @@ class Vision { std::array cameras{ Camera{consts::vision::FL_CAM_NAME, consts::vision::FL_ROBOT_TO_CAM, consts::vision::SINGLE_TAG_STD_DEV, - consts::vision::MULTI_TAG_STD_DEV, false}, + consts::vision::MULTI_TAG_STD_DEV, true}, Camera{consts::vision::FR_CAM_NAME, consts::vision::FR_ROBOT_TO_CAM, consts::vision::SINGLE_TAG_STD_DEV, consts::vision::MULTI_TAG_STD_DEV, false}, @@ -42,6 +42,6 @@ class Vision { Camera noteCamera{consts::vision::NOTE_CAM_NAME, consts::vision::ROBOT_TO_NOTE_CAM, consts::vision::SINGLE_TAG_STD_DEV, - consts::vision::MULTI_TAG_STD_DEV, true}; + consts::vision::MULTI_TAG_STD_DEV, false}; }; } // namespace str diff --git a/src/main/include/subsystems/FeederSubsystem.h b/src/main/include/subsystems/FeederSubsystem.h index 218a769..088c452 100644 --- a/src/main/include/subsystems/FeederSubsystem.h +++ b/src/main/include/subsystems/FeederSubsystem.h @@ -15,11 +15,19 @@ #include #include #include +#include class FeederSubsystem : public frc2::SubsystemBase { public: FeederSubsystem(); + + frc2::CommandPtr Feed(); + frc2::CommandPtr Stop(); + frc2::CommandPtr Eject(); + + frc2::Trigger GotNote() { return gotNote; } + /** * Will be called periodically whenever the CommandScheduler runs. */ @@ -73,6 +81,8 @@ class FeederSubsystem : public frc2::SubsystemBase { bool noteSensorDebouced{false}; bool hasNote{false}; + frc2::Trigger gotNote{[this] { return hasNote; }}; + units::volt_t feederWheelVoltageSetpoint{0_V}; units::volt_t currentFeederWheelVoltage{0_V}; }; diff --git a/src/main/include/subsystems/IntakeSubsystem.h b/src/main/include/subsystems/IntakeSubsystem.h index 762dbb9..e4c9e2f 100644 --- a/src/main/include/subsystems/IntakeSubsystem.h +++ b/src/main/include/subsystems/IntakeSubsystem.h @@ -16,6 +16,7 @@ #include #include #include +#include class IntakeSubsystem : public frc2::SubsystemBase { public: @@ -27,9 +28,12 @@ class IntakeSubsystem : public frc2::SubsystemBase { void Periodic() override; void SimulationPeriodic() override; bool TouchingNote(); + frc2::Trigger TouchedNote() { return touchedNote; } frc2::CommandPtr IntakeNote(); + frc2::CommandPtr Stop(); frc2::CommandPtr PoopNote(); + frc2::CommandPtr FakeNote(); private: void UpdateNTEntries(); @@ -68,8 +72,12 @@ class IntakeSubsystem : public frc2::SubsystemBase { nt::BooleanPublisher touchingNotePub{ nt->GetBooleanTopic("TouchingNote").Publish()}; + frc::Timer stateTimer{}; frc::Debouncer intakeSpikeDebouncer{.25_s, frc::Debouncer::kFalling}; bool isTouchingNote{false}; + bool simOverrideTorque{false}; + + frc2::Trigger touchedNote{[this] { return isTouchingNote; }}; units::volt_t intakeWheelVoltageSetpoint{0_V}; units::volt_t currentIntakeWheelVoltage{0_V}; diff --git a/src/main/include/subsystems/SwerveSubsystem.h b/src/main/include/subsystems/SwerveSubsystem.h index 6521fb7..e983e7d 100644 --- a/src/main/include/subsystems/SwerveSubsystem.h +++ b/src/main/include/subsystems/SwerveSubsystem.h @@ -64,7 +64,7 @@ class SwerveSubsystem : public frc2::SubsystemBase { frc2::CommandPtr TuneDrivePID(std::function isDone); void CalculateFoundNotePose(std::optional distanceToNote, std::optional angleToNote); frc::Pose2d GetFoundNotePose() const; - frc2::CommandPtr NoteAssist(std::function xVel, std::function yVel, std::function notePose); + frc2::CommandPtr NoteAssist(std::function xVel, std::function yVel, std::function rotOverride, std::function notePose); private: void SetupPathplanner(); void LoadChoreoTrajectories();