From ed527be03475449349328fa182e85a6cbfb3e4df Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Sat, 28 Oct 2023 14:41:32 -0500 Subject: [PATCH] Starting point for Bumblebee --- .../src/main/java/opmodes/MainTeleOp.java | 19 +++++++ .../ftc/teamcode/hardware/Claw.java | 46 ++++++++++++++++ .../ftc/teamcode/hardware/Gantry.java | 52 +++++++++++++++++++ .../ftc/teamcode/hardware/Lift.java | 33 ++++++++++++ .../ftc/teamcode/hardware/Robot.java | 14 ++++- .../ftc/teamcode/hardware/RobotConstants.java | 28 ++++++++++ build.dependencies.gradle | 2 +- 7 files changed, 191 insertions(+), 3 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Lift.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index 8c10398..a4a96d6 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java @@ -1,5 +1,7 @@ package opmodes; +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -12,6 +14,8 @@ public class MainTeleOp extends OpMode { @Override public void init() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + this.robot = new Robot(this.hardwareMap); telemetry.addData("Status", "Initialized"); } @@ -22,6 +26,21 @@ public class MainTeleOp extends OpMode { double y = -gamepad1.left_stick_y; double z = gamepad1.right_stick_x; + // Set the drive input this.robot.getDrive().setInput(x, y, z); + + // Pickup +// this.robot.getClaw().close(); +// this.robot.getClaw().open(); +// this.robot.getClaw().up(); +// this.robot.getClaw().down(); + + // Gantry +// this.robot.getGantry().up(); +// this.robot.getGantry().down(); +// this.robot.getGantry().armIn(); +// this.robot.getGantry().armOut(); +// this.robot.getGantry().intake(); +// this.robot.getGantry().deposit(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java new file mode 100644 index 0000000..6a92832 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_LEFT_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MAX; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MIN; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_RIGHT_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_MAX; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_MIN; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_NAME; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +public class Claw { + private final Servo claw; + private final Servo armLeft; + private final Servo armRight; + + public Claw(HardwareMap hardwareMap) { + this.claw = hardwareMap.get(Servo.class, CLAW_NAME); + this.armLeft = hardwareMap.get(Servo.class, CLAW_ARM_LEFT_NAME); + this.armRight = hardwareMap.get(Servo.class, CLAW_ARM_RIGHT_NAME); + this.armRight.setDirection(Servo.Direction.REVERSE); + } + + public void open() { + this.claw.setPosition(CLAW_MAX); + } + + public void close() { + this.claw.setPosition(CLAW_MIN); + } + + public void up() { + this.setArmPosition(PICKUP_ARM_MAX); + } + + public void down() { + this.setArmPosition(PICKUP_ARM_MIN); + } + + public void setArmPosition(double target) { + this.armLeft.setPosition(target); + this.armRight.setPosition(target); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java new file mode 100644 index 0000000..354bde5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_MAX; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_MIN; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_POSITIONS; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_NAME; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +import lombok.Getter; + +public class Gantry extends Lift { + private final Servo xServo; + private final Servo armServo; + private final Servo screwServo; + private int currentScrewIndex = 0; + + public Gantry(HardwareMap hardwareMap) { + super(hardwareMap); + this.xServo = hardwareMap.get(Servo.class, GANTRY_X_NAME); + this.armServo = hardwareMap.get(Servo.class, GANTRY_ARM_NAME); + this.screwServo = hardwareMap.get(Servo.class, GANTRY_SCREW_NAME); + } + + public void setX(double x) { + this.xServo.setPosition(x); + } + + public void armOut() { + this.armServo.setPosition(GANTRY_ARM_MAX); + } + + public void armIn() { + this.armServo.setPosition(GANTRY_ARM_MIN); + } + + public void resetScrew() { + this.currentScrewIndex = 0; + this.screwServo.setPosition(GANTRY_SCREW_POSITIONS[currentScrewIndex]); + } + + public void intake() { + this.screwServo.setPosition(GANTRY_SCREW_POSITIONS[this.currentScrewIndex++]); + } + + public void deposit() { + this.screwServo.setPosition(GANTRY_SCREW_POSITIONS[this.currentScrewIndex--]); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Lift.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Lift.java new file mode 100644 index 0000000..b4c4a17 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Lift.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_MOTOR_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_UP; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import lombok.Getter; + +public class Lift { + protected final DcMotor lift; + + protected Lift(HardwareMap hardwareMap) { + this.lift = hardwareMap.get(DcMotor.class, LIFT_MOTOR_NAME); + this.lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.lift.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + public void up() { + this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.lift.setTargetPosition(LIFT_UP); + } + + public void down() { + this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.lift.setTargetPosition(0); + } + + public void setInput(double x) { + this.lift.setPower(x); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index 4c750ce..05aefab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -2,18 +2,28 @@ package org.firstinspires.ftc.teamcode.hardware; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; + import lombok.Getter; public class Robot { @Getter - private Drive drive; + private MecanumDrive drive; + + @Getter + private Gantry gantry; + + @Getter + private Claw claw; public Robot(HardwareMap hardwareMap) { this.init(hardwareMap); } private void init(HardwareMap hardwareMap) { - this.drive = new Drive(hardwareMap); + this.drive = new MecanumDrive(hardwareMap); + this.gantry = new Gantry(hardwareMap); + this.claw = new Claw(hardwareMap); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java new file mode 100644 index 0000000..42ef648 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java @@ -0,0 +1,28 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import com.acmerobotics.dashboard.config.Config; + +@Config +public class RobotConstants { + public static final String LIFT_MOTOR_NAME = "lift"; + public static final String CLAW_ARM_LEFT_NAME = "clawArmLeft"; + public static final String CLAW_ARM_RIGHT_NAME = "clawArmRight"; + public static final String CLAW_NAME = "claw"; + public static final String GANTRY_X_NAME = "gantryX"; + public static final String GANTRY_ARM_NAME = "gantryArm"; + public static final String GANTRY_SCREW_NAME = "screw"; + + // Lift + public static int LIFT_UP = 100; + + // Arm + public static double PICKUP_ARM_MIN = 0; + public static double PICKUP_ARM_MAX = 1; + public static double CLAW_MIN = 0; + public static double CLAW_MAX = 1; + + // Gantry + public static double GANTRY_ARM_MIN = 0; + public static double GANTRY_ARM_MAX = 0; + public static double[] GANTRY_SCREW_POSITIONS = new double[] { 0, 0.1, 0.2, 1.0 }; +} diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 3cd3424..1a77d26 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -20,7 +20,7 @@ dependencies { implementation 'org.tensorflow:tensorflow-lite-task-vision:0.4.3' runtimeOnly 'org.tensorflow:tensorflow-lite:2.12.0' implementation 'androidx.appcompat:appcompat:1.2.0' - implementation 'com.acmerobotics.dashboard:dashboard:0.4.12' + implementation 'com.acmerobotics.dashboard:dashboard:0.4.13' compileOnly 'org.projectlombok:lombok:1.18.30' annotationProcessor 'org.projectlombok:lombok:1.18.30'