Skip to content

Commit

Permalink
commenting out all subsystems as we add them
Browse files Browse the repository at this point in the history
  • Loading branch information
@r4stered
r4stered committed Oct 2, 2024
1 parent 9255fe7 commit 28d4f57
Show file tree
Hide file tree
Showing 5 changed files with 69 additions and 68 deletions.
60 changes: 30 additions & 30 deletions src/main/cpp/Robot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,41 +32,41 @@ void Robot::RobotPeriodic() {
units::second_t loopTime = now - lastTotalLoopTime;
loopTimePub.Set((1 / loopTime).value());

m_container.GetNoteVisualizer().DisplayRobotNote(
m_container.GetFeederSubsystem().HasNote(),
m_container.GetSwerveSubsystem().GetRobotPose());
// m_container.GetNoteVisualizer().DisplayRobotNote(
// m_container.GetFeederSubsystem().HasNote(),
// m_container.GetSwerveSubsystem().GetRobotPose());

frc2::CommandScheduler::GetInstance().Run();
m_container.GetNoteVisualizer().Periodic();
UpdateVision();
m_container.GetSwerveSubsystem().CalculateFoundNotePose(m_container.GetVision().GetDistanceToNote(), m_container.GetVision().GetAngleToNote());
//m_container.GetNoteVisualizer().Periodic();
//UpdateVision();
//m_container.GetSwerveSubsystem().CalculateFoundNotePose(m_container.GetVision().GetDistanceToNote(), m_container.GetVision().GetAngleToNote());

lastTotalLoopTime = now;
}

void Robot::UpdateVision() {
auto visionEstimates = m_container.GetVision().GetCameraEstimatedPoses();
auto stdDevs = m_container.GetVision().GetPoseStdDevs(visionEstimates);

frc::Pose3d pose =
frc::Pose3d{m_container.GetSwerveSubsystem().GetRobotPose()};

cameraLocations[0] = pose.TransformBy(consts::vision::FL_ROBOT_TO_CAM);
cameraLocations[1] = pose.TransformBy(consts::vision::FR_ROBOT_TO_CAM);
cameraLocations[2] = pose.TransformBy(consts::vision::BL_ROBOT_TO_CAM);
cameraLocations[3] = pose.TransformBy(consts::vision::BR_ROBOT_TO_CAM);
cameraLocations[4] = pose.TransformBy(consts::vision::ROBOT_TO_NOTE_CAM);

cameraLocationsPub.Set(cameraLocations);

int i = 0;
for (const auto &est : visionEstimates) {
if (est.has_value()) {
// m_container.GetSwerveSubsystem().AddVisionMeasurement(est.value().estimatedPose.ToPose2d(),
// est.value().timestamp, stdDevs[i].value());
}
i++;
}
// auto visionEstimates = m_container.GetVision().GetCameraEstimatedPoses();
// auto stdDevs = m_container.GetVision().GetPoseStdDevs(visionEstimates);

// frc::Pose3d pose =
// frc::Pose3d{m_container.GetSwerveSubsystem().GetRobotPose()};

// cameraLocations[0] = pose.TransformBy(consts::vision::FL_ROBOT_TO_CAM);
// cameraLocations[1] = pose.TransformBy(consts::vision::FR_ROBOT_TO_CAM);
// cameraLocations[2] = pose.TransformBy(consts::vision::BL_ROBOT_TO_CAM);
// cameraLocations[3] = pose.TransformBy(consts::vision::BR_ROBOT_TO_CAM);
// cameraLocations[4] = pose.TransformBy(consts::vision::ROBOT_TO_NOTE_CAM);

// cameraLocationsPub.Set(cameraLocations);

// int i = 0;
// for (const auto &est : visionEstimates) {
// if (est.has_value()) {
// // m_container.GetSwerveSubsystem().AddVisionMeasurement(est.value().estimatedPose.ToPose2d(),
// // est.value().timestamp, stdDevs[i].value());
// }
// i++;
// }
}

void Robot::DisabledInit() {}
Expand Down Expand Up @@ -111,8 +111,8 @@ void Robot::SimulationPeriodic() {
// frc::sim::BatterySim::Calculate({m_container.GetSwerveSubsystem().GetSimulatedCurrentDraw()});
// frc::sim::RoboRioSim::SetVInVoltage(battVoltage);

m_container.GetVision().SimulationPeriodic(
m_container.GetSwerveSubsystem().GetOdomPose());
// m_container.GetVision().SimulationPeriodic(
// m_container.GetSwerveSubsystem().GetOdomPose());
}

