From 5eb278042b1f5a44bc6e77c5b34b348b2eb49392 Mon Sep 17 00:00:00 2001 From: Robert Teal Date: Wed, 20 Dec 2023 18:10:59 -0600 Subject: [PATCH] fixed teleop things, started new auto and varun is bad --- .../ftc/teamcode/hardware/Arm.java | 4 +- .../ftc/teamcode/hardware/DroneLauncher.java | 25 -------- .../ftc/teamcode/hardware/Robot.java | 9 +-- .../ftc/teamcode/hardware/Slides.java | 10 +-- .../ftc/teamcode/hardware/endGame_Mechs.java | 50 +++++++++++++++ .../opmode/autonomous/AbstractAuto.java | 5 +- .../opmode/autonomous/RedRightAuto.java | 55 +++++++++++++--- .../autonomous/vannobAuto_RightRed.java | 64 +++++++++++++++++++ .../teamcode/opmode/teleop/MainTeleOp.java | 32 ++++++++-- 9 files changed, 201 insertions(+), 53 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DroneLauncher.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/vannobAuto_RightRed.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java index e40b5cb..806499d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java @@ -24,8 +24,8 @@ public class Arm { private double wristPos; public static double ARM_KP = 0.08; - public static double doorOpenPos = 0.09; - public static double doorClosePos = 0.26; + public static double doorOpenPos = 0.3; + public static double doorClosePos = 0.61; public static double armStart = 0.24; public static double armScore = 0.95; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DroneLauncher.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DroneLauncher.java deleted file mode 100644 index 4bd9274..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DroneLauncher.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.firstinspires.ftc.teamcode.hardware; - -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.Servo; - -import org.firstinspires.ftc.teamcode.util.Constants; - -public class DroneLauncher { - private Servo servo; - public static double initPos = 0; - public static double launchPos = 0.5; - - public DroneLauncher(HardwareMap hardwareMap) { - this.servo = hardwareMap.get(Servo.class, "Drone Launcher"); - this.servo.setPosition(initPos); - } - - public void launch() { - this.servo.setPosition(launchPos); - } - - public void reset() { - this.servo.setPosition(initPos); - } -} 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 9808845..6f0c7ab 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 @@ -18,7 +18,7 @@ public class Robot { public Intake intake; public Slides slides; public Arm arm; - public DroneLauncher droneLauncher; + public endGame_Mechs endGameMechs; public double macroStartTime = 0; public int macroState = 0; @@ -28,7 +28,7 @@ public class Robot { private boolean camEnabled = true; public static double slideWait = 1.5; - public static double scoreWait = 1.25; + public static double scoreWait = 0.65; public static double armWait = 2.0; //Name the objects @@ -39,7 +39,7 @@ public class Robot { intake = new Intake(hardwareMap, UP); slides = new Slides(hardwareMap); arm = new Arm(hardwareMap); - droneLauncher = new DroneLauncher(hardwareMap); + endGameMechs = new endGame_Mechs(hardwareMap); camEnabled = true; } @@ -71,6 +71,7 @@ public class Robot { case(0): macroStartTime = runTime; arm.setDoor(OPEN); + slides.setTarget(slides.getTarget()+70); macroState++; break; case(1): @@ -104,6 +105,6 @@ public class Robot { } public String getTelemetry() { - return arm.getTelemetry(); + return String.format("Arm:\n------------\n%s\nSlides:\n------------\n%s", arm.getTelemetry(), slides.getTelemetry()); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java index 0a33c2a..e03bf16 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java @@ -19,12 +19,12 @@ public class Slides { public static PIDController controller = new PIDController(p, i, d); public static int targetMin = -10; - public static int targetMax = 555; + public static int targetMax = 830; public static int down = 0; public static int tier1 = 350; - public static int tier2 = 450; - public static int tier3 = 550; + public static int tier2 = 500; + public static int tier3 = 760; private int target = 0; @@ -71,6 +71,8 @@ public class Slides { return target; } + + public boolean atTarget() { return controller.atSetPoint(); } @@ -114,6 +116,6 @@ public class Slides { } public String getTelemetry() { - return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s\nHeightOffset: %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower()); + return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower()); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java new file mode 100644 index 0000000..1385653 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +@Config +public class endGame_Mechs { + private Servo servo; + private Servo hang1; + private Servo hang2; + public static double initPos = 0; + public static double launchPos = 0.5; + public static double hold = 0; + public static double release = 0.5; + public static double hold2 = 0; + public static double release2 = 0.5; + + public endGame_Mechs(HardwareMap hardwareMap) { + this.servo = hardwareMap.get(Servo.class, "Drone Launcher"); + this.servo.setPosition(initPos); + this.hang1 = hardwareMap.get(Servo.class, "Hanger 1"); + this.hang1.setPosition(hold); + this.hang2 = hardwareMap.get(Servo.class, "Hanger 2"); + this.hang2.setPosition(hold); + + } + + public void launch() { + this.servo.setPosition(launchPos); + } + + public void reset() { + this.servo.setPosition(initPos); + } + + + public void release() { + this.servo.setPosition(release); + this.servo.setPosition(release2); + } + + public void hold() { + this.servo.setPosition(hold); + this.servo.setPosition(hold2); + + } +} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java index b3cf8f9..c4a4b8c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java @@ -84,10 +84,6 @@ public abstract class AbstractAuto extends LinearOpMode { step.start(); } -// // update turret and slides position -// PoseStorage.slidesPosition = robot.actuators.getSlides(); -// PoseStorage.turretPosition = robot.actuators.getTurret(); - // while the step is running display telemetry step.whileRunning(); robot.update(currentRuntime); @@ -262,6 +258,7 @@ public abstract class AbstractAuto extends LinearOpMode { @Override public void end() { robot.intake.setDcMotor(0); + robot.intake.setpos(Intake.Position.UP); robot.arm.setDoor(Arm.DoorPosition.CLOSE); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedRightAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedRightAuto.java index ebdf22c..537f6c5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedRightAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedRightAuto.java @@ -15,6 +15,8 @@ public class RedRightAuto extends AbstractAuto { public Trajectory scoreYellow; public Trajectory load1; public Trajectory score1; + public Trajectory load2; + public Trajectory score2; public Trajectory parkRobot; @Override @@ -31,6 +33,12 @@ public class RedRightAuto extends AbstractAuto { Pose2d driveup1 = new Pose2d(12-6, -14, Math.toRadians(180)); Pose2d deposit1 = new Pose2d(48-6, -36-4, Math.toRadians(180)); + Pose2d lineup2 = new Pose2d(12, -14, Math.toRadians(180)); + Pose2d pickup2 = new Pose2d(-58, -14, Math.toRadians(180)); + + Pose2d driveup2 = new Pose2d(12-6, -14, Math.toRadians(180)); + Pose2d deposit2 = new Pose2d(48-6, -36-4, Math.toRadians(180)); + Pose2d park = new Pose2d(48, -12, Math.toRadians(180)); scorePurple = robot.drive.trajectoryBuilder(start) @@ -41,29 +49,53 @@ public class RedRightAuto extends AbstractAuto { .lineToLinearHeading(depositPreload) .build(); + int driveSpeed = 45; + load1 = robot.drive.trajectoryBuilder(scoreYellow.end()) .splineToConstantHeading(lineup1.vec(), lineup1.getHeading(), - SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL) ) .lineToLinearHeading(pickup1, - SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL) ) .build(); score1 = robot.drive.trajectoryBuilder(load1.end(), true) .lineToLinearHeading(driveup1, - SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL) ) .splineToConstantHeading(deposit1.vec(), deposit1.getHeading(), - SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL) ) .build(); - parkRobot = robot.drive.trajectoryBuilder(score1.end()) + load2 = robot.drive.trajectoryBuilder(score1.end()) + .splineToConstantHeading(lineup1.vec(), lineup1.getHeading(), + SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL) + ) + .lineToLinearHeading(pickup1, + SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL) + ) + .build(); + + score2 = robot.drive.trajectoryBuilder(load2.end(), true) + .lineToLinearHeading(driveup1, + SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL) + ) + .splineToConstantHeading(deposit1.vec(), deposit1.getHeading(), + SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL) + ) + .build(); + + parkRobot = robot.drive.trajectoryBuilder(score2.end()) .lineToLinearHeading(park) .build(); @@ -72,14 +104,19 @@ public class RedRightAuto extends AbstractAuto { @Override public void initializeSteps(int location) { + // score preloads followTrajectory(scorePurple); runIntake(-0.4, 0.5); -// followAndExtend(scoreYellow, Slides.Position.TIER1); -// followAndRetract(load1, Slides.Position.DOWN); - followTrajectory(scoreYellow); - followTrajectory(load1); + followAndExtend(scoreYellow, Slides.Position.TIER1); + + // cycle + followAndRetract(load1, Slides.Position.DOWN); intakeStack(Intake.Position.STACK5, Intake.Position.STACK4); followAndExtend(score1, Slides.Position.TIER1); + followAndRetract(load2, Slides.Position.DOWN); + intakeStack(Intake.Position.STACK3, Intake.Position.STACK2); + followAndExtend(score2, Slides.Position.TIER1); + followAndRetract(parkRobot, Slides.Position.DOWN); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/vannobAuto_RightRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/vannobAuto_RightRed.java new file mode 100644 index 0000000..6752599 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/vannobAuto_RightRed.java @@ -0,0 +1,64 @@ +package org.firstinspires.ftc.teamcode.opmode.autonomous; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; + +@Autonomous (name= "varun_is_Dumb") +public class vannobAuto_RightRed extends LinearOpMode { + public Robot robot; + int teamElementLocation = 2; + int macrostate = 0; + + @Override + public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); + + // make trajectories + + while (!(isStarted() || isStopRequested())) { + double currentRuntime = getRuntime(); + robot.update(currentRuntime); + +// int newLocation = robot.camera.getMarkerId(); +// if (newLocation != -1) { +// teamElementLocation = newLocation; +// } + + telemetry.addLine("Initialized"); + + telemetry.addLine("Randomization of T.O.M: "+teamElementLocation); + telemetry.addLine(robot.getTelemetry()); + telemetry.update(); + } + + while(macrostate < 6 || !isStopRequested()){ + //update everything + robot.update(getRuntime()); + + switch (macrostate){ + case (0): + //follow trajectory + macrostate++; + break; + case (1): + // run intake + macrostate++; + break; + case (2): + macrostate++; + break; + case (3): + macrostate++; + break; + case (4): + macrostate++; + break; + case (5): + macrostate++; + break; + } + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java index 6a76404..51b7991 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java @@ -9,7 +9,6 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.controller.Controller; -import org.firstinspires.ftc.teamcode.hardware.Intake; import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.hardware.Slides; @@ -19,7 +18,9 @@ public class MainTeleOp extends OpMode { public static double normal = 0.5; public static double turbo = 1; + public static double slow_mode = 0.15; public static double intakeMax = 0.65; + public static double intakeMax2 = -0.65; private Robot robot; private Controller controller1; @@ -32,7 +33,7 @@ public class MainTeleOp extends OpMode { this.robot = new Robot(hardwareMap); // robot.intake.setpos(Intake.Position.STACK1); - + this.robot.endGameMechs.hold(); while (robot.camera.getFrameCount() < 1) { telemetry.addLine("Initializing..."); telemetry.update(); @@ -40,6 +41,8 @@ public class MainTeleOp extends OpMode { telemetry.addLine("Initialized"); telemetry.update(); + + } @Override @@ -53,6 +56,11 @@ public class MainTeleOp extends OpMode { x *= turbo; y *= turbo; z *= turbo; + } + else if (controller1.getLeftTrigger().getValue() > 0.1) { + x *= slow_mode; + y *= slow_mode; + z *= slow_mode; } else { x *= normal; y *= normal; @@ -61,7 +69,16 @@ public class MainTeleOp extends OpMode { robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); // Driver 2 - robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax); + if (controller2.getRightTrigger().getValue()>=0.1){ + robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax); + } + else if(controller2.getLeftTrigger().getValue()>=0.1){ + robot.intake.setDcMotor(controller2.getLeftTrigger().getValue()*intakeMax2); + } + else { + robot.intake.setDcMotor(0); + } + if (controller2.getRightBumper().isJustPressed()) { robot.intake.incrementPos(); } @@ -71,10 +88,15 @@ public class MainTeleOp extends OpMode { // Drone launcher if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { - this.robot.droneLauncher.launch(); + this.robot.endGameMechs.launch(); } else { - this.robot.droneLauncher.reset(); + this.robot.endGameMechs.reset(); } + //Hang Servos + if (controller1.getX().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { + this.robot.endGameMechs.release(); + } + // macros switch (robot.runningMacro) {