Fixed up tele op and robot object

.idea/misc.xml

@@ -1,6 +1,7 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
-  <component name="ProjectRootManager" version="2" languageLevel="JDK_11" default="true" project-jdk-name="Android Studio default JDK" project-jdk-type="JavaSDK">
+  <component name="ExternalStorageConfigurationManager" enabled="true" />
+  <component name="ProjectRootManager" version="2" languageLevel="JDK_11" default="true" project-jdk-name="11" project-jdk-type="JavaSDK">
     <output url="file://$PROJECT_DIR$/build/classes" />
   </component>
   <component name="ProjectType">

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TensorFlowState.java

@@ -1,5 +1,7 @@
 package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
 
+import android.util.Log;
+
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
@@ -13,8 +15,8 @@ public class TensorFlowState extends CyberarmState {
     Robot robot;
     private List<Recognition> recognitions;
     private double checkTime;
-    private int manualPath;
+    private int manualPath = 0;
-    private int path = 0;
+    private int path = 3;
     private double leftDuck;
     private double middleDuck;
 
@@ -28,6 +30,7 @@ public class TensorFlowState extends CyberarmState {
         this.armExtension = armExtension;
         this.leftDuck = robot.configuration.variable(groupName, actionName, "leftDuck").value();
         this.middleDuck = robot.configuration.variable(groupName, actionName, "middleDuck").value();
+        this.checkTime = robot.configuration.variable(groupName, actionName, "time").value();
     }
 
     @Override
@@ -41,23 +44,22 @@ public class TensorFlowState extends CyberarmState {
         recognitions = robot.tensorflowDetections();
 
         if (runTime() < checkTime) {
-            if (manualPath != -1) {
+            if (recognitions != null && recognitions.size() != 0) {
+                if (recognitions.size() == 1) {
+                    Recognition recognition = recognitions.get(0);
 
-                if (recognitions != null) {
+                    if (recognition.getLeft() < leftDuck) {
-                    if (recognitions.size() == 1) {
+                        path = 0;
-                        Recognition recognition = recognitions.get(0);
-
-                        if (recognition.getLeft() < leftDuck) {
-                            path = 0;
-                        } else {
-                            path = 1;
-                        }
                     } else {
-                        path = 2;
+                        path = 1;
                     }
                 }
+            } else {
+                path = 2;
             }
         } else {
+            Log.i(TAG, "Choosen path: " + path);
+
             if (path == 0){
                 addState(new TurretArmExtension(robot, armExtension, groupName, "02_0"));
                 addState(new TurretArmRiser(robot, armRiser, groupName, "03_0_bottom"));
@@ -72,17 +74,27 @@ public class TensorFlowState extends CyberarmState {
                 addState(new TurretArmExtension(robot, armExtension, groupName, "04_0_top"));
             }
 
-          setHasFinished(true);
+            setHasFinished(true);
         }
     }
 
     @Override
     public void telemetry() {
-        for (Recognition recognition : robot.tensorflowDetections()) {
+        engine.telemetry.addData("Runtime", runTime());
+        engine.telemetry.addData("Check Time", checkTime);
+        engine.telemetry.addData("Path", path);
+        engine.telemetry.addLine();
+
+        if (recognitions == null) {
+            return;
+        }
+
+        for (Recognition recognition : recognitions) {
             engine.telemetry.addData("Label", recognition.getLabel());
             engine.telemetry.addData("Left", recognition.getLeft());
             engine.telemetry.addData("Top", recognition.getTop());
             engine.telemetry.addData("Confidence", recognition.getConfidence());
+            engine.telemetry.addLine();
         }
     }
 }

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java

@@ -28,7 +28,6 @@ public class TeleOpState extends CyberarmState {
     @Override
     public void start() {
         super.start();
-        // FIXME: Don't reset when doing autonomous stuff
         robot.whiteArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         robot.whiteArmBobbin.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.orangeArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
@@ -47,7 +46,7 @@ public class TeleOpState extends CyberarmState {
         robot.driveGoalRight.setPower(-engine.gamepad1.right_stick_y * maxDriveSpeed);
 
         // dispenser powered
-        if (engine.gamepad1.b){
+        if (engine.gamepad1.a){
             robot.orangeDispenser.setPosition(0);
         }
         // if not pressed dispenser off
@@ -74,11 +73,11 @@ public class TeleOpState extends CyberarmState {
         if (engine.gamepad1.dpad_up || engine.gamepad1.dpad_down) {
 
             if (engine.gamepad1.dpad_up) {
-                robot.orangeArmRiser.setPower(0.8);
+                robot.orangeArmRiser.setPower(1);
             }
 
             if (engine.gamepad1.dpad_down) {
-                robot.orangeArmRiser.setPower(-0.5);
+                robot.orangeArmRiser.setPower(-1);
             }
         }
         // nothing pressed nothing move...
@@ -86,14 +85,14 @@ public class TeleOpState extends CyberarmState {
             robot.orangeArmRiser.setPower(0);
         }
 
-        // button x pressed carousel wheel move.
+        // button x pressed carousel wheel move, for blue side, if b pressed wheel spins for red side;
 
-        if (engine.gamepad2.x){
+        if (engine.gamepad2.b){
-            robot.carouselWheel.setPower(0.5);
-        }
-        else if (engine.gamepad2.a){
             robot.carouselWheel.setPower(1);
         }
+        else if (engine.gamepad2.x){
+            robot.carouselWheel.setPower(-1);
+        }
         else {
             robot.carouselWheel.setPower(0);
         }
@@ -116,7 +115,7 @@ public class TeleOpState extends CyberarmState {
 
 
         // if b is pressed then depositor door opens, if not pressed not opening.
-        if (engine.gamepad2.b){
+        if (engine.gamepad2.a){
             robot.whiteDispenser.setPosition(.5);
         }
 
@@ -130,11 +129,11 @@ public class TeleOpState extends CyberarmState {
         if (engine.gamepad2.dpad_down || engine.gamepad2.dpad_up) {
 
             if (engine.gamepad2.dpad_up) {
-                robot.whiteArmRiser.setPower(8);
+                robot.whiteArmRiser.setPower(1);
             }
 
             if (engine.gamepad2.dpad_down) {
-                robot.whiteArmRiser.setPower(-0.5);
+                robot.whiteArmRiser.setPower(-1);
             }
         }
         // nothing pressed nothing move...

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/MecanumRobot.java

@@ -0,0 +1,37 @@
+package org.timecrafters.FreightFrenzy.MecanumRobot_Spencer;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+public class MecanumRobot {
+
+    private final CyberarmEngine engine;
+
+    // Drivetrain
+
+    public DcMotor driveFrontLeft, driveFrontRight, driveBackLeft, driveBackRight;
+
+
+
+    public MecanumRobot(CyberarmEngine engine) {
+        this.engine = engine;
+        initDrivetrain();
+    }
+    private void initDrivetrain() {
+
+        driveFrontRight = engine.hardwareMap.dcMotor.get("driveFrontRight");
+        driveFrontLeft  = engine.hardwareMap.dcMotor.get("driveFrontLeft");
+        driveBackRight  = engine.hardwareMap.dcMotor.get("driveBackRight");
+        driveBackLeft   = engine.hardwareMap.dcMotor.get("driveBackLeft");
+
+        driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+        driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+        driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
+    }
+}
+
+

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/Mecanum_Drive_Engine.java

@@ -0,0 +1,13 @@
+package org.timecrafters.FreightFrenzy.MecanumRobot_Spencer;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+
+@TeleOp(name = "TeleOp Mecanum Robot")
+
+public class Mecanum_Drive_Engine extends CyberarmEngine {
+    @Override
+    public void setup() { addState(new TeleOpStateMecanumRobot(new MecanumRobot(this)));}
+
+    }

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/MecanumRobot_Spencer/TeleOpStateMecanumRobot.java

@@ -0,0 +1,117 @@
+package org.timecrafters.FreightFrenzy.MecanumRobot_Spencer;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.MecanumRobot_Spencer.MecanumRobot;
+
+public class TeleOpStateMecanumRobot extends CyberarmState {
+
+    MecanumRobot robot;
+
+    public TeleOpStateMecanumRobot(MecanumRobot mecanumRobot) {
+
+    }
+
+    @Override
+    public void exec() {
+
+        // Slow Mode
+
+        if (engine.gamepad1.left_trigger > 0.5){
+        // TankDrive
+        // Left joystick forward; Left side forward;
+
+        robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 0.5);
+        robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 0.5);
+
+        // Right joystick forward; Right side forward;
+        robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 0.5);
+        robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 0.5);
+
+
+        // Strafing left and right
+
+        if (engine.gamepad1.left_bumper) {
+            robot.driveFrontLeft.setPower(0.5);
+            robot.driveBackLeft.setPower(-0.5);
+            robot.driveFrontRight.setPower(-0.5);
+            robot.driveBackRight.setPower(0.5);
+        } else if (engine.gamepad1.right_bumper) {
+            robot.driveFrontLeft.setPower(-0.5);
+            robot.driveBackLeft.setPower(0.5);
+            robot.driveFrontRight.setPower(0.5);
+            robot.driveBackRight.setPower(-0.5);
+    }
+//        else {
+//            robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 0.5);
+//            robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 0.5);
+//            robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 0.5);
+//            robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 0.5);
+
+
+       // }
+    // Normal Speed
+
+        if (engine.gamepad1.right_stick_button) {
+
+            // left stick forward; right side foward
+            robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1);
+            robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1);
+
+            // right stick foward; right side foward;
+            robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1);
+            robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1);
+
+        }
+            // Strafing left and right
+
+            if (engine.gamepad1.left_bumper) {
+                robot.driveFrontLeft.setPower(1);
+                robot.driveBackLeft.setPower(-1);
+                robot.driveFrontRight.setPower(-1);
+                robot.driveBackRight.setPower(1);
+            } else if (engine.gamepad1.right_bumper) {
+                robot.driveFrontLeft.setPower(-1);
+                robot.driveBackLeft.setPower(1);
+                robot.driveFrontRight.setPower(1);
+                robot.driveBackRight.setPower(-1);
+            } else {
+                robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1);
+                robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1);
+                robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1);
+                robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1);
+
+            }
+        }
+
+        else {
+            // left stick foward; right side foward
+            robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1);
+            robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1);
+
+            // right stick foward; right side foward;
+            robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1);
+            robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1);
+
+
+            // Strafing left and right
+
+//            if (engine.gamepad1.left_bumper) {
+//                robot.driveFrontLeft.setPower(1);
+//                robot.driveBackLeft.setPower(-1);
+//                robot.driveFrontRight.setPower(-1);
+//                robot.driveBackRight.setPower(1);
+//            } else if (engine.gamepad1.right_bumper) {
+//                robot.driveFrontLeft.setPower(-1);
+//                robot.driveBackLeft.setPower(1);
+//                robot.driveFrontRight.setPower(1);
+//                robot.driveBackRight.setPower(-1);
+//            } else {
+//                robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1);
+//                robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1);
+//                robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1);
+//                robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1);
+//
+//            }
+        }
+    }
+}