Skip to content

Commit

Permalink
fixed best algae method
Browse files Browse the repository at this point in the history
aravindkrishna2008 committed Jan 29, 2025
1 parent 4c40869 commit a2bceda
Showing 4 changed files with 34 additions and 2 deletions.
8 changes: 6 additions & 2 deletions README.md
Original file line number Diff line number Diff line change
@@ -173,6 +173,10 @@ To undo the going back:

## CppCheck Warnings
```
src\frc846\cpp\frc846\math\collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
src\frc846\cpp\frc846\math\collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
src/y2025/cpp/commands/teleop/drive_command.cc:20:24: warning: Variable 'targeting_algae' is assigned a value that is never used. [unreadVariable]
src/y2025/cpp/subsystems/abstract/gpd.cc:30:7: warning: The scope of the variable 'closest_algae_index' can be reduced. [variableScope]
src/y2025/cpp/subsystems/abstract/gpd.cc:30:27: warning: Variable 'closest_algae_index' is assigned a value that is never used. [unreadVariable]
src/y2025/cpp/subsystems/abstract/gpd.cc:40:27: warning: Variable 'closest_algae_index' is assigned a value that is never used. [unreadVariable]
src/frc846/cpp/frc846/math/collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
src/frc846/cpp/frc846/math/collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
```
22 changes: 22 additions & 0 deletions src/y2025/cpp/subsystems/abstract/gpd.cc
Original file line number Diff line number Diff line change
@@ -23,6 +23,28 @@ bool GPDSubsystem::VerifyHardware() { return true; }

void GPDSubsystem::Setup() {}

frc846::math::Vector2D GPDSubsystem::getBestGP(
const std::vector<frc846::math::Vector2D> algae,
const frc846::math::VectorND<units::feet_per_second, 2> robot_velocity) {
if (algae.empty()) { return {}; }
frc846::math::Vector2D closest_algae;
int closest_algae_index = -1;
units::degree_t min_angle = 180_deg;

for (size_t i = 0; i < algae.size(); i++) {
frc846::math::Vector2D relative_note = algae.at(i);
units::degree_t angle = robot_velocity.angleTo(relative_note, true);

if (angle < min_angle) {
min_angle = angle;
closest_algae = relative_note;
closest_algae_index = i;
}

return closest_algae;
}
}

GPDReadings GPDSubsystem::ReadFromHardware() {
GPDReadings readings;
frc846::robot::swerve::DrivetrainReadings drivetrain_readings =
2 changes: 2 additions & 0 deletions src/y2025/include/subsystems/abstract/control_input.h
Original file line number Diff line number Diff line change
@@ -21,6 +21,8 @@ struct ControlInputReadings {
bool lock_right_reef;

bool zero_bearing;

bool targeting_algae;
};

struct ControlInputTarget {
4 changes: 4 additions & 0 deletions src/y2025/include/subsystems/abstract/gpd.h
Original file line number Diff line number Diff line change
@@ -23,6 +23,10 @@ class GPDSubsystem
public:
GPDSubsystem(frc846::robot::swerve::DrivetrainSubsystem* drivetrain);

frc846::math::Vector2D getBestGP(
const std::vector<frc846::math::Vector2D> algae,
const frc846::math::VectorND<units::feet_per_second, 2> robot_velocity);

void Setup() override;

GPDTarget ZeroTarget() const override;

0 comments on commit a2bceda

Please sign in to comment.