From 151c866aded754cdf3d83926c0b491da19e2934b Mon Sep 17 00:00:00 2001
From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com>
Date: Sat, 25 Mar 2023 12:08:04 -0500
Subject: [PATCH] Minibot program- day 3

---
 .../minibots/states/Mini2023Bot.java          | 21 +++++++---
 .../minibots/states/Mini2023State.java        | 41 +++++++++++++------
 2 files changed, 44 insertions(+), 18 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023Bot.java b/TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023Bot.java
index 419b279..0aff8ca 100644
--- a/TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023Bot.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023Bot.java
@@ -1,7 +1,10 @@
 package org.timecrafters.minibots.states;
 
+import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.IMU;
 
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 import org.timecrafters.minibots.engines.Mini2023Engine;
@@ -11,8 +14,10 @@ public class Mini2023Bot {
     private final Mini2023Engine engine;
     public TimeCraftersConfiguration configuration;
 
-        public DcMotor Opportunity, Spirit, Victoria, Endeavour; //Don't ask. Just don't.
+        public DcMotor Opportunity, Spirit; //Don't ask. Just don't.
         public CRServo servoA, servoB, servoC; //Just be thankful the servos have 'normal' names.
+        public IMU imu;
+        public ModernRoboticsI2cRangeSensor hazcam;
 
     public Mini2023Bot(Mini2023Engine engine) {
         this.engine = engine;
@@ -21,15 +26,21 @@ public class Mini2023Bot {
 
     private void setupRobot() {
 
-        Opportunity = engine.hardwareMap.get(DcMotor.class, "Left Wheel");
-        Spirit = engine.hardwareMap.get(DcMotor.class, "Right Wheel");
-//        Victoria = engine.hardwareMap.get(DcMotor.class, "Rear Dead Wheel");
-//        Endeavour = engine.hardwareMap.get(DcMotor.class, "Front Dead Wheel");
+        Spirit = engine.hardwareMap.get(DcMotor.class, "Left Wheel");
+        Opportunity = engine.hardwareMap.get(DcMotor.class, "Right Wheel");
 
         servoA = engine.hardwareMap.get(CRServo.class, "Servo 1");
         servoB = engine.hardwareMap.get(CRServo.class, "Servo 2");
         servoC = engine.hardwareMap.get(CRServo.class, "Servo 3");
 
+        IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
+                RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
+                RevHubOrientationOnRobot.UsbFacingDirection.UP
+        ));
+
+        this.imu = engine.hardwareMap.get(IMU.class, "imu");
+        imu.initialize(parameters);
+
 
 
 //        configuration = new TimeCraftersConfiguration("2023 Mini");
diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023State.java b/TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023State.java
index 5e8e09f..a0b785a 100644
--- a/TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023State.java
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023State.java
@@ -9,51 +9,66 @@ public class Mini2023State extends CyberarmState {
 
     public double lThrust, rThrust, orbitSpeed;
 
+    public double deltaServo;
+
     public Mini2023State(Mini2023Bot robot) {this.robot = robot;}
 
+    public double getDeltaServo() {
+
+        return deltaServo;
+    }
+
+    @Override public void telemetry() {
+        engine.telemetry.addData("Right Drive Power", robot.Opportunity.getPower());
+        engine.telemetry.addData("Left Drive Power", robot.Spirit.getPower());
+}
+
 @Override
 public void init() {
     lThrust = 0;
     rThrust = 0;
     orbitSpeed = 0;
-    robot.servoA.setPower(0);
-    robot.servoB.setPower(0);
-    robot.servoC.setPower(0);
-    robot.Opportunity.setPower(lThrust);
-    robot.Spirit.setPower(rThrust);
+    robot.servoA.setPower(orbitSpeed);
+    robot.servoB.setPower(orbitSpeed);
+    robot.servoC.setPower(orbitSpeed);
+    robot.Spirit.setPower(lThrust);
+    robot.Opportunity.setPower(rThrust);
 }
 
 @Override
 public void exec() {
 
     if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
-        lThrust = engine.gamepad2.left_stick_y;
-        robot.Opportunity.setPower(lThrust);
+        lThrust = engine.gamepad1.left_stick_y * 0.5;
+        robot.Spirit.setPower(lThrust);
     } else {
         lThrust = 0;
-        robot.Opportunity.setPower(lThrust);
+        robot.Spirit.setPower(lThrust);
     }
 
     if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) {
-        rThrust = engine.gamepad1.right_stick_y;
-        robot.Spirit.setPower(rThrust);
+        rThrust = engine.gamepad1.right_stick_y * 0.5;
+        robot.Opportunity.setPower(rThrust);
     } else {
         rThrust = 0;
-        robot.Spirit.setPower(rThrust);
+        robot.Opportunity.setPower(rThrust);
     }
 
     if (engine.gamepad1.right_trigger > 0.1) {
-        orbitSpeed = engine.gamepad1.right_trigger * 0.75;
+        orbitSpeed = engine.gamepad1.right_trigger * 0.5;
         robot.servoA.setPower(orbitSpeed);
         robot.servoB.setPower(orbitSpeed);
         robot.servoC.setPower(orbitSpeed);
     } else if (engine.gamepad1.left_trigger > 0.1) {
-        orbitSpeed = engine.gamepad1.left_trigger * 0.75;
+        orbitSpeed = engine.gamepad1.left_trigger * 0.5;
         robot.servoA.setPower(orbitSpeed);
         robot.servoB.setPower(orbitSpeed);
         robot.servoC.setPower(orbitSpeed);
     } else {
         orbitSpeed = 0;
+        robot.servoA.setPower(orbitSpeed);
+        robot.servoB.setPower(orbitSpeed);
+        robot.servoC.setPower(orbitSpeed);
     }