Skip to content

Commit

Permalink
intake logic working i think
Browse files Browse the repository at this point in the history
  • Loading branch information
@r4stered
r4stered committed Oct 6, 2024
1 parent 2f1f4a1 commit b0f076c
Show file tree
Hide file tree
Showing 11 changed files with 172 additions and 58 deletions.
111 changes: 68 additions & 43 deletions src/main/cpp/RobotContainer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,53 +11,32 @@
RobotContainer::RobotContainer() { ConfigureBindings(); }

void RobotContainer::ConfigureBindings() {
// DEFAULT DRIVE COMMAND
swerveSubsystem.SetDefaultCommand(swerveSubsystem.Drive(
[this] {
return str::NegateIfRed(
frc::ApplyDeadband<double>(-controller.GetLeftY(), .1) *
frc::ApplyDeadband<double>(-driverController.GetLeftY(), .1) *
consts::swerve::physical::DRIVE_MAX_SPEED);
},
[this] {
return str::NegateIfRed(
frc::ApplyDeadband<double>(-controller.GetLeftX(), .1) *
frc::ApplyDeadband<double>(-driverController.GetLeftX(), .1) *
consts::swerve::physical::DRIVE_MAX_SPEED);
},
[this] {
return frc::ApplyDeadband<double>(-controller.GetRightX(), .1) *
return frc::ApplyDeadband<double>(-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<double>(-controller.GetLeftY(), .1) *
consts::swerve::physical::DRIVE_MAX_SPEED);
},
[this] {
return str::NegateIfRed(
frc::ApplyDeadband<double>(-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));
Expand All @@ -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));
Expand Down Expand Up @@ -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;
Expand All @@ -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<units::second_t()> 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<units::second_t()> 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);
});
}
38 changes: 38 additions & 0 deletions src/main/cpp/subsystems/FeederSubsystem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,8 @@
#include "constants/Constants.h"
#include <frc/DataLogManager.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <frc2/command/Commands.h>
#include <iostream>

FeederSubsystem::FeederSubsystem() {
SetName("FeederSubsystem");
Expand All @@ -17,15 +19,51 @@ 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) {
hasNote = true;
} else {
hasNote = false;
}

feederMotor.SetControl(feederMotorVoltageSetter.WithOutput(feederWheelVoltageSetpoint));

UpdateNTEntries();
}

Expand Down
29 changes: 26 additions & 3 deletions src/main/cpp/subsystems/IntakeSubsystem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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});
}

Expand All @@ -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));

Expand Down
8 changes: 6 additions & 2 deletions src/main/cpp/subsystems/SwerveSubsystem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -177,7 +177,7 @@ bool SwerveSubsystem::IsNearAmp() {
consts::yearSpecific::closeToAmpDistance;
}

frc2::CommandPtr SwerveSubsystem::NoteAssist(std::function<units::meters_per_second_t()> xVel, std::function<units::meters_per_second_t()> yVel, std::function<frc::Pose2d()> notePose) {
frc2::CommandPtr SwerveSubsystem::NoteAssist(std::function<units::meters_per_second_t()> xVel, std::function<units::meters_per_second_t()> yVel, std::function<units::radians_per_second_t()> rotOverride, std::function<frc::Pose2d()> notePose) {
return frc2::cmd::Sequence(
frc2::cmd::RunOnce(
[this, notePose] {
Expand All @@ -197,7 +197,7 @@ frc2::CommandPtr SwerveSubsystem::NoteAssist(std::function<units::meters_per_sec
{this})
.WithName("NoteAssist Init"),
frc2::cmd::Run(
[this, notePose, xVel, yVel] {
[this, notePose, xVel, yVel, rotOverride] {
frc::Pose2d currentPose = GetRobotPose();

frc::Translation2d diff = currentPose.Translation() - notePose().Translation();
Expand All @@ -207,6 +207,10 @@ frc2::CommandPtr SwerveSubsystem::NoteAssist(std::function<units::meters_per_sec
thetaController.Calculate(
currentPose.Rotation().Radians())};

if(rotOverride() != 0_rad_per_s) {
thetaSpeed = rotOverride();
}

swerveDrive.Drive(xVel(), yVel(), thetaSpeed, true);
},
{this})
Expand Down
14 changes: 10 additions & 4 deletions src/main/include/RobotContainer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,23 +19,29 @@ class RobotContainer {

frc2::Command *GetAutonomousCommand();

frc2::CommandPtr IntakeNote();

SwerveSubsystem &GetSwerveSubsystem();
ShooterSubsystem &GetShooterSubsystem();
IntakeSubsystem &GetIntakeSubsystem();
FeederSubsystem &GetFeederSubsystem();
//str::Vision &GetVision();
//str::NoteVisualizer &GetNoteVisualizer();
str::NoteVisualizer &GetNoteVisualizer();

private:
void ConfigureBindings();
frc2::CommandXboxController controller{0};
frc2::CommandXboxController driverController{0};
frc2::CommandXboxController operatorController{1};

frc2::CommandPtr RumbleDriver(std::function<units::second_t()> timeToRumble);
frc2::CommandPtr RumbleOperator(std::function<units::second_t()> 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};
Expand Down
2 changes: 1 addition & 1 deletion src/main/include/constants/FeederConstants.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 2 additions & 2 deletions src/main/include/constants/IntakeConstants.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
4 changes: 2 additions & 2 deletions src/main/include/str/Vision.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ class Vision {
std::array<Camera, 4> 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},
Expand All @@ -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
Loading

0 comments on commit b0f076c

Please sign in to comment.