[nick] Added TODO and FIXME for potential bugs and improvements

src/main/cpp/ControllerInterface.cpp

@@ -8,7 +8,7 @@ void ControllerInterface::UpdateRobotControlData(RobotControlData &controlData)
     UpdateSmartplannerInput(controlData);
     UpdateClimberInput(controlData);
 
-    // code for the VibrateController function
+    // TODO: Move vibration code into its own method
     if (m_timer.Get().value()>=m_globalDuration)
     {
         m_pilot.SetRumble(frc::GenericHID::RumbleType::kLeftRumble, 0.0);

src/main/cpp/HAL/CoralLauncherHAL.cpp

@@ -65,11 +65,11 @@ double CoralLauncher::GetLeftWheelSpeed()
 }
 bool CoralLauncher::AreFlywheelsAtDesiredSpeed()
 {
-    return ((std::fabs(GetRightWheelSpeed() - m_desiredRightSpeed)<=SMALL_NUM)&&(std::fabs(GetLeftWheelSpeed() - m_desiredLeftSpeed)<=SMALL_NUM));
+    return ((std::fabs(GetRightWheelSpeed() - m_desiredRightSpeed)<=FLYWHEEL_ERROR)&&(std::fabs(GetLeftWheelSpeed() - m_desiredLeftSpeed)<=FLYWHEEL_ERROR));
 }
 
 bool CoralLauncher::BeamBreakStatus()
 {
-    //todo: implement beam break status code
+    // TODO: Is there any additional code required here?
     return m_beam_break.Get();
 }

src/main/cpp/InputManager/AlgaeRemoverManager.cpp

@@ -1,6 +1,5 @@
 #include "InputManager/AlgaeRemoverManager.h"
 
