Updated Vexy/Yellow minibot (disabled code in autonomous drive states)

TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java

@@ -3,8 +3,14 @@ package dev.cyberarm.minibots.yellow;
 import com.arcrobotics.ftclib.hardware.motors.MotorEx;
 import com.arcrobotics.ftclib.hardware.motors.MotorGroup;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.hardware.rev.RevTouchSensor;
 import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.TouchSensor;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
@@ -14,46 +20,88 @@ import dev.cyberarm.engine.V2.CyberarmEngine;
 
 public class YellowMinibot {
     private final CyberarmEngine engine;
-    public final TimeCraftersConfiguration config;
-    public MotorEx leftFront, rightFront, leftBack, rightBack;
-    public MotorGroup left, right;
-    public IMU imu;
-    public CRServo droneLauncher;
-    public final double wheelCircumference, distancePerTick, imuAngleOffset, initialFacing;
+    public TimeCraftersConfiguration config;
+    public final DcMotorEx backLeft, frontLeft, frontRight, backRight;
+    public final IMU imu;
+    public final Servo leftClaw, rightClaw, droneLatch;
+    public final CRServo arm;
+    public final TouchSensor armEndStopLeft, armEndStopRight;
+
+    public double DRIVETRAIN_WHEEL_DIAMETER_MM = 90.0;
+    public int    DRIVETRAIN_MOTOR_TICKS = 4;
+    public double DRIVETRAIN_MOTOR_GEAR_RATIO = 72.0;
+    public double DRIVETRAIN_MAX_VELOCITY_MM = 305.0;
+    public double LEFT_CLAW_CLOSED_POSITION = 0.0;
+    public double LEFT_CLAW_OPEN_POSITION = 0.0;
+    public double RIGHT_CLAW_CLOSED_POSITION = 0.0;
+    public double RIGHT_CLAW_OPEN_POSITION = 0.0;
+    public double DRONE_LATCH_INITIAL_POSITION = 0.0;
+    public double DRONE_LATCH_LAUNCH_POSITION = 0.0;
+
     public YellowMinibot(CyberarmEngine engine) {
         this.engine = engine;
 
-        this.config = new TimeCraftersConfiguration("Vexy_2023");
+        reloadConfig();
 
-        leftFront = new MotorEx(engine.hardwareMap, "leftFront");
-        rightFront = new MotorEx(engine.hardwareMap, "rightFront");
+        backLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("backLeft");
+        frontLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("frontLeft");
+        frontRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("frontRight");
+        backRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("backRight");
 
-        leftBack = new MotorEx(engine.hardwareMap, "leftBack");
-        rightBack = new MotorEx(engine.hardwareMap, "rightBack");
+        backLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+        frontLeft.setDirection(DcMotorSimple.Direction.FORWARD);
+        frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
+        backRight.setDirection(DcMotorSimple.Direction.FORWARD);
 
-        wheelCircumference = Math.PI * (50.0 * 2); //wheelRadius * wheelRadius;
-        distancePerTick = 288 / wheelCircumference; // raw motor encoder * gear ratio
+        backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
 
-        leftFront.setDistancePerPulse(distancePerTick);
-        rightFront.setDistancePerPulse(distancePerTick);
-        leftBack.setDistancePerPulse(distancePerTick);
-        rightBack.setDistancePerPulse(distancePerTick);
+        backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
-        left = new MotorGroup(leftFront, leftBack);
-        right = new MotorGroup(rightFront, rightBack);
+        leftClaw = engine.hardwareMap.servo.get("leftClaw"); /// Port 0
+        rightClaw = engine.hardwareMap.servo.get("rightClaw"); /// Port 1
+        arm = engine.hardwareMap.crservo.get("arm"); /// Port 2
+        arm.setDirection(DcMotorSimple.Direction.REVERSE);
+        droneLatch = engine.hardwareMap.servo.get("droneLatch"); /// Port 3
 
-        imu = engine.hardwareMap.get(IMU.class, "imu");
+        armEndStopLeft = engine.hardwareMap.touchSensor.get("armEndStopLeft");
+        armEndStopRight = engine.hardwareMap.touchSensor.get("armEndStopRight");
+
+        imu = engine.hardwareMap.get(IMU.class, "imu"); // Embedded | IC2 port 0
         IMU.Parameters parameters = new IMU.Parameters(
                 new RevHubOrientationOnRobot(
                         RevHubOrientationOnRobot.LogoFacingDirection.UP,
                         RevHubOrientationOnRobot.UsbFacingDirection.RIGHT));
 
         imu.initialize(parameters);
+    }
 
-        imuAngleOffset = 0;
-        initialFacing = facing();
+    public void reloadConfig() {
+        this.config = new TimeCraftersConfiguration("cyberarm_Vexy_Yellow");
 
-        droneLauncher = engine.hardwareMap.crservo.get("droneLauncher"); /// Port 5
+        loadConstants();
+    }
+
+    public void loadConstants() {
+        DRIVETRAIN_WHEEL_DIAMETER_MM = config.variable("Robot", "Drivetrain_Tuning", "wheel_diameter_mm").value();
+        DRIVETRAIN_MOTOR_TICKS = config.variable("Robot", "Drivetrain_Tuning", "motor_ticks").value();
+        DRIVETRAIN_MOTOR_GEAR_RATIO = config.variable("Robot", "Drivetrain_Tuning", "motor_gear_ratio").value();
+        DRIVETRAIN_MAX_VELOCITY_MM = config.variable("Robot", "Drivetrain_Tuning", "max_velocity_mm").value();
+
+        LEFT_CLAW_CLOSED_POSITION = config.variable("Robot", "LeftClaw_Tuning", "open_position").value();
+        LEFT_CLAW_OPEN_POSITION = config.variable("Robot", "LeftClaw_Tuning", "closed_position").value();
+
+
+        RIGHT_CLAW_OPEN_POSITION = config.variable("Robot", "RightClaw_Tuning", "open_position").value();
+        RIGHT_CLAW_CLOSED_POSITION = config.variable("Robot", "RightClaw_Tuning", "closed_position").value();
+
+        DRONE_LATCH_INITIAL_POSITION = config.variable("Robot", "DroneLatch_Tuning", "initial_position").value();
+        DRONE_LATCH_LAUNCH_POSITION = config.variable("Robot", "DroneLatch_Tuning", "launch_position").value();
     }
 
     public void standardTelemetry() {
@@ -61,35 +109,45 @@ public class YellowMinibot {
 
         engine.telemetry.addLine("Motors");
         engine.telemetry.addData(
-                "Left Front",
+                "Front Left",
                 "Power: %.2f, Current: %.2f mAmp, Position: %d",
-                leftFront.motorEx.getPower(),
-                leftFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
-                leftFront.motorEx.getCurrentPosition());
+                frontLeft.getPower(),
+                frontLeft.getCurrent(CurrentUnit.MILLIAMPS),
+                frontLeft.getCurrentPosition());
         engine.telemetry.addData(
-                "Right Front",
+                "Front Right",
                 "Power: %.2f, Current: %.2f mAmp, Position: %d",
-                rightFront.motorEx.getPower(),
-                rightFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
-                rightFront.motorEx.getCurrentPosition());
+                frontRight.getPower(),
+                frontRight.getCurrent(CurrentUnit.MILLIAMPS),
+                frontRight.getCurrentPosition());
         engine.telemetry.addData(
-                "Left Back",
+                "Back Left",
                 "Power: %.2f, Current: %.2f mAmp, Position: %d",
-                leftBack.motorEx.getPower(),
-                leftBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
-                leftBack.motorEx.getCurrentPosition());
+                backLeft.getPower(),
+                backLeft.getCurrent(CurrentUnit.MILLIAMPS),
+                backLeft.getCurrentPosition());
         engine.telemetry.addData(
-                "Right Back",
+                "Back Right",
                 "Power: %.2f, Current: %.2f mAmp, Position: %d",
-                rightBack.motorEx.getPower(),
-                rightBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
-                rightBack.motorEx.getCurrentPosition());
+                backRight.getPower(),
+                backRight.getCurrent(CurrentUnit.MILLIAMPS),
+                backRight.getCurrentPosition());
 
         engine.telemetry.addLine();
