diff --git a/OffSeasonBot2019/src/main/java/com/team2813/lib/angleAdjustment/AngleAdjustment.java b/OffSeasonBot2019/src/main/java/com/team2813/lib/angleAdjustment/AngleAdjustment.java new file mode 100644 index 0000000..97e44b2 --- /dev/null +++ b/OffSeasonBot2019/src/main/java/com/team2813/lib/angleAdjustment/AngleAdjustment.java @@ -0,0 +1,40 @@ +package com.team2813.lib.angleAdjustment; + +public class AngleAdjustment { + private double initialRight; + private double finalRight; + private double initialLeft; + private double finalLeft; + private double angleChange; + private final double GEAR_RATIO; + private final double WHEEL_DIAMETER; + private final double WHEEL_CIRCUMFERENCE; + private final int PULSES_PER_REVOLUTION; + //TODO add a parameter, either WHEEL_DIAMETER + public AngleAdjustment(double initialRight, double initialLeft, double angleChange, int pulsesPerRevolution, double wheelDiameter, double gearRatio){ + this.initialRight = initialRight; + this.initialLeft = initialLeft; + this.angleChange = angleChange; + PULSES_PER_REVOLUTION = pulsesPerRevolution; + WHEEL_DIAMETER = wheelDiameter; + WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI; + GEAR_RATIO = gearRatio; + calculate(); + //TODO convert degrees to encoder values which the left and right must travel + } + + public double getFinalRight() { + return finalRight; + } + + public double getFinalLeft() { + return finalLeft; + } + + private void calculate() { + double counts = angleChange / 360 * 42; + double wheelDegree = counts * GEAR_RATIO; + this.finalRight = initialRight + wheelDegree; + this.finalLeft = initialLeft + wheelDegree; + } +}