From 6f2ed777c212693b0be470d7ffc2afc91ec094be Mon Sep 17 00:00:00 2001 From: sihan Date: Wed, 1 Nov 2023 20:50:46 -0500 Subject: [PATCH] Intake, drive and hang for prime on boiler plate. --- TeamCode/build.gradle | 1 + .../src/main/java/opmodes/MainTeleOp.java | 27 ------- .../ftc/teamcode/hardware/Robot.java | 70 ++++++++++++++++--- .../roadrunner/drive/MecanumDrive.java | 23 +++--- .../ftc/teamcode/opmodes/MainTeleOp.java | 40 +++++++++++ .../ftc/teamcode/util/Configurables.java | 10 +-- .../ftc/teamcode/util/Constants.java | 2 + build.gradle | 2 +- 8 files changed, 126 insertions(+), 49 deletions(-) delete mode 100644 TeamCode/src/main/java/opmodes/MainTeleOp.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 442471b..dc65aac 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -29,5 +29,6 @@ dependencies { implementation 'org.apache.commons:commons-math3:3.6.1' implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7' + implementation 'org.openftc:easyopencv:1.7.0' implementation 'com.acmerobotics.roadrunner:core:0.5.6' } diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java deleted file mode 100644 index 8c10398..0000000 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ /dev/null @@ -1,27 +0,0 @@ -package opmodes; - -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.hardware.Robot; - -@TeleOp(name = "MainTeleOp", group = "Main") -public class MainTeleOp extends OpMode { - - private Robot robot; - - @Override - public void init() { - this.robot = new Robot(this.hardwareMap); - telemetry.addData("Status", "Initialized"); - } - - @Override - public void loop() { - double x = gamepad1.left_stick_x; - double y = -gamepad1.left_stick_y; - double z = gamepad1.right_stick_x; - - this.robot.getDrive().setInput(x, y, z); - } -} 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..d96a838 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 @@ -1,19 +1,71 @@ package org.firstinspires.ftc.teamcode.hardware; -import com.qualcomm.robotcore.hardware.HardwareMap; +import static org.firstinspires.ftc.teamcode.util.Configurables.LOCK; +import static org.firstinspires.ftc.teamcode.util.Configurables.LOCKSPEED; +import static org.firstinspires.ftc.teamcode.util.Configurables.UNLOCK; +import static org.firstinspires.ftc.teamcode.util.Configurables.UNLOCKSPEED; +import static org.firstinspires.ftc.teamcode.util.Constants.LEFT; +import static org.firstinspires.ftc.teamcode.util.Constants.RIGHT; -import lombok.Getter; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; public class Robot { + private MecanumDrive drive; + private Hang hang; + private Intake intake; - @Getter - private Drive drive; + public Robot init(HardwareMap hardwareMap) { + this.drive = new MecanumDrive(hardwareMap); + this.hang = new Hang().init(hardwareMap); - public Robot(HardwareMap hardwareMap) { - this.init(hardwareMap); + return this; } - private void init(HardwareMap hardwareMap) { - this.drive = new Drive(hardwareMap); + public Intake getIntake() {return this.intake;} + + public Hang getHang() {return this.hang;} + + public MecanumDrive getDrive() {return this.drive;} + + public static class Intake { + private DcMotor intake = null; + + public Intake init(HardwareMap hardwareMap) { + this.intake = hardwareMap.dcMotor.get("intake"); + this.intake.setDirection(DcMotorSimple.Direction.REVERSE); + this.intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + return this; + } + + public void spinIn() {this.intake.setPower(0.7);} + + public void spinOut() {this.intake.setPower(-0.7);} } -} + + public static class Hang { + private Servo hangLeft; + private Servo hangRight; + + public Hang init(HardwareMap hardwareMap) { + this.hangLeft = hardwareMap.get(Servo.class, LEFT); + this.hangRight = hardwareMap.get(Servo.class, RIGHT); + return this; + } + + public void lock() { + this.hangLeft.setPosition(LOCKSPEED); + this.hangRight.setPosition(LOCK); + } + + public void release() { + this.hangLeft.setPosition(UNLOCKSPEED); + this.hangRight.setPosition(UNLOCK); + } + } + +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java index 4296787..cce0932 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java @@ -8,6 +8,9 @@ import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveCons import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.RUN_USING_ENCODER; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.TRACK_WIDTH; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.encoderTicksToInches; +import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA; +import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic; +import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV; import androidx.annotation.NonNull; @@ -29,6 +32,7 @@ import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.PIDFCoefficients; @@ -40,9 +44,7 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceRunner; import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.LynxModuleUtil; -import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV; -import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA; -import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic; + import java.util.ArrayList; import java.util.Arrays; import java.util.List; @@ -53,7 +55,7 @@ import java.util.List; * Simple mecanum drive hardware implementation for REV hardware. */ @Config -public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive { +public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive { public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0); public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); @@ -309,9 +311,14 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive return new ProfileAccelerationConstraint(maxAccel); } - public void setInput(double x, double y, double z) { - Pose2d vel = new Pose2d(x, y, z); - setDrivePower(vel); - update(); + public void setInput(Gamepad gamepad1, Gamepad gamepad2) { + double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + this.setWeightedDrivePower( + new Pose2d( + gamepad1.left_stick_y* -1, + gamepad1.left_stick_x*-1, + -gamepad1.right_stick_x + )); } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java new file mode 100644 index 0000000..678c49b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java @@ -0,0 +1,40 @@ +package org.firstinspires.ftc.teamcode.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; + +@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "CenterStage", group = "Development") +public class MainTeleOp extends OpMode { + private Robot robot; + private double speedMultiplier = 1.0; // Start at 100% speed + private MecanumDrive drive; + + @Override + public void init() { + this.robot = new Robot().init(hardwareMap); + this.robot.getHang().lock(); + } + + @Override + public void loop() { +//Drive + robot.getDrive().setInput(gamepad1, gamepad2); +//Hang + if (gamepad1.b) { + this.robot.getHang().release(); + } else { + this.robot.getHang().lock(); + } +//Intake + if (gamepad1.x) { + this.robot.getIntake().spinIn(); + } else if (gamepad1.y) { + this.robot.getIntake().spinOut(); + } + + + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java index a2f575b..350c5a3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java @@ -1,9 +1,5 @@ package org.firstinspires.ftc.teamcode.util; -import org.opencv.core.Point; -import org.opencv.core.Rect; -import org.opencv.core.Size; - public class Configurables { // Robot Constants public static double R_ARM_POWER = 0.2; @@ -40,4 +36,10 @@ public class Configurables { public static double CV_MIN_GOAL_AREA = 0; public static double CV_MAX_GOAL_AREA = 0.3; + //Servo Positions + public static double UNLOCKSPEED = .5; + public static double LOCKSPEED = .1; + public static double UNLOCK = .5; + public static double LOCK = .1; + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java index 9a4afca..199cdf2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java @@ -51,4 +51,6 @@ public class Constants { public static final String STACK_WEBCAM = "Stack Webcam"; public static final String TARGETING_WEBCAM = "Targeting Webcam"; public static final String IMU_SENSOR = "imu"; + public static final String LEFT = "leftHang"; + public static final String RIGHT = "rightHang"; } \ No newline at end of file diff --git a/build.gradle b/build.gradle index 8969a41..265f3f7 100644 --- a/build.gradle +++ b/build.gradle @@ -11,7 +11,7 @@ buildscript { } dependencies { // Note for FTC Teams: Do not modify this yourself. - classpath 'com.android.tools.build:gradle:7.2.0' + classpath 'com.android.tools.build:gradle:7.3.1' } }