From 022b77fdb7624248097c12dec54804aed9deaa43 Mon Sep 17 00:00:00 2001
From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com>
Date: Tue, 25 Oct 2022 20:29:53 -0500
Subject: [PATCH] autnomous work added

---
 .../Autonomous/States/DriverState.java        | 21 ++++++++++++++++---
 .../Autonomous/States/RotationState.java      |  3 ++-
 .../testing/states/PrototypeBot1.java         |  3 +++
 .../testing/states/PrototypeTeleOPState.java  |  4 ++--
 4 files changed, 25 insertions(+), 6 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java
index 059bd2c..4d997b8 100644
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java
@@ -1,5 +1,7 @@
 package org.timecrafters.Autonomous.States;
 
+import com.qualcomm.robotcore.hardware.DcMotor;
+
 import org.cyberarm.engine.V2.CyberarmState;
 import org.timecrafters.testing.states.PrototypeBot1;
 
@@ -14,13 +16,26 @@ public class DriverState extends CyberarmState {
     private double drivePower;
     private int traveledDistance;
 
+    @Override
+    public void start() {
+        robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+    }
+
     @Override
     public void exec() {
 
-        if (robot.frontRightDrive.getCurrentPosition() < traveledDistance){
-            robot.backLeftDrive.setPower(drivePower * 0.5);
+        if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance){
+            robot.backLeftDrive.setPower(drivePower);
             robot.backRightDrive.setPower(drivePower);
-            robot.frontLeftDrive.setPower(drivePower * 0.5);
+            robot.frontLeftDrive.setPower(drivePower);
             robot.frontRightDrive.setPower(drivePower);
         } else {
             robot.backLeftDrive.setPower(0);
diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java
index b09de56..c3e4dac 100644
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java
@@ -39,7 +39,8 @@ public class RotationState extends CyberarmState {
 
     @Override
     public void telemetry() {
-        engine.telemetry.addData("Robot Rotation", RobotRotation);
+        engine.telemetry.addData("Robot IMU Rotation", RobotRotation);
+        engine.telemetry.addData("Robot Target Rotation", targetRotation);
         engine.telemetry.addData("Drive Power", drivePower);
 
     }
diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java
index a676126..5c3aedf 100644
--- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java
+++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java
@@ -80,6 +80,9 @@ public class PrototypeBot1 {
             collectorLeft = engine.hardwareMap.crservo.get("Collector Left");
             collectorRight = engine.hardwareMap.crservo.get("Collector Right");
 
+            collectorLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+            collectorRight.setDirection(DcMotorSimple.Direction.FORWARD);
+
             // Arm
             LowRiserLeft = engine.hardwareMap.servo.get("LowRiserLeft");
             LowRiserRight = engine.hardwareMap.servo.get("LowRiserRight");
diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
index c680943..b5dbdd9 100644
--- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
+++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
@@ -105,7 +105,7 @@ public class PrototypeTeleOPState extends CyberarmState {
 
         if (engine.gamepad1.right_trigger > 0) {
             drivePower = engine.gamepad1.right_trigger;
-            robot.backLeftDrive.setPower(0.1);
+            robot.backLeftDrive.setPower(drivePower);
             robot.backRightDrive.setPower(drivePower);
             robot.frontLeftDrive.setPower(drivePower);
             robot.frontRightDrive.setPower(drivePower);
@@ -113,7 +113,7 @@ public class PrototypeTeleOPState extends CyberarmState {
 
         if (engine.gamepad1.left_trigger > 0) {
             drivePower = engine.gamepad1.left_trigger;
-            robot.backLeftDrive.setPower(-0.1);
+            robot.backLeftDrive.setPower(-drivePower);
             robot.backRightDrive.setPower(-drivePower);
             robot.frontLeftDrive.setPower(-drivePower);
             robot.frontRightDrive.setPower(-drivePower);