Remove Redundant Imports: Remove any unused imports to clean up the code.
Optimize Command Initialization: Group similar command initializations together to reduce redundancy.
Use Constants: Use constants for repeated values to avoid magic numbers.
Simplify Method Calls: Simplify method calls and reduce nested calls where possible.
Refactor Long Methods: Break down long methods into smaller, more manageable methods.

There might be future changes

Signed-off-by: RITESH RAJ AS <riteshrajas.arulselvan@gmail.com>

There might be future changes

Signed-off-by: RITESH RAJ AS <riteshrajas.arulselvan@gmail.com>

Ensured the FODC command requires the drivetrain subsystem by calling addRequirements(drivetrain) in the constructor.
Added DoubleSupplier for dynamic value updates:
Introduced DoubleSupplier for c_leftX and c_leftY to dynamically supply joystick values.
Reverted Shuffleboard calls back to SmartDashboard:
Replaced all Shuffleboard calls with SmartDashboard calls to avoid issues with duplicate titles and simplify the code.

Signed-off-by: RITESH RAJ AS <riteshrajas.arulselvan@gmail.com>

Added DoubleSupplier for dynamic value updates.
Reverted Shuffleboard calls back to SmartDashboard.
Pushed changes to GitHub.
Semi-successfully implemented FODC natively into the robot code.
Simulated without issues; testing on the robot is pending but expected to work on the first try.
Tags: testing required

Signed-off-by: RITESH RAJ AS <riteshrajas.arulselvan@gmail.com>

…em when the angle error is within ±5 degrees.

Added logic to allow the robot to move freely in all directions like a swerve drive when the angle error is within the specified range.
Updated SmartDashboard to display relevant angle and error values.
Ensured the drivetrain control logic continues if the angle error is outside the specified range.

Signed-off-by: RITESH RAJ AS <riteshrajas.arulselvan@gmail.com>

- Added target angle calculation based on joystick input
- Integrated FODCSubsystem for filtered angle and angular velocity
- Applied rotational correction using PID based on angle error
- Enabled dynamic speed adjustment based on performance metrics
- Updated SmartDashboard with filtered values and control outputs

Signed-off-by: RITESH RAJ AS <riteshrajas.arulselvan@gmail.com>

Signed-off-by: RITESH RAJ AS <riteshrajas.arulselvan@gmail.com>

Changed isFinished to always return false to ensure the command runs indefinitely.
Added Shuffleboard Integration:
Added a ShuffleboardTab to display various parameters.
Added parameters to the Shuffleboard tab in the initialize method.
Updated parameters in the execute method.
Dynamic Speed Multiplier:
Added a method adjustSpeedMultiplierBasedOnPerformance to dynamically adjust the speed multiplier based on performance metrics.
Used this method in the execute method to adjust the speed multiplier.
Refactored Constructor:
Updated the constructor to initialize the ShuffleboardTab.
Refactored execute Method:
Added logic to update Shuffleboard values.
Added logic to adjust the speed multiplier dynamically.
Removed Kalman Filter Control:
The Kalman filter no longer influences how the robot moves but still runs in the background.
Remapped Robot Movement:
Remapped how the robot moves with a bunch of speed shifters.

Signed-off-by: RITESH RAJ AS <riteshrajas.arulselvan@gmail.com>

Changed isFinished to always return false to ensure the command runs indefinitely.
Added Shuffleboard Integration:
Added a ShuffleboardTab to display various parameters.
Added parameters to the Shuffleboard tab in the initialize method.
Updated parameters in the execute method.
Dynamic Speed Multiplier:
Added a method adjustSpeedMultiplierBasedOnPerformance to dynamically adjust the speed multiplier based on performance metrics.
Used this method in the execute method to adjust the speed multiplier.
Refactored Constructor:
Updated the constructor to initialize the ShuffleboardTab.
Refactored execute Method:
Added logic to update Shuffleboard values.
Added logic to adjust the speed multiplier dynamically.
Removed Kalman Filter Control:
The Kalman filter no longer influences how the robot moves but still runs in the background.
Remapped Robot Movement:
Remapped how the robot moves with a bunch of speed shifters.

Signed-off-by: RITESH RAJ AS <riteshrajas.arulselvan@gmail.com>