Implemented Pizza bot teleop, updated Vexy's teleop control scheme

2025-12-15 16:52:35 +00:00 · 2023-11-11 18:10:46 -06:00
parent 4f55a49590
commit ef919b821b
7 changed files with 443 additions and 0 deletions
--- a/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/PizzaMinibot.java
+++ b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/PizzaMinibot.java
@@ -0,0 +1,117 @@
+package dev.cyberarm.minibots.pizza;
+
+import com.arcrobotics.ftclib.hardware.motors.MotorEx;
+import com.arcrobotics.ftclib.hardware.motors.MotorGroup;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+
+public class PizzaMinibot {
+    public static double GRIPPER_CLOSED = 0.333; // ~90 degrees
+    public static double GRIPPER_OPEN = 0.75; // ~205 degrees
+
+    public static double ARM_COLLECT = 0.0; // ~? degrees
+    public static double ARM_PRECOLLECT = 0.05; // ~? degrees
+    public static double ARM_DELIVER = 0.28; // ~? degrees
+    public static double ARM_STOW = 0.72; // ~? degrees
+    public static double ARM_HOVER_5_STACK = 0.10;
+    public static double ARM_HOVER_4_STACK = 0.08;
+    public static double ARM_HOVER_3_STACK = 0.07;
+    public static double ARM_HOVER_2_STACK = 0.06;
+    public static double ARM_HOVER_1_STACK = ARM_PRECOLLECT;
+    public MotorEx leftFront, rightFront, leftBack, rightBack;
+    public MotorGroup left, right;
+    public IMU imu;
+    public Servo gripper, arm;
+    public int armStackPosition = -1;
+
+    private CyberarmEngine engine;
+
+    public PizzaMinibot(CyberarmEngine engine) {
+        this.engine = engine;
+        leftFront = new MotorEx(engine.hardwareMap, "leftFront");
+        rightFront = new MotorEx(engine.hardwareMap, "rightFront");
+
+        leftBack = new MotorEx(engine.hardwareMap, "leftBack");
+        rightBack = new MotorEx(engine.hardwareMap, "rightBack");
+
+        rightFront.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
+        rightBack.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
+
+        leftFront.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
+        leftBack.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        double wheelRadius = 75.0 / 2; // mm
+        double distancePerTick = Math.PI * wheelRadius * wheelRadius;
+        leftFront.setDistancePerPulse(distancePerTick);
+        rightFront.setDistancePerPulse(distancePerTick);
+        leftBack.setDistancePerPulse(distancePerTick);
+        rightBack.setDistancePerPulse(distancePerTick);
+
+        left = new MotorGroup(leftFront, leftBack);
+        right = new MotorGroup(rightFront, rightBack);
+
+        imu = engine.hardwareMap.get(IMU.class, "imu");
+        IMU.Parameters parameters = new IMU.Parameters(
+                new RevHubOrientationOnRobot(
+                        RevHubOrientationOnRobot.LogoFacingDirection.UP,
+                        RevHubOrientationOnRobot.UsbFacingDirection.LEFT));
+
+        imu.initialize(parameters);
+
+        gripper = engine.hardwareMap.servo.get("gripper");
+        arm = engine.hardwareMap.servo.get("arm");
+
+        gripper.setPosition(PizzaMinibot.GRIPPER_CLOSED);
+    }
+
+    public void standardTelemetry() {
+        engine.telemetry.addLine();
+
+        engine.telemetry.addLine("Motors");
+        engine.telemetry.addData(
+                "Left Front",
+                "Power: %.2f, Current: %.2f mAmp, Position: %d",
+                leftFront.motorEx.getPower(),
+                leftFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
+                leftFront.motorEx.getCurrentPosition());
+        engine.telemetry.addData(
+                "Right Front",
+                "Power: %.2f, Current: %.2f mAmp, Position: %d",
+                rightFront.motorEx.getPower(),
+                rightFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
+                rightFront.motorEx.getCurrentPosition());
+        engine.telemetry.addData(
+                "Left Back",
+                "Power: %.2f, Current: %.2f mAmp, Position: %d",
+                leftBack.motorEx.getPower(),
+                leftBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
+                leftBack.motorEx.getCurrentPosition());
+        engine.telemetry.addData(
+                "Right Back",
+                "Power: %.2f, Current: %.2f mAmp, Position: %d",
+                rightBack.motorEx.getPower(),
+                rightBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
+                rightBack.motorEx.getCurrentPosition());
+
+        engine.telemetry.addLine();
+        engine.telemetry.addLine("Servos");
+        engine.telemetry.addData("Gripper", gripper.getPosition());
+        engine.telemetry.addData("Arm", gripper.getPosition());
+        engine.telemetry.addLine();
+        engine.telemetry.addData("Arm Stack Position", armStackPosition);
+
+        engine.telemetry.addLine();
+    }
+
+    public void teleopTelemetry() {
+        engine.telemetry.addLine();
+
+        engine.telemetry.addLine();
+    }
+}
--- a/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaTeleOpEngine.java
+++ b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaTeleOpEngine.java
@@ -0,0 +1,26 @@
+package dev.cyberarm.minibots.pizza.engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+import dev.cyberarm.minibots.pizza.PizzaMinibot;
+import dev.cyberarm.minibots.pizza.states.Pilot;
+
+@TeleOp(name = "Cyberarm Pizza Teleop", group = "MINIBOT")
+public class PizzaTeleOpEngine extends CyberarmEngine {
+    PizzaMinibot robot;
+    @Override
+    public void setup() {
+        robot = new PizzaMinibot(this);
+
+        addState(new Pilot(robot));
+    }
+
+    @Override
+    public void loop() {
+        robot.standardTelemetry();
+        robot.teleopTelemetry();
+
+        super.loop();
+    }
+}
--- a/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/Pilot.java
+++ b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/Pilot.java
@@ -0,0 +1,148 @@
+package dev.cyberarm.minibots.pizza.states;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.minibots.pizza.PizzaMinibot;
+
+public class Pilot extends CyberarmState {
+    final PizzaMinibot robot;
+
+    public Pilot(PizzaMinibot robot) {
+        this.robot = robot;
+    }
+    @Override
+    public void exec() {
+        /// --- IMU Reset --- ///
+        if (engine.gamepad1.guide) {
+            robot.imu.resetYaw();
+        }
+
+        /// --- GRIPPER --- ///
+        if (engine.gamepad1.x) {
+            robot.gripper.setPosition(PizzaMinibot.GRIPPER_CLOSED);
+        } else if (engine.gamepad1.b) {
+            robot.gripper.setPosition(PizzaMinibot.GRIPPER_OPEN);
+        }
+
+        /// --- ARM --- ///
+        if (engine.gamepad1.dpad_down) {
+            robot.arm.setPosition(PizzaMinibot.ARM_STOW);
+            robot.armStackPosition = -1;
+        } else if (engine.gamepad1.dpad_up) {
+            robot.arm.setPosition(PizzaMinibot.ARM_COLLECT);
+            robot.armStackPosition = -1;
+        } else if (engine.gamepad1.dpad_right) {
+            robot.arm.setPosition(PizzaMinibot.ARM_PRECOLLECT);
+            robot.armStackPosition = -1;
+        } else if (engine.gamepad1.dpad_left) {
+            robot.arm.setPosition(PizzaMinibot.ARM_DELIVER);
+            robot.armStackPosition = -1;
+        }
+
+        /// --- DRIVE --- ///
+//        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 botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+
+        // Rotate the movement direction counter to the bot's rotation
+        double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
+        double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);
+
+        rotX = rotX * 1.1;  // Counteract imperfect strafing
+
+        // Denominator is the largest motor power (absolute value) or 1
+        // This ensures all the powers maintain the same ratio,
+        // but only if at least one is out of the range [-1, 1]
+        double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
+        double frontLeftPower = (rotY + rotX + rx) / denominator;
+        double backLeftPower = (rotY - rotX + rx) / denominator;
+        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 maxPower = 0.5;
+
+        robot.leftFront.motorEx.setPower(frontLeftPower * maxPower);
+        robot.leftBack.motorEx.setPower(backLeftPower * maxPower);
+        robot.rightFront.motorEx.setPower(frontRightPower * maxPower);
+        robot.rightBack.motorEx.setPower(backRightPower * maxPower);
+    }
+
+    @Override
+    public void buttonDown(Gamepad gamepad, String button) {
+        if (gamepad == engine.gamepad1) {
+            if (button.equals("left_bumper")) {
+                switch (robot.armStackPosition) {
+                    case -1:
+                        robot.arm.setPosition(PizzaMinibot.ARM_HOVER_5_STACK);
+                        robot.armStackPosition = 5;
+                        break;
+                    case 5:
+                        robot.arm.setPosition(PizzaMinibot.ARM_HOVER_5_STACK);
+                        robot.armStackPosition = 5;
+                        break;
+                    case 4:
+                        robot.arm.setPosition(PizzaMinibot.ARM_HOVER_5_STACK);
+                        robot.armStackPosition = 5;
+                        break;
+                    case 3:
+                        robot.arm.setPosition(PizzaMinibot.ARM_HOVER_4_STACK);
+                        robot.armStackPosition = 4;
+                        break;
+                    case 2:
+                        robot.arm.setPosition(PizzaMinibot.ARM_HOVER_3_STACK);
+                        robot.armStackPosition = 3;
+                        break;
+                    case 1:
+                        robot.arm.setPosition(PizzaMinibot.ARM_HOVER_2_STACK);
+                        robot.armStackPosition = 2;
+                        break;
+                    default:
+                        break;
+                }
+            }
+            if (button.equals("right_bumper")) {
+                switch (robot.armStackPosition) {
+                    case -1:
+                        robot.arm.setPosition(PizzaMinibot.ARM_HOVER_5_STACK);
+                        robot.armStackPosition = 5;
+                        break;
+                    case 5:
+                        robot.arm.setPosition(PizzaMinibot.ARM_HOVER_4_STACK);
+                        robot.armStackPosition = 4;
+                        break;
+                    case 4:
+                        robot.arm.setPosition(PizzaMinibot.ARM_HOVER_3_STACK);
+                        robot.armStackPosition = 3;
+                        break;
+                    case 3:
+                        robot.arm.setPosition(PizzaMinibot.ARM_HOVER_2_STACK);
+                        robot.armStackPosition = 2;
+                        break;
+                    case 2:
+                        robot.arm.setPosition(PizzaMinibot.ARM_HOVER_1_STACK);
+                        robot.armStackPosition = 1;
+                        break;
+                    default:
+                        break;
+                }
+            }
+        }
+    }
+}
+
--- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java
+++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java
@@ -0,0 +1,38 @@
+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.robotcore.hardware.IMU;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+
+public class YellowMinibot {
+    public MotorEx leftFront, rightFront, leftBack, rightBack;
+    public MotorGroup left, right;
+    public IMU imu;
+    public YellowMinibot(CyberarmEngine engine) {
+        leftFront = new MotorEx(engine.hardwareMap, "leftFront");
+        rightFront = new MotorEx(engine.hardwareMap, "rightFront");
+
+        leftBack = new MotorEx(engine.hardwareMap, "leftBack");
+        rightBack = new MotorEx(engine.hardwareMap, "rightBack");
+
+        double distancePerTick = 1962.5; // 3.14 * 25mm * 25mm
+        leftFront.setDistancePerPulse(distancePerTick);
+        rightFront.setDistancePerPulse(distancePerTick);
+        leftBack.setDistancePerPulse(distancePerTick);
+        rightBack.setDistancePerPulse(distancePerTick);
+
+        left = new MotorGroup(leftFront, leftBack);
+        right = new MotorGroup(rightFront, rightBack);
+
+        imu = engine.hardwareMap.get(IMU.class, "imu");
+        IMU.Parameters parameters = new IMU.Parameters(
+                new RevHubOrientationOnRobot(
+                        RevHubOrientationOnRobot.LogoFacingDirection.UP,
+                        RevHubOrientationOnRobot.UsbFacingDirection.RIGHT));
+
+        imu.initialize(parameters);
+    }
+}
--- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/engines/YellowMinibotTeleOpEngine.java
+++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/engines/YellowMinibotTeleOpEngine.java
@@ -0,0 +1,19 @@
+package dev.cyberarm.minibots.yellow.engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+import dev.cyberarm.minibots.yellow.YellowMinibot;
+import dev.cyberarm.minibots.yellow.states.Pilot;
+
+@TeleOp(name = "Cyberarm Yellow Teleop", group = "MINIBOT")
+public class YellowMinibotTeleOpEngine extends CyberarmEngine {
+    public YellowMinibot robot;
+    @Override
+    public void setup() {
+        robot = new YellowMinibot(this);
+
+        addState(new Pilot(robot));
+//        addState(new Move(robot, 10, 8, 0.25, 0.25));
+    }
+}
--- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Move.java
+++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Move.java
@@ -0,0 +1,34 @@
+package dev.cyberarm.minibots.yellow.states;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.minibots.yellow.YellowMinibot;
+
+public class Move extends CyberarmState {
+    final YellowMinibot robot;
+    final double leftDistance, rightDistance, leftPower, rightPower;
+    public Move(YellowMinibot robot, double leftDistance, double rightDistance, double leftPower, double rightPower) {
+        this.robot = robot;
+
+        this.leftDistance = leftDistance;
+        this.rightDistance = rightDistance;
+        this.leftPower = leftPower;
+        this.rightPower = rightPower;
+    }
+
+    @Override
+    public void start() {
+        robot.leftFront.setTargetDistance(leftDistance);
+        robot.leftBack.setTargetDistance(leftDistance);
+
+        robot.leftFront.setTargetDistance(rightDistance);
+        robot.leftBack.setTargetDistance(rightDistance);
+    }
+
+    @Override
+    public void exec() {
+        if (robot.leftFront.atTargetPosition()) {
+            robot.leftFront.setVelocity(0);
+            robot.leftBack.setVelocity(0);
+        }
+    }
+}
--- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java
+++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java
@@ -0,0 +1,61 @@
+package dev.cyberarm.minibots.yellow.states;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.minibots.yellow.YellowMinibot;
+
+public class Pilot extends CyberarmState {
+    final YellowMinibot robot;
+
+    public Pilot(YellowMinibot robot) {
+        this.robot = robot;
+    }
+    @Override
+    public void exec() {
+        /// --- IMU Reset --- ///
+        if (engine.gamepad1.guide) {
+            robot.imu.resetYaw();
+        }
+
+        /// --- DRIVE --- ///
+//        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 botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+
+        // Rotate the movement direction counter to the bot's rotation
+        double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
+        double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);
+
+        rotX = rotX * 1.1;  // Counteract imperfect strafing
+
+        // Denominator is the largest motor power (absolute value) or 1
+        // This ensures all the powers maintain the same ratio,
+        // but only if at least one is out of the range [-1, 1]
+        double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
+        double frontLeftPower = (rotY + rotX + rx) / denominator;
+        double backLeftPower = (rotY - rotX + rx) / denominator;
+        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 maxPower = 1.0;
+
+        robot.leftFront.motorEx.setPower(frontLeftPower * maxPower);
+        robot.leftBack.motorEx.setPower(backLeftPower * maxPower);
+        robot.rightFront.motorEx.setPower(frontRightPower * maxPower);
+        robot.rightBack.motorEx.setPower(backRightPower * maxPower);
+    }
+}