From 3950a83ac1e27f6a1c39250d31651814c51d827b Mon Sep 17 00:00:00 2001
From: Carlos Rivas <carlos@riv.as>
Date: Tue, 1 Oct 2024 10:19:40 -0700
Subject: [PATCH] Added encoder naming and usage

---
 .../ftc/teamcode/BasicOmniOpMode_Linear.java  | 33 +++++++++++++++++++
 .../ftc/teamcode/PedroConstants.java          |  6 ++--
 2 files changed, 36 insertions(+), 3 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java
index 16f97c0..e33996f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java
@@ -30,6 +30,8 @@
 package org.firstinspires.ftc.teamcode;
 
 
+import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
+import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
@@ -38,12 +40,19 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
 import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
+import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
+import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
+import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
+import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
 
 import org.firstinspires.ftc.teamcode.PedroConstants.*;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
+
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 /*
@@ -83,6 +92,9 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
     private DcMotor leftBackDrive = null;
     private DcMotor rightFrontDrive = null;
     private DcMotor rightBackDrive = null;
+    private Encoder leftEncoder;
+    private Encoder rightEncoder;
+    private Encoder strafeEncoder;
 
     @Override
     public void runOpMode() {
@@ -109,6 +121,20 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
         rightFrontDrive.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
         rightBackDrive.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
 
+        // TODO: replace these with your encoder ports
+        leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
+        rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
+        strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
+
+        // TODO: reverse any encoders necessary
+        leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
+        rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
+        strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
+
+        leftEncoder.reset();
+        rightEncoder.reset();
+        strafeEncoder.reset();
+
         // Wait for the game to start (driver presses START)
         telemetry.addData("Status", "Initialized");
         telemetry.update();
@@ -168,10 +194,17 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
             leftBackDrive.setPower(leftBackPower);
             rightBackDrive.setPower(rightBackPower);
 
+            leftEncoder.update();
+            rightEncoder.update();
+            strafeEncoder.update();
+
             // Show the elapsed game time and wheel power.
             telemetry.addData("Status", "Run Time: " + runtime.toString());
             telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
             telemetry.addData("Back  left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
+            telemetry.addData("Encoder Left", leftEncoder.getDeltaPosition());
+            telemetry.addData("Encoder Right", rightEncoder.getDeltaPosition());
+            telemetry.addData("Encoder Back (Right is +)", strafeEncoder.getDeltaPosition());
             telemetry.update();
         }
     }}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java
index a238c58..f63c7ee 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java
@@ -33,9 +33,9 @@ public class PedroConstants {
             = RevHubOrientationOnRobot.UsbFacingDirection.UP;
 
     // Robot encoders
-    public static final String LEFT_ENCODER = "front-left";
-    public static final String RIGHT_ENCODER = "front-right";
-    public static final String BACK_ENCODER = "back-left";
+    public static final String LEFT_ENCODER = "encoder-left";
+    public static final String RIGHT_ENCODER = "encoder-right";
+    public static final String BACK_ENCODER = "encoder-back";
 
     // Robot encoder direction
     public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;