From 03e144fe401d0abf41bc05fdf41a66ddd8b9a635 Mon Sep 17 00:00:00 2001
From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com>
Date: Tue, 26 Sep 2023 19:29:22 -0500
Subject: [PATCH] TeleOp for drivetrain with FTC Lib

---
 .../CenterStage/Common/PrototypeRobot.java    | 43 +++++++++++++++++++
 .../States/PrototypeRobotDrivetrainState.java |  1 +
 2 files changed, 44 insertions(+)

diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
index 0f8bc05..25c46ae 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
@@ -1,14 +1,57 @@
 package org.timecrafters.CenterStage.Common;
 
+import com.arcrobotics.ftclib.drivebase.HDrive;
+import com.arcrobotics.ftclib.hardware.RevIMU;
+import com.arcrobotics.ftclib.hardware.motors.MotorEx;
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
 import org.timecrafters.Library.Robot;
 
+import dev.cyberarm.engine.V2.CyberarmEngine;
+
 public class PrototypeRobot extends Robot {
+
+    private HardwareMap hardwareMap;
+    private MotorEx frontLeft, frontRight, backLeft, backRight;
+    private RevIMU imu;
+
+    private HDrive xDrive;
     private String string;
+    private CyberarmEngine engine;
+
     public PrototypeRobot(String string) {
         this.string = string;
     }
+
     @Override
     public void setup() {
         System.out.println("Bacon: " + this.string);
+        this.hardwareMap = CyberarmEngine.instance.hardwareMap;
+        this.engine = CyberarmEngine.instance;
+
+        //MOTORS
+        frontRight = new MotorEx(hardwareMap, "frontRight");
+        frontLeft = new MotorEx(hardwareMap, "frontLeft");
+        backRight = new MotorEx(hardwareMap, "backRight");
+        backLeft = new MotorEx(hardwareMap, "backLeft");
+
+        //IMU
+        imu = hardwareMap.get(RevIMU.class, "imu");
+        imu.init(new BNO055IMU.Parameters());
+
+        // input motors exactly as shown below
+        xDrive = new HDrive(frontLeft, frontRight,
+                backLeft, backRight);
+
     }
+
+    public void driveTrainTeleOp() {
+        xDrive.driveFieldCentric(engine.gamepad1.left_stick_x, engine.gamepad1.left_stick_y, engine.gamepad1.right_stick_x, heading());
+    }
+
+    public double heading() {
+        return imu.getHeading();
+    }
+
 }
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java
index 50546f2..14c3446 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java
@@ -11,5 +11,6 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
     }
     @Override
     public void exec() {
+        robot.driveTrainTeleOp();
     }
 }