From 287fef6443056d42208bd352af4c0842f646a468 Mon Sep 17 00:00:00 2001
From: Scott Barnes <scott@tearabite.com>
Date: Sat, 28 Oct 2023 12:32:28 -0500
Subject: [PATCH] All working

---
 .../src/main/java/opmodes/MainTeleOp.java     | 27 +++++++++
 TeamCode/src/main/java/opmodes/Sandbox.java   | 28 ----------
 .../ftc/teamcode/hardware/Drive.java          | 55 ++++++++++++++++---
 3 files changed, 73 insertions(+), 37 deletions(-)
 create mode 100644 TeamCode/src/main/java/opmodes/MainTeleOp.java
 delete mode 100644 TeamCode/src/main/java/opmodes/Sandbox.java

diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java
new file mode 100644
index 0000000..8c10398
--- /dev/null
+++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java
@@ -0,0 +1,27 @@
+package opmodes;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+
+@TeleOp(name = "MainTeleOp", group = "Main")
+public class MainTeleOp extends OpMode {
+
+    private Robot robot;
+
+    @Override
+    public void init() {
+        this.robot = new Robot(this.hardwareMap);
+        telemetry.addData("Status", "Initialized");
+    }
+
+    @Override
+    public void loop() {
+        double x = gamepad1.left_stick_x;
+        double y = -gamepad1.left_stick_y;
+        double z = gamepad1.right_stick_x;
+
+        this.robot.getDrive().setInput(x, y, z);
+    }
+}
diff --git a/TeamCode/src/main/java/opmodes/Sandbox.java b/TeamCode/src/main/java/opmodes/Sandbox.java
deleted file mode 100644
index 755a6a1..0000000
--- a/TeamCode/src/main/java/opmodes/Sandbox.java
+++ /dev/null
@@ -1,28 +0,0 @@
-package opmodes;
-
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.firstinspires.ftc.teamcode.hardware.Camera;
-import org.firstinspires.ftc.teamcode.hardware.Robot;
-
-/*
- * Demonstrates an empty iterative OpMode
- */
-@TeleOp(name = "Sandbox", group = "Experimental")
-public class Sandbox extends OpMode {
-
-    private Camera camera;
-
-    @Override
-    public void init() {
-        telemetry.addData("Status", "Initialized");
-        this.camera = new Camera(this.hardwareMap);
-        this.camera.initTargetingCamera();
-    }
-
-    @Override
-    public void loop() {
-
-    }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java
index 50bd0ee..d673664 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java
@@ -1,28 +1,65 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
+import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_LEFT;
+import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_RIGHT;
+import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_LEFT;
+import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_RIGHT;
+
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 public class Drive {
-
     private DcMotor frontLeft;
     private DcMotor frontRight;
     private DcMotor backLeft;
     private DcMotor backRight;
 
     public Drive(HardwareMap hardwareMap) {
-        this.init(hardwareMap);
+        // Drive
+        this.frontLeft = hardwareMap.get(DcMotor.class, WHEEL_FRONT_LEFT);
+        this.frontRight = hardwareMap.get(DcMotor.class, WHEEL_FRONT_RIGHT);
+        this.backLeft = hardwareMap.get(DcMotor.class, WHEEL_BACK_LEFT);
+        this.backRight = hardwareMap.get(DcMotor.class, WHEEL_BACK_RIGHT);
+        this.frontLeft.setDirection(DcMotor.Direction.REVERSE);
+        this.frontRight.setDirection(DcMotor.Direction.FORWARD);
+        this.backLeft.setDirection(DcMotor.Direction.REVERSE);
+        this.backRight.setDirection(DcMotor.Direction.FORWARD);
+        this.frontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        this.frontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        this.backLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        this.backRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
     }
 
-    private void init(HardwareMap hardwareMap) {
-        this.frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
-        this.frontRight = hardwareMap.get(DcMotor.class, "frontRight");
-        this.backLeft = hardwareMap.get(DcMotor.class, "backLeft");
-        this.backRight = hardwareMap.get(DcMotor.class, "backRight");
+    public void setInput(double x, double y, double z) {
+        // instantiate motor power variables
+        double flPower, frPower, blPower, brPower;
+
+        flPower = x + y + z;
+        frPower = -x + y - z;
+        blPower = -x + y + z;
+        brPower = x + y - z;
+
+        double max = Math.max(Math.max(flPower, frPower), Math.max(blPower, brPower));
+        if (max > 1) {
+            flPower /= max;
+            frPower /= max;
+            blPower /= max;
+            brPower /= max;
+        }
+
+        // actually set the motor powers
+        frontLeft.setPower(flPower);
+        frontRight.setPower(frPower);
+        backLeft.setPower(blPower);
+        backRight.setPower(brPower);
     }
 
-    public void setInput(Gamepad gamepad1,Gamepad gamepad2) {
+    public void setInput(Gamepad gamepad1, Gamepad gamepad2) {
+        double x = gamepad1.left_stick_x;
+        double y = -gamepad1.left_stick_y;
+        double z = gamepad1.right_stick_x;
 
+        setInput(x, y, z);
     }
-}
+}
\ No newline at end of file