+
         engine.telemetry.addLine("Servos");
-        engine.telemetry.addData("droneLauncher", droneLauncher.getPower());
+        engine.telemetry.addData("left claw", leftClaw.getPosition());
+        engine.telemetry.addData("right claw", rightClaw.getPosition());
+        engine.telemetry.addData("arm", arm.getPower());
+        engine.telemetry.addData("droneLatch", droneLatch.getPosition());
 
         engine.telemetry.addLine();
+
+        engine.telemetry.addData("arm endstop left", armEndStopLeft.isPressed());
+        engine.telemetry.addData("arm endstop right", armEndStopRight.isPressed());
+
+        engine.telemetry.addLine();
+
     }
 
     public void teleopTelemetry() {
@@ -97,46 +155,4 @@ public class YellowMinibot {
 
         engine.telemetry.addLine();
     }
-
-    public double distanceMM(int ticks) {
-        return distancePerTick * ticks;
-    }
-
-
-    public double initialFacing() {
-        return initialFacing;
-    }
-
-    public double facing() {
-        double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
-
-        return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0;
-    }
-
-    public double heading() {
-        return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0);
-//        return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
-    }
-
-    public double turnRate() {
-        return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED
-    }
-
-    public boolean isBetween(double value, double min, double max) {
-        return value >= min && value <= max;
-    }
-
-    // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38
-    public double angleDiff(double from, double to) {
-        double value = (to - from + 180);
-
-        double fmod = (value - 0.0) % (360.0 - 0.0);
-
-        return (fmod < 0 ? fmod + 360.0 : fmod +  0.0) - 180;
-    }
-
-    public double lerp(double min, double max, double t)
-    {
-        return min + (max - min) * t;
-    }
-}
+}

TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java

@@ -1,40 +1,43 @@
 package dev.cyberarm.minibots.yellow.states;
 
