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 d3ad605..838b8f6 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 @@ -65,7 +65,7 @@ public class Arm { } } - public void openDoor(DoorPos.DoorPosition pos) { + public void setDoor(DoorPos.DoorPosition pos) { if (pos == DoorPos.DoorPosition.OPEN) { dServo.setPosition(doorOpenpos); } else if (pos == DoorPos.DoorPosition.CLOSE) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Hieght.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Height.java similarity index 84% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Hieght.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Height.java index 3a28e4d..5a3dd49 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Hieght.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Height.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.teamcode.hardware; -public class Hieght { +public class Height { public enum height { ASCEND, HOLD, DESCEND, RESET } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java index 40019ec..afd28e0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java @@ -76,6 +76,10 @@ public class Intake { dcMotor.setPower(pwr); } + public double getPower() { + return dcMotor.getPower(); + } + public String getTelemetry() { return "lServo: "+lServo.getPosition()+"rServo: "+rServo.getPosition()+"dcMotor: "+dcMotor.getPower(); } 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 a9db0b0..6d9e1ee 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,6 +1,8 @@ package org.firstinspires.ftc.teamcode.hardware; import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.UP; +import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.CLOSE; +import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.OPEN; import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -41,6 +43,7 @@ public class Robot { case(0): macroStartTime = runTime; slides.setTarget(slidePos); + arm.setDoor(CLOSE); macroState ++; break; case(1): @@ -63,16 +66,27 @@ public class Robot { switch(macroState) { case(0): macroStartTime = runTime; - arm.setArmPos(Arm.Position.INTAKE); - arm.setWristPos(Arm.Position.INTAKE); + arm.setDoor(OPEN); macroState++; break; + //Ind_sleeper case(1): - if (runTime > macroStartTime + 1) { + if (runTime > macroStartTime + 2) { macroState ++; } break; case(2): + macroStartTime = runTime; + arm.setArmPos(Arm.Position.INTAKE); + arm.setWristPos(Arm.Position.INTAKE); + macroState++; + break; + case(3): + if (runTime > macroStartTime + 1.1) { + macroState ++; + } + break; + case(4): macroStartTime = runTime; slides.setTarget(pos); macroState = 0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueRightAuto_Drivepathonly_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueRightAuto_Drivepathonly_.java new file mode 100644 index 0000000..d2e127f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueRightAuto_Drivepathonly_.java @@ -0,0 +1,79 @@ +package org.firstinspires.ftc.teamcode.opmode.autonomous; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +@Autonomous(name = "BlueRightAuto(Drive only)") +public class BlueRightAuto_Drivepathonly_ extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); +// add more trajectories here + Pose2d start = new Pose2d(-24, 61.5, Math.toRadians(-90)); + +// Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90)); + Pose2d dropMiddle = new Pose2d(-24, 40.5, Math.toRadians(-180)); +// Pose2d dropRight = new Pose2d(24, 60, Math.toRadians(-90)); + + Pose2d stack1 = new Pose2d(-68, 40.5, Math.toRadians(-180)); + + Pose2d dropMiddle2 = new Pose2d(-24, 40.5, Math.toRadians(-180)); + + Pose2d bmid = new Pose2d(-24, 5, Math.toRadians(-180)); + + Pose2d bmid2 = new Pose2d(24, 5, Math.toRadians(-180)); + + Pose2d alimb = new Pose2d(60, 40.5, Math.toRadians(-180)); + + Pose2d score = new Pose2d(60, 36, Math.toRadians(180)); + + drive.setPoseEstimate(start); +// add this per trajectories + Trajectory scorePurple = drive.trajectoryBuilder(start) + .lineToLinearHeading(dropMiddle) + .build(); + + Trajectory stack= drive.trajectoryBuilder(dropMiddle) + .lineToLinearHeading(stack1) + .build(); + + Trajectory back_to_mid= drive.trajectoryBuilder(stack1) + .lineToLinearHeading(dropMiddle2) + .build(); + + Trajectory front_gate= drive.trajectoryBuilder(dropMiddle2) + .lineToLinearHeading(bmid) + .build(); + + Trajectory front_gate_pt_2= drive.trajectoryBuilder(bmid) + .lineToLinearHeading(bmid2) + .build(); + + Trajectory about_to_score= drive.trajectoryBuilder(bmid2) + + + .lineToLinearHeading(alimb) + .build(); + +// the scorePurple. should be whatever the start pose2d thing was + Trajectory scoreYellow = drive.trajectoryBuilder(scorePurple.end()) + .lineToLinearHeading(score) + .build(); + + waitForStart(); + + if(isStopRequested()) return; +// add this per trajectories + drive.followTrajectory(scorePurple); + drive.followTrajectory(stack); + drive.followTrajectory(back_to_mid); + drive.followTrajectory(front_gate); + drive.followTrajectory(front_gate_pt_2); + drive.followTrajectory(about_to_score); + drive.followTrajectory(scoreYellow); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto_ignore.java similarity index 85% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto_ignore.java index 0a7d9da..9881292 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto_ignore.java @@ -8,11 +8,11 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; @Autonomous(name = "Simple Blue Left Auto") -public class SimpleBlueLeftAuto extends LinearOpMode { +public class SimpleBlueLeftAuto_ignore extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - +// add more trajectories here Pose2d start = new Pose2d(24, 61.5, Math.toRadians(-90)); // Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90)); @@ -22,11 +22,11 @@ public class SimpleBlueLeftAuto extends LinearOpMode { Pose2d score = new Pose2d(60, 36, Math.toRadians(180)); drive.setPoseEstimate(start); - +// add this per trajectories Trajectory scorePurple = drive.trajectoryBuilder(start) .lineToLinearHeading(dropMiddle) .build(); - +// the scorePurple. should be whatever the start pose@d thing was Trajectory scoreYellow = drive.trajectoryBuilder(scorePurple.end()) .lineToLinearHeading(score) .build(); @@ -34,7 +34,7 @@ public class SimpleBlueLeftAuto extends LinearOpMode { waitForStart(); if(isStopRequested()) return; - +// add this per trajectories drive.followTrajectory(scorePurple); // drive.wait(1); drive.followTrajectory(scoreYellow); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/NewTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/NewTeleop.java index e9b3f04..3cc2a1e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/NewTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/NewTeleop.java @@ -1,10 +1,12 @@ package org.firstinspires.ftc.teamcode.opmode.teleop; +import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.CLOSE; +import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.OPEN; + import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.teamcode.controller.Controller; import org.firstinspires.ftc.teamcode.hardware.Intake; @@ -63,7 +65,7 @@ public class NewTeleop extends OpMode { // macros switch (robot.runningMacro) { - case(0): // manual mode + case (0): // manual mode robot.slides.increaseTarget(controller2.getLeftStick().getY()); if (controller2.getX().isJustPressed()) { robot.runningMacro = 1; @@ -74,17 +76,22 @@ public class NewTeleop extends OpMode { } else if (controller2.getA().isJustPressed()) { robot.runningMacro = 4; } + if (robot.intake.getPower() >= 0.01) { + robot.arm.setDoor(OPEN); + } else { + robot.arm.setDoor(CLOSE); + } break; - case(1): + case (1): robot.extendMacro(Slides.Position.TIER1, getRuntime()); break; - case(2): + case (2): robot.extendMacro(Slides.Position.TIER2, getRuntime()); break; - case(3): + case (3): robot.extendMacro(Slides.Position.TIER3, getRuntime()); break; - case(4): + case (4): robot.resetMacro(Slides.Position.DOWN, getRuntime()); break; }