From ef919b821b0277a72578f842ef0f33bc13a3eff0 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sat, 11 Nov 2023 18:10:46 -0600 Subject: [PATCH] Implemented Pizza bot teleop, updated Vexy's teleop control scheme --- .../cyberarm/minibots/pizza/PizzaMinibot.java | 117 ++++++++++++++ .../pizza/engines/PizzaTeleOpEngine.java | 26 +++ .../cyberarm/minibots/pizza/states/Pilot.java | 148 ++++++++++++++++++ .../minibots/yellow/YellowMinibot.java | 38 +++++ .../engines/YellowMinibotTeleOpEngine.java | 19 +++ .../cyberarm/minibots/yellow/states/Move.java | 34 ++++ .../minibots/yellow/states/Pilot.java | 61 ++++++++ 7 files changed, 443 insertions(+) create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/pizza/PizzaMinibot.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaTeleOpEngine.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/Pilot.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/yellow/engines/YellowMinibotTeleOpEngine.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Move.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/PizzaMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/PizzaMinibot.java new file mode 100644 index 0000000..cd8a325 --- /dev/null +++ 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(); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaTeleOpEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaTeleOpEngine.java new file mode 100644 index 0000000..7942449 --- /dev/null +++ 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(); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/Pilot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/Pilot.java new file mode 100644 index 0000000..1d30750 --- /dev/null +++ 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; + } + } + } + } +} + diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java new file mode 100644 index 0000000..2c2d1d3 --- /dev/null +++ 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); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/engines/YellowMinibotTeleOpEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/engines/YellowMinibotTeleOpEngine.java new file mode 100644 index 0000000..2baaad2 --- /dev/null +++ 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)); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Move.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Move.java new file mode 100644 index 0000000..1622b77 --- /dev/null +++ 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); + } + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java new file mode 100644 index 0000000..9a5d919 --- /dev/null +++ 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); + } +}