+import com.qualcomm.robotcore.hardware.Gamepad;
+
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 
 import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.engine.V2.Utilities;
 import dev.cyberarm.minibots.yellow.YellowMinibot;
 
 public class Pilot extends CyberarmState {
     final YellowMinibot robot;
+    private boolean leftClawOpen, rightClawOpen;
 
     public Pilot(YellowMinibot robot) {
         this.robot = robot;
+
+        this.leftClawOpen = false;
+        this.rightClawOpen = false;
     }
     @Override
     public void exec() {
-        /// --- IMU Reset --- ///
-        if (engine.gamepad1.guide) {
-            robot.imu.resetYaw();
-        }
+        drivetrain();
+        armController();
+        clawControllers();
+        droneLatchController();
+    }
 
-        /// --- DRONE --- ///
-        if (engine.gamepad1.y) {
-            robot.droneLauncher.setPower(1.0);
-        } else if (engine.gamepad1.a) {
-            robot.droneLauncher.setPower(0.0);
-        }
-
-        /// --- DRIVE --- ///
+    private void drivetrain()
+    {
 //        robot.left.set(engine.gamepad1.left_stick_y);
 //        robot.right.set(engine.gamepad1.right_stick_y);
 
 
         // https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html#field-centric
 
-        double y = engine.gamepad1.left_stick_y; // Remember, Y stick value is reversed;
-        double x = -engine.gamepad1.left_stick_x;
-        double rx = -engine.gamepad1.right_stick_x;
+        double y = -engine.gamepad1.left_stick_y; // Remember, Y stick value is reversed;
+        double x = engine.gamepad1.left_stick_x;
+        double rx = engine.gamepad1.right_stick_x;
 
         double botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
 
@@ -53,16 +56,72 @@ public class Pilot extends CyberarmState {
         double frontRightPower = (rotY - rotX - rx) / denominator;
         double backRightPower = (rotY + rotX - rx) / denominator;
 
-//        frontLeftPower *= 24.0;
-//        backLeftPower *= 24.0;
-//        frontRightPower *= 24.0;
-//        backRightPower *= 24.0;
+        double velocity = Utilities.unitToTicks(
+                robot.DRIVETRAIN_MOTOR_TICKS,
+                robot.DRIVETRAIN_MOTOR_GEAR_RATIO,
+                robot.DRIVETRAIN_WHEEL_DIAMETER_MM,
+                DistanceUnit.MM,
+                robot.DRIVETRAIN_MAX_VELOCITY_MM);
 
-        double maxPower = 1.0;
+        robot.backLeft.setVelocity(frontLeftPower * velocity);
+        robot.frontLeft.setVelocity(backLeftPower * velocity);
+        robot.frontRight.setVelocity(frontRightPower * velocity);
+        robot.backRight.setVelocity(backRightPower * velocity);
+    }
 
-        robot.leftFront.motorEx.setPower(frontLeftPower * maxPower);
-        robot.leftBack.motorEx.setPower(backLeftPower * maxPower);
-        robot.rightFront.motorEx.setPower(frontRightPower * maxPower);
-        robot.rightBack.motorEx.setPower(backRightPower * maxPower);
+    private void armController()
+    {
+        double armPower = engine.gamepad1.right_trigger - engine.gamepad1.left_trigger;
+        if (armPower > 0) {
+            if (robot.armEndStopLeft.isPressed() || robot.armEndStopRight.isPressed())
+                armPower = 0;
+        }
+
+        robot.arm.setPower(armPower);
+    }
+
+    private void clawControllers()
+    {
+        robot.leftClaw.setPosition(leftClawOpen ? robot.LEFT_CLAW_OPEN_POSITION : robot.LEFT_CLAW_CLOSED_POSITION);
+        robot.rightClaw.setPosition(rightClawOpen ? robot.RIGHT_CLAW_OPEN_POSITION : robot.RIGHT_CLAW_CLOSED_POSITION);
+    }
+
+    private void droneLatchController()
+    {}
+
+    @Override
+    public void telemetry() {
+        robot.standardTelemetry();
+        robot.teleopTelemetry();
+    }
+
+    @Override
+    public void buttonDown(Gamepad gamepad, String button) {
+        if (gamepad == engine.gamepad1)
+        {
+            switch (button) {
+                case "guide":
+                    /// --- IMU Reset --- ///
+                    robot.imu.resetYaw();
+                    break;
+                case "start":
+                    robot.reloadConfig();
+                    break;
+                case "left_bumper":
+                    leftClawOpen = !leftClawOpen;
+                    break;
+                case "right_bumper":
+                    rightClawOpen = !rightClawOpen;
+                    break;
+                case "y":
+                    /// DRONE LATCH ///
+                    robot.droneLatch.setPosition(robot.DRONE_LATCH_LAUNCH_POSITION);
+                    break;
+                case "a":
+                    /// DRONE LATCH ///
+                    robot.droneLatch.setPosition(robot.DRONE_LATCH_INITIAL_POSITION);
+                    break;
+            }
+        }
     }
 }

TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Rotate.java

@@ -47,40 +47,40 @@ public class Rotate extends CyberarmState {
 
     @Override
     public void exec() {
-        if (Math.abs(robot.angleDiff(robot.facing(), heading)) <= tolerance) {
-            robot.left.set(0);
-            robot.right.set(0);
-
-            setHasFinished(true);
-
-            return;
-        }
-
-        if (runTime() >= timeoutMS) {
-            robot.left.set(0);
-            robot.right.set(0);
-
-            setHasFinished(true);
-        }
-
-        double angleDiff = robot.angleDiff(robot.facing() + robot.turnRate(), heading);
-        double velocity = robot.lerp(minVelocity, maxVelocity, Range.clip(Math.abs(angleDiff) / 90.0, 0.0, 1.0));
-
-        if (angleDiff > 0) {
-            robot.left.set(-velocity);
-            robot.right.set(velocity);
-        } else {
-            robot.left.set(velocity);
-            robot.right.set(-velocity);
-        }
+//        if (Math.abs(robot.angleDiff(robot.facing(), heading)) <= tolerance) {
+//            robot.left.set(0);
+//            robot.right.set(0);
+//
+//            setHasFinished(true);
+//
+//            return;
+//        }
+//
+//        if (runTime() >= timeoutMS) {
+//            robot.left.set(0);
+//            robot.right.set(0);
+//
+//            setHasFinished(true);
+//        }
+//
+//        double angleDiff = robot.angleDiff(robot.facing() + robot.turnRate(), heading);
+//        double velocity = robot.lerp(minVelocity, maxVelocity, Range.clip(Math.abs(angleDiff) / 90.0, 0.0, 1.0));
+//
+//        if (angleDiff > 0) {
+//            robot.left.set(-velocity);
+//            robot.right.set(velocity);
+//        } else {
+//            robot.left.set(velocity);
+//            robot.right.set(-velocity);
+//        }
     }
 
     @Override
     public void telemetry() {
-        engine.telemetry.addLine();
-        engine.telemetry.addData("Robot Heading", robot.facing());
-        engine.telemetry.addData("Robot Target Heading", heading);
-        engine.telemetry.addData("Robot Angle Diff", robot.angleDiff(robot.facing() + robot.turnRate(), heading));
-        engine.telemetry.addData("Robot Turn Rate", robot.turnRate());
+//        engine.telemetry.addLine();
+//        engine.telemetry.addData("Robot Heading", robot.facing());
+//        engine.telemetry.addData("Robot Target Heading", heading);
+//        engine.telemetry.addData("Robot Angle Diff", robot.angleDiff(robot.facing() + robot.turnRate(), heading));
+//        engine.telemetry.addData("Robot Turn Rate", robot.turnRate());
     }
 }

TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/StrafeMove.java

@@ -38,26 +38,26 @@ public class StrafeMove extends CyberarmState {
 
     @Override
     public void start() {
-        robot.leftFront.setTargetDistance(distanceMM);
-
-        robot.left.setPositionTolerance(tolerance);
-        robot.right.setPositionTolerance(tolerance);
-
-        double motorVelocity = (distanceMM < 0 ? velocity * -1 : velocity);
-
-        robot.leftFront.set(motorVelocity);
-        robot.rightFront.set(-motorVelocity);
-
-        robot.leftBack.set(-motorVelocity);
-        robot.rightBack.set(motorVelocity);
+//        robot.leftFront.setTargetDistance(distanceMM);
+//
+//        robot.left.setPositionTolerance(tolerance);
+//        robot.right.setPositionTolerance(tolerance);
+//
+//        double motorVelocity = (distanceMM < 0 ? velocity * -1 : velocity);
+//
+//        robot.leftFront.set(motorVelocity);
+//        robot.rightFront.set(-motorVelocity);
+//
+//        robot.leftBack.set(-motorVelocity);
+//        robot.rightBack.set(motorVelocity);
     }
 
     @Override
     public void exec() {
-        if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(distanceMM) ||  runTime() >= timeoutMS) {
-            robot.left.set(0);
-            robot.right.set(0);
-            setHasFinished(true);
-        }
+//        if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(distanceMM) ||  runTime() >= timeoutMS) {
+//            robot.left.set(0);
+//            robot.right.set(0);
+//            setHasFinished(true);
+//        }
     }
 }

TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/TankMove.java

@@ -43,51 +43,51 @@ public class TankMove extends CyberarmState {
 
     @Override
     public void start() {
-        robot.left.resetEncoder();
-        robot.right.resetEncoder();
-
-        robot.left.setTargetDistance(leftDistanceMM);
-        robot.right.setTargetDistance(rightDistanceMM);
-
-        robot.left.setPositionTolerance(tolerance);
-        robot.right.setPositionTolerance(tolerance);
-
-        robot.left.set(leftVelocity);
-        robot.right.set(rightVelocity);
+//        robot.left.resetEncoder();
+//        robot.right.resetEncoder();
+//
+//        robot.left.setTargetDistance(leftDistanceMM);
+//        robot.right.setTargetDistance(rightDistanceMM);
+//
+//        robot.left.setPositionTolerance(tolerance);
+//        robot.right.setPositionTolerance(tolerance);
+//
+//        robot.left.set(leftVelocity);
+//        robot.right.set(rightVelocity);
     }
 
     @Override
     public void exec() {
-        if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM)) {
-            robot.left.set(0);
-        }
-        if (robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) {
-            robot.right.set(0);
-        }
-
-        if (
-                (robot.left.atTargetPosition() && robot.right.atTargetPosition()) ||
-                (Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM) && robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM))  ||
-                runTime() >= timeoutMS) {
-            robot.left.set(0);
-            robot.right.set(0);
-            setHasFinished(true);
-        }
+//        if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM)) {
+//            robot.left.set(0);
+//        }
+//        if (robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) {
+//            robot.right.set(0);
+//        }
+//
+//        if (
+//                (robot.left.atTargetPosition() && robot.right.atTargetPosition()) ||
+//                (Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM) && robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM))  ||
+//                runTime() >= timeoutMS) {
+//            robot.left.set(0);
+//            robot.right.set(0);
+//            setHasFinished(true);
+//        }
     }
 
     @Override
     public void telemetry() {
-        engine.telemetry.addLine();
-
-        engine.telemetry.addData("Left distance", robot.leftFront.getDistance());
-        engine.telemetry.addData("Left position", robot.left.getPositions().get(0));
-        engine.telemetry.addData("Left speed", robot.left.getSpeeds().get(0));
-        engine.telemetry.addData("Left velocity", robot.left.getVelocity());
-        engine.telemetry.addLine();
-
-        engine.telemetry.addData("Right distance", robot.rightFront.getDistance());
-        engine.telemetry.addData("Right position", robot.right.getPositions().get(0));
-        engine.telemetry.addData("Right speed", robot.right.getSpeeds().get(0));
-        engine.telemetry.addData("Right velocity", robot.right.getVelocity());
+//        engine.telemetry.addLine();
+//
+//        engine.telemetry.addData("Left distance", robot.leftFront.getDistance());
+//        engine.telemetry.addData("Left position", robot.left.getPositions().get(0));
+//        engine.telemetry.addData("Left speed", robot.left.getSpeeds().get(0));
+//        engine.telemetry.addData("Left velocity", robot.left.getVelocity());
+//        engine.telemetry.addLine();
+//
+//        engine.telemetry.addData("Right distance", robot.rightFront.getDistance());
+//        engine.telemetry.addData("Right position", robot.right.getPositions().get(0));
+//        engine.telemetry.addData("Right speed", robot.right.getSpeeds().get(0));
+//        engine.telemetry.addData("Right velocity", robot.right.getVelocity());
     }
 }

TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java

@@ -19,7 +19,8 @@ public class SodiPizzaMinibotObject extends Robot {
 
     public HardwareMap hardwareMap;
     public DcMotor leftFront, rightFront, leftBack, rightBack;
-    public Servo shoulder, gripper, launcher;
+//    public Servo shoulder, gripper;
+    public Servo launcher;
     public IMU imu;
     public Rev2mDistanceSensor distSensor;
     private String string;
@@ -70,8 +71,8 @@ public class SodiPizzaMinibotObject extends Robot {
         rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
         //Servo Defining
-        shoulder = engine.hardwareMap.servo.get("arm");
-        gripper = engine.hardwareMap.servo.get("gripper");
+//        shoulder = engine.hardwareMap.servo.get("arm");
+//        gripper = engine.hardwareMap.servo.get("gripper");
         launcher = engine.hardwareMap.servo.get("launcher");
 
         //Distance Sensor

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java

@@ -26,7 +26,9 @@ public class SodiPizzaTeleOPState extends CyberarmState {
     public final double minInput = 0.1 /* <- Minimum input from stick to send command */;
     public double lastToldAngle /* <- The angle the bot was last told to stop at */, proportion, integral, derivative;
     public float approxObjPos /* <- Approximate distance away the nearest obstruction is */, objData1, objData2, objData3;
-    private int armPos, objectPos;
+    private double lfPower, rfPower, lbPower, rbPower;
+    private float yTransitPercent, xTransitPercent, rotPercent, percentDenom;
+    private int objectPos;
     private boolean droneLaunched;
     private char buttonPressed;
     private GamepadChecker gp1checker, gp2checker;
@@ -48,12 +50,6 @@ public class SodiPizzaTeleOPState extends CyberarmState {
 
     @Override
     public void telemetry() {
-        engine.telemetry.addData("Arm should be at Position ", armPos);
-        engine.telemetry.addData("Arm servo is at ", robot.shoulder.getPosition());
-
-        engine.telemetry.addData("Button Pressed = ", buttonPressed);
-
-        engine.telemetry.addLine();
         engine.telemetry.addData("Launcher Servo: ", robot.launcher.getPosition());
         engine.telemetry.addData("Drone Launched?", droneLaunched);
     }
@@ -77,15 +73,18 @@ public class SodiPizzaTeleOPState extends CyberarmState {
         gp2checker = new GamepadChecker(engine, engine.gamepad2);
 
         lastMoveTime = System.currentTimeMillis();
-        armPos = 0;
+        lastDistRead = System.currentTimeMillis();
     }
 
     @Override
     public void exec() {
+        
+        
+        
 
         if (Math.abs(engine.gamepad1.left_stick_y) < minInput &&
                 Math.abs(engine.gamepad1.left_stick_x) < minInput &&
-                Math.abs(engine.gamepad1.right_stick_x) < minInput) /* <- input from ONLY left stick y means to move forward or backward */{
+                Math.abs(engine.gamepad1.right_stick_x) < minInput){
 
             drivePower = 0;
             robot.leftFront.setPower(drivePower);
@@ -94,69 +93,37 @@ public class SodiPizzaTeleOPState extends CyberarmState {
             robot.rightBack.setPower(drivePower);
         }
 
-        if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > lastToldAngle + 0.5) {
-            if (Math.abs(engine.gamepad1.right_stick_x) > minInput &&
-            Math.abs(engine.gamepad1.left_stick_y) > minInput) {
-                robot.rightFront.setPower(robot.leftFront.getPower() * 0.8);
-                robot.rightBack.setPower(robot.leftBack.getPower() * 0.8);
-            }
-
-        } else if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle - 0.5 &&
-                Math.abs(engine.gamepad1.left_stick_y) > minInput) {
-
-            robot.leftFront.setPower(robot.rightFront.getPower() * 0.8);
-            robot.leftBack.setPower(robot.rightBack.getPower() * 0.8);
+        if (Math.abs(yTransitPercent) > 0.01) {
 
+            percentDenom = 100;
         } else {
-
-            robot.leftFront.setPower(drivePower);
-            robot.rightFront.setPower(drivePower);
-            robot.leftBack.setPower(drivePower);
-            robot.rightBack.setPower(drivePower);
+            percentDenom = 0;
         }
 
-        if (engine.gamepad1.start && !engine.gamepad1.a) /*<-- reset everything: encoders, imu, and armPos int*/ {
+        if (Math.abs(xTransitPercent) > 0.01) {
 
-            robot.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-            robot.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-            robot.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-            robot.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-            robot.imu.resetYaw();
+            percentDenom = percentDenom + 100;
         }
 
-        if (Math.abs(engine.gamepad1.left_stick_y) > minInput &&
-            Math.abs(engine.gamepad1.left_stick_x) < minInput &&
-            armPos == 0/* &&
-                robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > lastToldAngle - 0.5 &&
-                robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle + 0.5*/) {
+        if (Math.abs(rotPercent) > 0.01) {
 
-            drivePower = engine.gamepad1.left_stick_y;
-            robot.leftFront.setPower(drivePower);
-            robot.rightFront.setPower(drivePower);
-            robot.leftBack.setPower(drivePower);
-            robot.rightBack.setPower(drivePower);
+            percentDenom = percentDenom + 100;
         }
+        yTransitPercent = engine.gamepad1.left_stick_y * 100;
+        xTransitPercent = engine.gamepad1.left_stick_x * 100;
+        rotPercent = engine.gamepad1.right_stick_x * -100;
 
-        if (Math.abs(engine.gamepad1.left_stick_x) > minInput &&
-            armPos == 0) {
+        lfPower = ((yTransitPercent + -xTransitPercent + rotPercent) / percentDenom);
+        robot.leftFront.setPower(lfPower);
 
-            drivePower = engine.gamepad1.left_stick_x;
-            robot.leftFront.setPower(-drivePower);
-            robot.rightFront.setPower(drivePower);
-            robot.leftBack.setPower(drivePower);
-            robot.rightBack.setPower(-drivePower);
-        }
+        rfPower = ((yTransitPercent + xTransitPercent + -rotPercent) / percentDenom);
+        robot.rightFront.setPower(rfPower);
 
-        if (Math.abs(engine.gamepad1.right_stick_x) > minInput &&
-                armPos == 0) {
+        lbPower = ((yTransitPercent + xTransitPercent + rotPercent) / percentDenom);
+        robot.leftBack.setPower(lbPower);
 
-            drivePower = engine.gamepad1.right_stick_x;
-            robot.leftFront.setPower(-drivePower);
-            robot.rightFront.setPower(drivePower);
-            robot.leftBack.setPower(-drivePower);
-            robot.rightBack.setPower(drivePower);
-            lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
-        }
+        rbPower = ((yTransitPercent + -xTransitPercent + -rotPercent) / percentDenom);
+        robot.rightBack.setPower(rbPower);
 
         if (engine.gamepad2.left_stick_button) {
             if (System.currentTimeMillis() - lastMoveTime >= 200) {
@@ -182,140 +149,140 @@ public class SodiPizzaTeleOPState extends CyberarmState {
             }
         }
 
-        // This moves the arm to Collect position, which is at servo position 0.00.
-        if (engine.gamepad2.a && !engine.gamepad2.start) {
-            armPos = 1;
-        }
-
-        if (armPos == 1) {
-
-            buttonPressed = 'a';
-
-            if (Math.abs(drivePower) > 0.25) {
-                drivePower = 0.15;
-                robot.leftFront.setPower(drivePower);
-                robot.leftBack.setPower(drivePower);
-                robot.rightFront.setPower(drivePower);
-                robot.rightBack.setPower(drivePower);
-            } else {
-                //if servo's position is greater than Collect position with a run-to tolerance of 0.05,
-                //decrement position at a rate of 0.05 per 150 milliseconds.
-                if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 150) {
-                    robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
-                    lastMoveTime = System.currentTimeMillis();
-                } else if (System.currentTimeMillis() - lastMoveTime >= 150) {
-                    robot.shoulder.setPosition(ARM_COLLECT);
-                    armPos = 0;
-                }
-
-            }
-
-        }
-        //End of code for armPos = 1
-
-        // This moves the arm to Precollect position, which is at servo position 0.05.
-        if (engine.gamepad2.x) {
-            armPos = 2;
-        }
-
-        if (armPos == 2) {
-
-            buttonPressed = 'x';
-
-            if (Math.abs(drivePower) > 0.25) {
-                drivePower = 0.15;
-                robot.leftFront.setPower(drivePower);
-                robot.leftBack.setPower(drivePower);
-                robot.rightFront.setPower(drivePower);
-                robot.rightBack.setPower(drivePower);
-            } else {
-                //if servo's position is greater than Precollect position with a run-to tolerance of 0.05,
-                //decrement position at a rate of 0.05 per 150 milliseconds.
-                if (robot.shoulder.getPosition() > ARM_PRECOLLECT + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
-                    robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
-                    lastMoveTime = System.currentTimeMillis();
-                }//Incrementing from Collect position is unnecessary, because Collect is within the tolerance of run-to.
-                 else if (System.currentTimeMillis() - lastMoveTime >= 150) {
-                    robot.shoulder.setPosition(ARM_PRECOLLECT);
-                    armPos = 0;
-                }
-
-            }
-        }
-        //End of code for armPos = 2
-
-        // This moves the arm to Deliver position, which is at servo position 0.28.
-        if (engine.gamepad2.b && !engine.gamepad2.start) {
-            armPos = 3;
-        }
-
-        if (armPos == 3) {
-
-            buttonPressed = 'b';
-
-            if (Math.abs(drivePower) > 0.25) {
-                drivePower = 0.15;
-                robot.leftFront.setPower(drivePower);
-                robot.leftBack.setPower(drivePower);
-                robot.rightFront.setPower(drivePower);
-                robot.rightBack.setPower(drivePower);
-            } else {
-                //if servo's position is less than Deliver position with a run-to tolerance of 0.05,
-                //increment position at a rate of 0.05 per 150 milliseconds.
-                if (robot.shoulder.getPosition() < ARM_DELIVER - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
-                    robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
-                    lastMoveTime = System.currentTimeMillis();
-                }//if servo's position is greater than Deliver position with a run-to tolerance of 0.05,
-                //decrement position at a rate of 0.05 per 150 milliseconds.
-                else if (robot.shoulder.getPosition() > ARM_DELIVER + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
-                    robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
-                    lastMoveTime = System.currentTimeMillis();
-                } else if (System.currentTimeMillis() - lastMoveTime >= 150) {
-                    robot.shoulder.setPosition(ARM_DELIVER);
-                    armPos = 0;
-                }
-
-            }
-        }
-        //End of code for armPos = 3
-
-        // This moves the arm to Stow position, which is at servo position 0.72.
-         if (engine.gamepad2.y) {
-            armPos = 4;
-        }
-
-        if (armPos == 4) {
-
-            buttonPressed = 'y';
-
-            if (Math.abs(drivePower) > 0.25) {
-                drivePower = 0.15;
-                robot.leftFront.setPower(drivePower);
-                robot.leftBack.setPower(drivePower);
-                robot.rightFront.setPower(drivePower);
-                robot.rightBack.setPower(drivePower);
-            } else {
-                //if servo's position is less than Collect position with a run-to tolerance of 0.05,
-                //increment position at a rate of 0.05 per 150 milliseconds.
-                if (robot.shoulder.getPosition() < ARM_STOW - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
-                    robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
-                    lastMoveTime = System.currentTimeMillis();
-                }
-                //Decrementing is unnecessary, because servo is mechanically inhibited from further advancing.
-                else if (System.currentTimeMillis() - lastMoveTime >= 150) {
-                    robot.shoulder.setPosition(ARM_STOW);
-                    armPos = 0;
-                }
-
-            }
-        }
+//        // This moves the arm to Collect position, which is at servo position 0.00.
+//        if (engine.gamepad2.a && !engine.gamepad2.start) {
+//            armPos = 1;
+//        }
+//
+//        if (armPos == 1) {
+//
+//            buttonPressed = 'a';
+//
+//            if (Math.abs(drivePower) > 0.25) {
+//                drivePower = 0.15;
+//                robot.leftFront.setPower(drivePower);
+//                robot.leftBack.setPower(drivePower);
+//                robot.rightFront.setPower(drivePower);
+//                robot.rightBack.setPower(drivePower);
+//            } else {
+//                //if servo's position is greater than Collect position with a run-to tolerance of 0.05,
+//                //decrement position at a rate of 0.05 per 150 milliseconds.
+//                if (robot.shoulder.getPosition() > ARM_PRECOLLECT && System.currentTimeMillis() - lastMoveTime >= 150) {
+//                    robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
+//                    lastMoveTime = System.currentTimeMillis();
+//                } else if (System.currentTimeMillis() - lastMoveTime >= 150) {
+//                    robot.shoulder.setPosition(ARM_COLLECT);
+//                    armPos = 0;
+//                }
+//
+//            }
+//
+//        }
+//        //End of code for armPos = 1
+//
+//        // This moves the arm to Precollect position, which is at servo position 0.05.
+//        if (engine.gamepad2.x) {
+//            armPos = 2;
+//        }
+//
+//        if (armPos == 2) {
+//
+//            buttonPressed = 'x';
+//
+//            if (Math.abs(drivePower) > 0.25) {
+//                drivePower = 0.15;
+//                robot.leftFront.setPower(drivePower);
+//                robot.leftBack.setPower(drivePower);
+//                robot.rightFront.setPower(drivePower);
+//                robot.rightBack.setPower(drivePower);
+//            } else {
+//                //if servo's position is greater than Precollect position with a run-to tolerance of 0.05,
+//                //decrement position at a rate of 0.05 per 150 milliseconds.
+//                if (robot.shoulder.getPosition() > ARM_PRECOLLECT + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
+//                    robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
+//                    lastMoveTime = System.currentTimeMillis();
+//                }//Incrementing from Collect position is unnecessary, because Collect is within the tolerance of run-to.
+//                 else if (System.currentTimeMillis() - lastMoveTime >= 150) {
+//                    robot.shoulder.setPosition(ARM_PRECOLLECT);
+//                    armPos = 0;
+//                }
+//
+//            }
+//        }
+//        //End of code for armPos = 2
+//
+//        // This moves the arm to Deliver position, which is at servo position 0.28.
+//        if (engine.gamepad2.b && !engine.gamepad2.start) {
+//            armPos = 3;
+//        }
+//
+//        if (armPos == 3) {
+//
+//            buttonPressed = 'b';
+//
+//            if (Math.abs(drivePower) > 0.25) {
+//                drivePower = 0.15;
+//                robot.leftFront.setPower(drivePower);
+//                robot.leftBack.setPower(drivePower);
+//                robot.rightFront.setPower(drivePower);
+//                robot.rightBack.setPower(drivePower);
+//            } else {
+//                //if servo's position is less than Deliver position with a run-to tolerance of 0.05,
+//                //increment position at a rate of 0.05 per 150 milliseconds.
+//                if (robot.shoulder.getPosition() < ARM_DELIVER - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
+//                    robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
+//                    lastMoveTime = System.currentTimeMillis();
+//                }//if servo's position is greater than Deliver position with a run-to tolerance of 0.05,
+//                //decrement position at a rate of 0.05 per 150 milliseconds.
+//                else if (robot.shoulder.getPosition() > ARM_DELIVER + 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
+//                    robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
+//                    lastMoveTime = System.currentTimeMillis();
+//                } else if (System.currentTimeMillis() - lastMoveTime >= 150) {
+//                    robot.shoulder.setPosition(ARM_DELIVER);
+//                    armPos = 0;
+//                }
+//
+//            }
+//        }
+//        //End of code for armPos = 3
+//
+//        // This moves the arm to Stow position, which is at servo position 0.72.
+//         if (engine.gamepad2.y) {
+//            armPos = 4;
+//        }
+//
+//        if (armPos == 4) {
+//
+//            buttonPressed = 'y';
+//
+//            if (Math.abs(drivePower) > 0.25) {
+//                drivePower = 0.15;
+//                robot.leftFront.setPower(drivePower);
+//                robot.leftBack.setPower(drivePower);
+//                robot.rightFront.setPower(drivePower);
+//                robot.rightBack.setPower(drivePower);
+//            } else {
+//                //if servo's position is less than Collect position with a run-to tolerance of 0.05,
+//                //increment position at a rate of 0.05 per 150 milliseconds.
+//                if (robot.shoulder.getPosition() < ARM_STOW - 0.05 && System.currentTimeMillis() - lastMoveTime >= 150) {
+//                    robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
+//                    lastMoveTime = System.currentTimeMillis();
+//                }
+//                //Decrementing is unnecessary, because servo is mechanically inhibited from further advancing.
+//                else if (System.currentTimeMillis() - lastMoveTime >= 150) {
+//                    robot.shoulder.setPosition(ARM_STOW);
+//                    armPos = 0;
+//                }
+//
+//            }
+//        }
         //End of code for armPos = 4
 
-        if (engine.gamepad2.dpad_left) {
-            robot.gripper.setPosition(GRIPPER_OPEN);
-        } else if (engine.gamepad2.dpad_right) {
-            robot.gripper.setPosition(GRIPPER_CLOSED);
-        }
+//        if (engine.gamepad2.dpad_left) {
+//            robot.gripper.setPosition(GRIPPER_OPEN);
+//        } else if (engine.gamepad2.dpad_right) {
+//            robot.gripper.setPosition(GRIPPER_CLOSED);
+//        }
 
         gp1checker.update();
         gp2checker.update();