diff --git a/src/main/java/frc/robot/classes/Kinematics.java b/src/main/java/frc/robot/classes/Kinematics.java index 99c2a08c..c89c3616 100644 --- a/src/main/java/frc/robot/classes/Kinematics.java +++ b/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); } } diff --git a/src/main/java/frc/robot/classes/SmoothControl.java b/src/main/java/frc/robot/classes/SmoothControl.java index e1c7bd89..f9a8b5bd 100644 --- a/src/main/java/frc/robot/classes/SmoothControl.java +++ b/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 diff --git a/src/main/java/frc/robot/commands/AutoBalance.java b/src/main/java/frc/robot/commands/AutoBalance.java index c26ceaa4..be70a1ab 100644 --- a/src/main/java/frc/robot/commands/AutoBalance.java +++ b/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; } diff --git a/src/main/java/frc/robot/commands/Rotate.java b/src/main/java/frc/robot/commands/Rotate.java index 8ba3acda..9bf3e22a 100644 --- a/src/main/java/frc/robot/commands/Rotate.java +++ b/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; diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index c0ca191b..6c4993aa 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/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()); } /** diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 4158ada0..a248c6f8 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/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)); diff --git a/src/main/java/frc/robot/subsystems/Targeting.java b/src/main/java/frc/robot/subsystems/Targeting.java index 153d9b0c..b86a68c5 100644 --- a/src/main/java/frc/robot/subsystems/Targeting.java +++ b/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;