Code cleanup

src/main/java/frc/robot/classes/Kinematics.java

@@ -14,12 +14,12 @@ public Kinematics(Position2D startingPosition) {
     m_previousRightEncoderPosition = 0;
   }
 
-  // This function runs the kinematics caluclation,
+  // This function runs the kinematics calculation,
   // given the delta Psi to use instead fo calculating from encoders
-  public void calculatePosition(double leftEncoderPostion, double rightEncoderPosition, double heading) {
+  public void calculatePosition(double leftEncoderPosition, double rightEncoderPosition, double heading) {
     double currentHeading = 0.0d;
 
-    updateCurrentPose(leftEncoderPostion, rightEncoderPosition);
+    updateCurrentPose(leftEncoderPosition, rightEncoderPosition);
 
     // Limit psi between Pi and -Pi
     currentHeading = limitRadians(heading);
@@ -28,13 +28,13 @@ public void calculatePosition(double leftEncoderPostion, double rightEncoderPosi
     m_currentPose.setHeadingRadians(currentHeading);
   }
 
-  // This function runs the kinematics caluclation,
+  // This function runs the kinematics calculation,
   // calculating delta Psi based off of encoder distance traveled
-  public void calculatePosition(double leftEncoderPostion, double rightEncoderPosition) {
-    updateCurrentPose(leftEncoderPostion, rightEncoderPosition);
+  public void calculatePosition(double leftEncoderPosition, double rightEncoderPosition) {
+    updateCurrentPose(leftEncoderPosition, rightEncoderPosition);
   }
 
-  private void updateCurrentPose(double leftEncoderPostion, double rightEncoderPosition) {
+  private void updateCurrentPose(double leftEncoderPosition, double rightEncoderPosition) {
     double deltaLeft = 0.0d;
     double deltaRight = 0.0d;
     double deltaX = 0.0d;
@@ -43,11 +43,11 @@ private void updateCurrentPose(double leftEncoderPostion, double rightEncoderPos
     double newPsi = 0.0d;
 
     // Calculate distance traveled
-    deltaLeft = (leftEncoderPostion - m_previousLeftEncoderPosition);
+    deltaLeft = (leftEncoderPosition - m_previousLeftEncoderPosition);
     deltaRight = (rightEncoderPosition - m_previousRightEncoderPosition);
 
     // Save distance traveled
-    m_previousLeftEncoderPosition = leftEncoderPostion;
+    m_previousLeftEncoderPosition = leftEncoderPosition;
     m_previousRightEncoderPosition = rightEncoderPosition;
 
     // Use encoders to calculate heading
@@ -58,7 +58,7 @@ private void updateCurrentPose(double leftEncoderPostion, double rightEncoderPos
     deltaPsi = limitRadians(deltaPsi);
     newPsi = limitRadians(m_currentPose.getHeadingRadians() + deltaPsi);
 
-    // Calcualte new X, Y
+    // Calculate new X, Y
     deltaX = ((deltaLeft + deltaRight) / 2) * Math.cos(m_currentPose.getHeadingRadians()); // previous heading is in
                                                                                            // radians
     deltaY = ((deltaLeft + deltaRight) / 2) * Math.sin(m_currentPose.getHeadingRadians()); // previous heading is in
@@ -98,17 +98,11 @@ public double distanceFromPose(Position2D checkpoint) {
     double y1 = m_currentPose.getY();
     double x2 = checkpoint.getX();
     double y2 = checkpoint.getY();
-    double distance = Math.sqrt(Math.pow((x2 - x1), 2) + Math.pow((y2 - y1), 2));
-
-    return distance;
+    return (Math.sqrt(Math.pow((x2 - x1), 2) + Math.pow((y2 - y1), 2)));
   }
 
   public boolean atPose(Position2D checkpoint, double threshold) {
     double distanceFromPose = distanceFromPose(checkpoint);
-    if (distanceFromPose <= threshold) {
-      return true;
-    } else {
-      return false;
-    }
+    return (distanceFromPose <= threshold);
   }
 }

src/main/java/frc/robot/classes/SmoothControl.java

@@ -8,7 +8,7 @@ public SmoothControl() {
   }
 
   // Modification of equation 13, Calculates the omegaDesired (in radians)
-  // modified by velocity aggressivness given required turn rate
+  // modified by velocity aggressiveness given required turn rate
   public double computeTurnRate(Position2D currentPose, Position2D targetPose, double givenMaxVelocity, boolean inReverse) {
     double omegaDesired = 0.0d; 
     double poseHeading = currentPose.getHeadingRadians(); // Convert heading to radians

src/main/java/frc/robot/commands/AutoBalance.java

@@ -8,7 +8,7 @@ public class AutoBalance extends CommandBase {
     private static final double PITCH_OFFSET = -73.0d;
     private static final int BALANCE_THRESHOLD = 4;
     private static final int BALANCE_ITERATIONS = 20;
-    private static final double INTERGRAL_LIMIT = 5;
+    private static final double INTEGRAL_LIMIT = 5;
 
     private double m_pitch;
     private double m_error = 0.0d;
@@ -21,7 +21,7 @@ public class AutoBalance extends CommandBase {
 
     // start (gives throttle) (may make it overshoot if too high)
     private double m_P = 0.0031d;
-    // finicky (depends on situation) (within 5 to 3 degress of error)
+    // finicky (depends on situation) (within 5 to 3 degrees of error)
     private double m_I = 0.00d;
     // good rule of thumb for d: m_d = m_p * 10
     private double m_D = 0.04;
@@ -92,7 +92,7 @@ public void execute() {
 
         m_change = m_error - m_lastError;
 
-        if (Math.abs(m_errorIntegral) < INTERGRAL_LIMIT) {
+        if (Math.abs(m_errorIntegral) < INTEGRAL_LIMIT) {
             // Accumulate the error into the integral
             m_errorIntegral += m_error;
         }

src/main/java/frc/robot/commands/Rotate.java

@@ -7,7 +7,7 @@ public class Rotate extends CommandBase {
 
     private final Drivetrain m_drivetrain;
     private double m_targetDegrees;
-    private double initalAngle;
+    private double initialAngle;
     private double m_lastError = 0.0d;
     private double m_errorIntegral = 0.0d;
     private double m_error;
@@ -20,7 +20,7 @@ public class Rotate extends CommandBase {
     public Rotate(Drivetrain drivetrain, double targetDegrees) {
         m_drivetrain = drivetrain;
         m_targetDegrees = targetDegrees;
-        initalAngle = m_drivetrain.getGyroYawDegrees();
+        initialAngle = m_drivetrain.getGyroYawDegrees();
 
         addRequirements(m_drivetrain);
     }
@@ -36,7 +36,7 @@ public void initialize() {
     @Override
     public void execute() {
         double percentOutput = 0.0d;
-        m_error = Math.abs(m_drivetrain.getGyroYawDegrees() - Math.abs(initalAngle - m_targetDegrees));
+        m_error = Math.abs(m_drivetrain.getGyroYawDegrees() - Math.abs(initialAngle - m_targetDegrees));
 
         m_change = m_error - m_lastError;
 

src/main/java/frc/robot/subsystems/Arm.java

@@ -174,7 +174,7 @@ public void periodic() {
                 pivotTalonFX.set(-PIVOT_CALIBRATION_MOTOR_SPEED);
             }
         } else {
-            // We are now calibrated and we know the position of the arm!
+            // We are now calibrated, and we know the position of the arm!
             moveToPosition(theta, rInches);
         }
     }
@@ -209,13 +209,11 @@ public void rotateTo(double radians) {
      * @return
      */
     public double getPivotEncoderUnits() {
-        double encoderUnits = pivotTalonFX.getSelectedSensorPosition();
-        return encoderUnits;
+        return (pivotTalonFX.getSelectedSensorPosition());
     }
 
     public double getExtenderEncoderUnits() {
-        double encoderUnits = extenderTalonFX.getSelectedSensorPosition();
-        return encoderUnits;
+        return (extenderTalonFX.getSelectedSensorPosition());
     }
 
     /**

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -85,7 +85,7 @@ public Drivetrain(Kinematics kinematics) {
         leftTalonLead.clearStickyFaults();
         rightTalonLead.clearStickyFaults();
 
-        // Set facotry defaults for onboard PID
+        // Set factory defaults for onboard PID
         leftTalonLead.configFactoryDefault();
         rightTalonLead.configFactoryDefault();
 
@@ -192,7 +192,7 @@ public void periodic() {
                 put("Kinematics Heading (degrees)", currentPose.getHeadingDegrees());
                 put("Left Encoder Velocity (Ft per S)", getLeftEncoderVelocity());
                 put("Left Encoder Position (Ft)", getLeftEncoderPosition());
-                put("Right Encoder Veloctiy (Ft per S)", getRightEncoderVelocity());
+                put("Right Encoder Velocity (Ft per S)", getRightEncoderVelocity());
                 put("Right Encoder Position (Ft)", getRightEncoderPosition());
                 put("Raw Left Encoder", leftTalonLead.getSelectedSensorPosition(0));
                 put("Raw Right Encoder", rightTalonLead.getSelectedSensorPosition(0));
@@ -320,7 +320,7 @@ public double getRightEncoderPosition() {
      * @return left encoder Velocity in ft/s
      */
     public double getLeftEncoderVelocity() {
-        // Returns the velocity of encoder by claculating the velocity from encoder
+        // Returns the velocity of encoder by calculating the velocity from encoder
         // units of click/100ms to ft/s
         return SPIKE293Utils.controllerVelocityToFeetPerSec(leftTalonLead.getSelectedSensorVelocity());
     }
@@ -331,7 +331,7 @@ public double getLeftEncoderVelocity() {
      * @return right encoder Velocity in ft/s
      */
     public double getRightEncoderVelocity() {
-        // Returns the velocity of encoder by claculating the velocity from encoder
+        // Returns the velocity of encoder by calculating the velocity from encoder
         // units of click/100ms to ft/s
         return SPIKE293Utils.controllerVelocityToFeetPerSec(rightTalonLead.getSelectedSensorVelocity());
     }
@@ -342,8 +342,8 @@ public double getRightEncoderVelocity() {
      * @return robot Velocity in ft/s
      */
     public double getRobotVelocity() {
-        // Returns the velocity of the robot by taking the averga of the velcity on both
-        // sides of the robor
+        // Returns the velocity of the robot by taking the average of the velocity on both
+        // sides of the robot
         return (getLeftEncoderVelocity() + getRightEncoderVelocity()) / 2.0d;
     }
 
@@ -398,7 +398,7 @@ public void resetGyro(double headingDegrees) {
         setupGyro(navX, headingDegrees);
     }
 
-    // rotates robot according to give degress using arc length formula
+    // rotates robot according to give degrees using arc length formula
     public void rotateDegrees(double angle) {
         double radians = Math.toRadians(angle);
         double arcLength = (radians * (TRACK_WIDTH_FEET / 2.0));

src/main/java/frc/robot/subsystems/Targeting.java

@@ -46,7 +46,7 @@ public Targeting() {
         m_limeData.getEntry("ledMode").setNumber(LIMELIGHT_LED_ON);
         m_limeData.getEntry("pipeline").setNumber(1.0d);
 
-        SmartDashboard.putBoolean("isTargetted", false);
+        SmartDashboard.putBoolean("isTargeted", false);
 
         controlLight(true);
     }
@@ -55,7 +55,7 @@ public Targeting() {
     public void periodic() {
         SmartDashboard.putNumber("Tx", m_targetX.getDouble(10000));
         SmartDashboard.putNumber("Ty", m_targetY.getDouble(10000));
-        SmartDashboard.putBoolean("isTargetted", isTargeted());
+        SmartDashboard.putBoolean("isTargeted", isTargeted());
         SmartDashboard.putNumber("Distance from Target", calcDistance());
     }
 
@@ -76,7 +76,6 @@ public double calcShooterRPM() {
         double retval = DEFAULT_TARGET_RPM;
         double ty = m_targetY.getDouble(0.0);
         if (m_tAcquired.getDouble(0.0) == TARGET_ACQUIRED) {
-            //retv  al = (-30.07 * ty) + 1690.42;
             retval = (230 * Math.pow(Math.E, ((-0.237 * ty) - 1.5))) + 1680.48;
             if(retval > 2900.0){
                 retval = 2900.0;