-
 void AlgaeRemoverManager::HandleInput(RobotControlData &robotControlData){
     if(robotControlData.algaeInput.RunRemoverTop){
         m_pivotAngleToTop = true;
@@ -10,6 +9,7 @@ void AlgaeRemoverManager::HandleInput(RobotControlData &robotControlData){
         m_pivotAngleToBottom = true;
     }
 
+    // TODO: Can these conditionals be separate functions?
     if(m_pivotAngleToTop){
         m_AlgaeRemover.ProfiledMoveToAngle(m_pivotAngleToRemoveTop); 
         m_removerTimer.Reset();

src/main/cpp/InputManager/CoralLauncherManager.cpp

@@ -1,6 +1,5 @@
 #include "InputManager/CoralLauncherManager.h"
 
-
 void CoralLauncherManager::HandleInput(RobotControlData &robotControlData){
     if(robotControlData.coralInput.setFlywheelToL1Speed){
         m_setFlywheelToL1Speed = true;
@@ -17,13 +16,13 @@ void CoralLauncherManager::HandleInput(RobotControlData &robotControlData){
     }
 
     if(m_setFlywheelToL1Speed){
-        m_CoralLauncher.SetWheelSpeeds(300.0, 500.0); //configure speeds
+        m_CoralLauncher.SetWheelSpeeds(300.0, 500.0);
     }
     if(m_setFlywheelToL2Speed){
-        m_CoralLauncher.SetWheelSpeeds(1050.0, 1050.0); //config speeds
+        m_CoralLauncher.SetWheelSpeeds(1050.0, 1050.0);
     }
     if(m_setFlywheelToZeroSpeed){
-        m_CoralLauncher.SetWheelSpeeds(0.0,0.0);
+        m_CoralLauncher.SetWheelSpeeds(0.0, 0.0);
     }
     m_CoralLauncher.SetIndexerSpeeds(robotControlData.coralInput.indexerSpeeds);
     // if(m_CoralLauncher.BeamBreakStatus()){

src/main/cpp/MoveToPose.cpp

@@ -60,65 +60,9 @@ units::degrees_per_second_t MoveToPose::angularRotation(frc::Rotation2d current,
     }
 
     return units::angular_velocity::degrees_per_second_t{val};
-
-    // switch (m_MoveAngleToState)
-    // {
-    //     case 0:
-    //     {
-
-
-
-    //         double start = m_current.Rotation().Degrees().value();
-    //         double end = desired.Degrees().value();
-
-    //         m_turn = end - start;
-    //         if (m_turn > 180.0)
-    //         {
-    //             m_turn = m_turn - 360.0;
-    //         }
-    //         else if (m_turn < -180.0)
-    //         {
-    //             m_turn = m_turn + 360.0;
-    //         }
-
-
-    //         m_angularVelocity = 0_deg_per_s;
-    //         m_timer.Restart();
-    //         m_MoveAngleToState++;
-    //         break;
-    //     }
-    //     case 1:
-    //     {
-    //         auto setPoint = m_Profile.Calculate(
-    //             m_timer.Get(),
-    //             frc::TrapezoidProfile<units::degrees>::State{units::degree_t{0.0f}, 0_deg_per_s},
-    //             frc::TrapezoidProfile<units::degrees>::State{units::degree_t{m_turn}, 0_deg_per_s}    // insert the better end state here       
-    //         );
-
-    //         m_angularVelocity = setPoint.velocity;
-
-    //         if (m_Profile.IsFinished(m_timer.Get())) {
-    //             m_MoveAngleToState++;
-    //         }
-
-    //         break;
-    //     }
-    //     case 2:
-    //     {
-    //         m_timer.Stop();
-    //         m_MoveAngleToState++;
-    //         break;
-    //     }
-    //     default:
-    //     {
-    //         break;
-    //     }
-    // }
-    // return m_angularVelocity;
 };
 
 std::pair<units::feet_per_second_t, units::feet_per_second_t> MoveToPose::linearTranslation(frc::Pose2d desired) {
-    // double distance = sqrt(pow((double)desired.X() - current.X(), 2.0) + pow(0.0 + desired.Y() - current.Y(), 2.0));
     switch (m_MoveTranslationToState)
     {
     case 0:
@@ -142,7 +86,7 @@ std::pair<units::feet_per_second_t, units::feet_per_second_t> MoveToPose::linear
 
         m_timerLin.Restart();
         m_MoveTranslationToState++;
-        // FIXME: switch statement fall-through, is this intentional?
+        break;
     }
     case 1:
     {

src/main/cpp/SmartPlanner.cpp

@@ -1,6 +1,5 @@
 #include "SmartPlanner.h"
 
-
 SmartPlanner::SmartPlanner(PhotonVisionCamera &cam, WPISwerveDrive &swerve)
     : m_Cam(cam)
     , m_Swerve(swerve)

src/main/include/ControllerInterface.h

@@ -1,4 +1,5 @@
 #pragma once
+
 #include <frc/XboxController.h>
 #include "RobotControlData.h"
 #include <frc/Timer.h>

src/main/include/HAL/CoralLauncherHAL.h

@@ -1,4 +1,5 @@
 #pragma once
+
 #include "RobotControlData.h"
 #include "rev/SparkMax.h"
 #include <frc/Timer.h>
@@ -21,8 +22,8 @@ class CoralLauncher
         rev::spark::SparkMax m_leftMotor{42, rev::spark::SparkMax::MotorType::kBrushless};
         rev::spark::SparkMax m_indexer1{40, rev::spark::SparkMax::MotorType::kBrushless};
         rev::spark::SparkMax m_indexer2{41, rev::spark::SparkMax::MotorType::kBrushless};
-        frc::DigitalInput m_beam_break{0}; //moved from CoralLauncherManager; move back if needed
+        frc::DigitalInput m_beam_break{0};
         double m_desiredRightSpeed;
         double m_desiredLeftSpeed; 
-        const double SMALL_NUM = 0.001; //change variable name accordingly; make a more suitable name                            
+        const double FLYWHEEL_ERROR = 0.001;                          
 };

src/main/include/InputManager/AlgaeRemoverManager.h

@@ -1,22 +1,5 @@
 #pragma once
 
-// #include "RobotControlData.h"
-// #include "RobotControlData.h"
-// #include "rev/SparkMax.h"
-// #include "rev/SparkAbsoluteEncoder.h"
-// #include "rev/SparkClosedLoopController.h"
-// #include <frc/Timer.h>
-// #include <frc/trajectory/TrapezoidProfile.h>
-// #include <units/length.h>
-// #include <units/velocity.h>
-// #include <units/angle.h>
-// #include <units/acceleration.h>
-// #include <units/angular_velocity.h>
-// #include <units/angular_acceleration.h>
-// #include <frc/trajectory/TrapezoidProfile.h>
-// #include <frc/smartdashboard/SmartDashboard.h>
-// #include <frc/DigitalInput.h>
-
 #include "HAL/AlgaeRemoverHAL.h"
 
 // half of these are probably not needed
@@ -34,8 +17,8 @@ class AlgaeRemoverManager
     bool m_pivotAngleToTop = false;
     bool m_pivotAngleToBottom = false;
     double m_pivotAngle;
-    double m_pivotAngleToRemoveTop = 45.0; //this will probably change later depending on arm design. this is just for default
-    double m_pivotAngleToRemoveBottom = 45.0; //this will probably change later depending on arm design. this is just for default
+    double m_pivotAngleToRemoveTop = 45.0; // TODO: Tune this?
+    double m_pivotAngleToRemoveBottom = 45.0; // TODO: Tune this?
     double m_RemoverSpeed = 0.0;
     units::second_t m_RemoverTime = 2.0_s;
     frc::Timer m_removerTimer;

src/main/include/InputManager/CoralLauncherManager.h

@@ -4,7 +4,6 @@
 #include "RobotControlData.h"
 #include <frc/DigitalInput.h>
 
-
 class CoralLauncherManager
 {
 public:

src/main/include/MechanismConfig.h

@@ -6,7 +6,6 @@
 namespace {
 namespace ratbot
 {
-
     const double VOLTAGE_COMPENSATION = 10.5f;
 
     namespace CoralLauncherConfig
@@ -80,7 +79,7 @@ namespace ratbot
 
     namespace MoveToPoseConfig
     {
-        // TODO: tune values on carpet
+        // TODO: We need to tune these values for field carpet
         const double MAX_TURN_SPEED_DEG_PER_SEC = 120.0f;
         const double TURN_FEED_FORWARD_DEG_PER_SEC = 10.0f;
     }

src/main/include/MoveToPose.h

@@ -14,8 +14,6 @@
 #include <units/length.h>
 #include <units/velocity.h>
 #include <units/acceleration.h>
-//#include <units/math.h>
-
 
 class MoveToPose
 {
@@ -56,13 +54,5 @@ class MoveToPose
         // Uses swerve odometry to generate the 'initial' pose
         frc::ChassisSpeeds move(frc::Pose2d current, frc::Pose2d desired);
         void reset();
-        bool isDone();
-
-        // TODO: all functions below should be private
-        //       all functions should have (current, desired)... instead of (initial, desired) or (desired, current)
-
-        // Moves robot from initial to desired
-
-        // TODO: create helper functions as needed
-
-};
+        bool isDone();         
+};

src/main/include/PhotonVisionCamera.h

@@ -23,6 +23,5 @@ class PhotonVisionCamera
         PhotonVisionCamera(std::string name, frc::Transform3d robotToCamera);
         std::optional<photon::EstimatedRobotPose> GetPose();
         void SaveResult();
-        int GetAprilTagID();
-
+        int GetAprilTagID();        
 };