#ifndef RUNNING_FRC_TESTS
Expand Down
51 changes: 26 additions & 25 deletions src/main/cpp/RobotContainer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,14 +50,14 @@ void RobotContainer::ConfigureBindings() {
// controller.Start().WhileTrue(
// swerveSubsystem.WheelRadius(frc2::sysid::Direction::kForward));

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);
}));
// 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);
// }));

// controller.A().WhileTrue(swerveSubsystem.SysIdDriveQuasistaticTorque(frc2::sysid::Direction::kForward));
// controller.B().WhileTrue(swerveSubsystem.SysIdDriveQuasistaticTorque(frc2::sysid::Direction::kReverse));
Expand All @@ -69,13 +69,13 @@ void RobotContainer::ConfigureBindings() {
// controller.POVUp().OnTrue(swerveSubsystem.TuneDrivePID([this] { return
// controller.Start().Get(); }));

controller.A().WhileTrue(shooterSubsystem.RunShooter(
[] { return consts::shooter::PRESET_SPEEDS::AMP; }));
controller.A().OnFalse(shooterSubsystem.RunShooter(
[] { return consts::shooter::PRESET_SPEEDS::OFF; }));
// 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.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));
Expand Down Expand Up @@ -115,27 +115,28 @@ void RobotContainer::ConfigureBindings() {
}

frc2::Command *RobotContainer::GetAutonomousCommand() {
return autos.GetSelectedCommand();
//return autos.GetSelectedCommand();
return nullptr;
}

SwerveSubsystem &RobotContainer::GetSwerveSubsystem() {
return swerveSubsystem;
}

ShooterSubsystem &RobotContainer::GetShooterSubsystem() {
return shooterSubsystem;
}
// ShooterSubsystem &RobotContainer::GetShooterSubsystem() {
// return shooterSubsystem;
// }

IntakeSubsystem &RobotContainer::GetIntakeSubsystem() {
return intakeSubsystem;
}

FeederSubsystem &RobotContainer::GetFeederSubsystem() {
return feederSubsystem;
}
// FeederSubsystem &RobotContainer::GetFeederSubsystem() {
// return feederSubsystem;
// }

str::Vision &RobotContainer::GetVision() { return vision; }
// str::Vision &RobotContainer::GetVision() { return vision; }

str::NoteVisualizer &RobotContainer::GetNoteVisualizer() {
return noteVisualizer;
}
// str::NoteVisualizer &RobotContainer::GetNoteVisualizer() {
// return noteVisualizer;
// }
4 changes: 2 additions & 2 deletions src/main/cpp/subsystems/SwerveSubsystem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,8 @@ SwerveSubsystem::SwerveSubsystem()
consts::swerve::pathplanning::ROTATION_D})) {
SetName("SwerveSubsystem");
frc::SmartDashboard::PutData(this);
SetupPathplanner();
LoadChoreoTrajectories();
//SetupPathplanner();
//LoadChoreoTrajectories();
}

void SwerveSubsystem::UpdateSwerveOdom() { swerveDrive.UpdateSwerveOdom(); }
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/commit.txt
View file
Original file line number Diff line number Diff line change
@@ -1 +1 @@
c83f2be
9255fe7
20 changes: 10 additions & 10 deletions src/main/include/RobotContainer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,23 +20,23 @@ class RobotContainer {
frc2::Command *GetAutonomousCommand();

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

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

SwerveSubsystem swerveSubsystem;
ShooterSubsystem shooterSubsystem;
// ShooterSubsystem shooterSubsystem;
IntakeSubsystem intakeSubsystem;
FeederSubsystem feederSubsystem;
str::Vision vision;
str::NoteVisualizer noteVisualizer;
// FeederSubsystem feederSubsystem;
// str::Vision vision;
// str::NoteVisualizer noteVisualizer;

Autos autos{swerveSubsystem, shooterSubsystem, intakeSubsystem,
feederSubsystem};
// Autos autos{swerveSubsystem, shooterSubsystem, intakeSubsystem,
// feederSubsystem};
};

0 comments on commit 28d4f57

Please sign in to comment.