diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java index 9d0970057818..7de6b2472b37 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java @@ -47,9 +47,7 @@ public class Hardware extends HardwareMapper implements TriOdoProvider { public DcMotor encoderRight; @Override - public DcMotor getLeftEncoder() { - return encoderLeft; - } + public DcMotor getLeftEncoder() { return encoderLeft; } @Override public DcMotor getRightEncoder() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pose2PoseTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pose2PoseTest.java index bcdbef85f071..137585524ab7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pose2PoseTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pose2PoseTest.java @@ -18,6 +18,7 @@ public class Pose2PoseTest extends LinearOpMode { public static final double ACCEPT_DIST = 1.0; // inch. euclidean distance public static final double ACCEPT_TURN = degrees(5); // radian. + // power biases public static final Motion.Calibrate CALIBRATION = new Motion.Calibrate(1.0, 1.0, 1.0); private static double degrees(double deg) { @@ -29,6 +30,7 @@ private static double degrees(double deg) { public void runOpMode() { Hardware hardware = new Hardware(hardwareMap); EncoderTracking tracker = new EncoderTracking(hardware); + // Pose targets to go thru Pose[] targets = { new Pose(24, 0, degrees(90)), new Pose(24, 24, degrees(180)), @@ -43,7 +45,9 @@ public void runOpMode() { timer.reset(); boolean wait = false; while (opModeIsActive()) { + // Updates pose tracker.step(); + // Gets current pose Pose p = tracker.getPose(); if (wait) { hardware.driveMotors.setAll(0); @@ -56,11 +60,13 @@ public void runOpMode() { wait = false; } } else { + // Calculates the distance between current pose and target pose double linear = p.linearDistanceTo(targets[targetIndex]); double angular = p.subtractAngle(targets[targetIndex]); if (linear > ACCEPT_DIST || abs(angular) > ACCEPT_TURN) { targetTime.reset(); } + // Waits at the target for one second if (targetTime.time() > 1.0) { targetIndex++; wait = true; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/EncoderTracking.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/EncoderTracking.java index 9fa03389f28b..c07d7207de3d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/EncoderTracking.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/EncoderTracking.java @@ -24,6 +24,7 @@ private double tick2inch(int ticks) { double lastLeft, lastCenter, lastRight; final DcMotor leftEncoder, centerEncoder, rightEncoder; + // TriOdoProvider provides the methods for the getting the encoders public EncoderTracking(TriOdoProvider encoderSource) { leftEncoder = encoderSource.getLeftEncoder(); centerEncoder = encoderSource.getCenterEncoder(); @@ -37,20 +38,21 @@ public EncoderTracking(TriOdoProvider encoderSource) { lastCenter = tick2inch(centerEncoder.getCurrentPosition()); lastRight = tick2inch(rightEncoder.getCurrentPosition()); } - + // Updates the pose public void step() { + // Current encoder values double currentLeft = tick2inch(leftEncoder.getCurrentPosition()); double currentCenter = tick2inch(centerEncoder.getCurrentPosition()); double currentRight = tick2inch(rightEncoder.getCurrentPosition()); - + // Change in the encoders' values double deltaLeft = currentLeft - lastLeft; double deltaCenter = currentCenter - lastCenter; double deltaRight = currentRight - lastRight; - + // Change in distances double deltaTurn = (deltaRight - deltaLeft) / trackWidth; double deltaForward = (deltaLeft + deltaRight) / 2.; double deltaStrafe = deltaCenter - forwardOffset * deltaTurn; - + // Predicts how much our robot turns so we know how much forward and strafe we now need - the heading changes enough in each loop that we need to predict it double nextHeading = heading + deltaTurn; double avgHead = (heading + nextHeading) / 2.; double deltaX = deltaForward * cos(avgHead) - deltaStrafe * sin(avgHead); @@ -64,7 +66,7 @@ public void step() { lastCenter = currentCenter; lastRight = currentRight; } - +// Gets current pose public Pose getPose() { return new Pose( x, y, heading diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/Pose.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/Pose.kt index c89f0cd24683..d43e14a64939 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/Pose.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/Pose.kt @@ -34,7 +34,7 @@ data class Pose( fun subtractAngle(other: Pose): Double { return wrapAngle(other.heading - heading) } - + // Sets the power? fun to(other: Pose, robot: TriOdoProvider): Motion { val dh = wrapAngle(other.heading - heading) val dx = other.x - x diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/TriOdoProvider.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/TriOdoProvider.java index f51a234d1433..07a03e8ac74f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/TriOdoProvider.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/TriOdoProvider.java @@ -1,7 +1,9 @@ package org.firstinspires.ftc.teamcode.mmooover; import com.qualcomm.robotcore.hardware.DcMotor; - +// An interface describes what methods a class needs to have +// The hardware.java has methods and other stuff that can be used no matter what robot hub thing +// so we used the interface which describes what methods we will provide for this specific program public interface TriOdoProvider { DcMotor getLeftEncoder(); DcMotor getRightEncoder();