diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1998ef5..eb47e81 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,11 @@ public class RobotContainer { configureBindings(); } + // The objective should be to have the subsystems expose methods that return commands + // that can be bound to the triggers provided by the CommandXboxController class. + // This mindset should help keep RobotContainer a little cleaner this year. + // This is not to say you can't trigger or command chain here (see driveCardnial drivetrain example), + // but generally avoid any situation where the keyword "new" is involved private void configureBindings() { drivetrain.setDefaultCommand(drivetrain.teleopCommand( primary::getLeftX, diff --git a/src/main/java/frc/robot/utilities/SwerveUtils.java b/src/main/java/frc/robot/utilities/SwerveUtils.java index 48fe870..18d2ab7 100644 --- a/src/main/java/frc/robot/utilities/SwerveUtils.java +++ b/src/main/java/frc/robot/utilities/SwerveUtils.java @@ -12,11 +12,9 @@ public class SwerveUtils { public static double StepTowards(double _current, double _target, double _stepsize) { if (Math.abs(_current - _target) <= _stepsize) { return _target; - } - else if (_target < _current) { + } else if (_target < _current) { return _current - _stepsize; - } - else { + } else { return _current + _stepsize; } } @@ -38,18 +36,15 @@ public class SwerveUtils { if (difference <= _stepsize) { return _target; - } - else if (difference > Math.PI) { //does the system need to wrap over eventually? + } else if (difference > Math.PI) { //does the system need to wrap over eventually? //handle the special case where you can reach the target in one step while also wrapping if (_current + 2*Math.PI - _target < _stepsize || _target + 2*Math.PI - _current < _stepsize) { return _target; - } - else { + } else { return WrapAngle(_current - stepDirection * _stepsize); //this will handle wrapping gracefully } - } - else { + } else { return _current + stepDirection * _stepsize; } } @@ -75,14 +70,11 @@ public class SwerveUtils { if (_angle == twoPi) { // Handle this case separately to avoid floating point errors with the floor after the division in the case below return 0.0; - } - else if (_angle > twoPi) { + } else if (_angle > twoPi) { return _angle - twoPi*Math.floor(_angle / twoPi); - } - else if (_angle < 0.0) { + } else if (_angle < 0.0) { return _angle + twoPi*(Math.floor((-_angle) / twoPi)+1); - } - else { + } else { return _angle; } }