From 5ba6de512f2f1cae32392527bca7232d8efe9bae Mon Sep 17 00:00:00 2001 From: UntitledRice Date: Thu, 16 Nov 2023 16:56:51 -0600 Subject: [PATCH] added opmode jsut for drivebase to driver pratice one at a time moved turbo to driver A button and works --- .../ftc/teamcode/hardware/Arm.java | 19 +- .../ftc/teamcode/hardware/Camera.java | 3 - .../ftc/teamcode/hardware/Intake.java | 2 +- .../{Slides.java => KhangSlides.java} | 4 +- .../ftc/teamcode/hardware/Robot.java | 73 ++- .../ftc/teamcode/hardware/SlidePos.java | 2 +- .../ftc/teamcode/hardware/robby/Slides.java | 126 +++++ .../opmode/teleop/AbstractTeleOp.java | 448 +++++++++++----- .../ftc/teamcode/opmode/teleop/BlueTeam.java | 11 + .../ftc/teamcode/opmode/teleop/Drivebase.java | 47 ++ .../opmode/teleop/ExperimentalKhangMain.java | 506 +++++++++--------- .../{KhangMain.java => Meet1TeleOp.java} | 38 +- .../teamcode/opmode/teleop/PoseStorage.java | 9 + .../ftc/teamcode/opmode/teleop/RedTeam.java | 11 + .../ftc/teamcode/opmode/teleop/Team.java | 6 + .../teamcode/vision/TargetingPipeline.java | 3 - 16 files changed, 874 insertions(+), 434 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/{Slides.java => KhangSlides.java} (98%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/robby/Slides.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeam.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Drivebase.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/{KhangMain.java => Meet1TeleOp.java} (89%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/PoseStorage.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeam.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Team.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 befaac3..9382cde 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 @@ -5,6 +5,8 @@ import com.arcrobotics.ftclib.controller.PController; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.teamcode.hardware.robby.Slides; + @Config public class Arm { @@ -13,11 +15,16 @@ public class Arm { private Servo rAServo; private Servo lAServo; private Servo wristServo; - private SlidePos.SPosition pos = SlidePos.SPosition.DOWN; + private Slides.Position pos = Slides.Position.DOWN; private PController armController; private double armControllerTarget; private double ARM_KP = 0.001; + + public enum APosition { + SDOWN, SCORE, SAFTEYDOWN, SAFTEYUP; + } + public Arm(HardwareMap hardwareMap) { wristServo = hardwareMap.get(Servo.class, "Wrist Servo"); dServo = hardwareMap.get(Servo.class, "Door Servo"); @@ -47,22 +54,22 @@ public class Arm { } } - public void setPos(ArmPos.APosition tape) { - if (tape == ArmPos.APosition.SDOWN) { + public void setPos(APosition tape) { + if (tape == APosition.SDOWN) { this.armControllerTarget = startingArmPos; lAServo.setPosition(startingArmPos); rAServo.setPosition(startingArmPos); wristServo.setPosition(startingWristPos); - } else if (tape == ArmPos.APosition.SCORE) { + } else if (tape == APosition.SCORE) { this.armControllerTarget = armScorePos; lAServo.setPosition(armScorePos); rAServo.setPosition(armScorePos); wristServo.setPosition(wristScorePos); - } else if (tape == ArmPos.APosition.SAFTEYDOWN) { + } else if (tape == APosition.SAFTEYDOWN) { lAServo.setPosition(safteyDownPos); rAServo.setPosition(safteyDownPos); wristServo.setPosition(wristScorePos); - } else if (tape == ArmPos.APosition.SAFTEYUP) { + } else if (tape == APosition.SAFTEYUP) { lAServo.setPosition(safteyUpPos); rAServo.setPosition(safteyUpPos); wristServo.setPosition(wristScorePos); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java index 04f617d..9465b6f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java @@ -82,9 +82,6 @@ public class Camera { return (targetingCameraInitialized ? targetingPipeline.getBlue() : INVALID_DETECTION); } - public Detection getBlack() { - return (targetingCameraInitialized ? targetingPipeline.getBlack() : INVALID_DETECTION); - } //return frame rate public int getFrameCount() { 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 3c8be72..215d325 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 @@ -34,7 +34,7 @@ public class Intake { public static double up = 0.30; public static double motorPower = 0; - public Intake(HardwareMap hardwareMap) { + public Intake(HardwareMap hardwareMap, Position up) { lServo = hardwareMap.get(Servo.class, "Left Servo"); lServo.setDirection(Servo.Direction.REVERSE); rServo = hardwareMap.get(Servo.class, "Right Servo"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/KhangSlides.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/KhangSlides.java index 52d06b5..ef90406 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/KhangSlides.java @@ -6,10 +6,10 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; @Config -public class Slides { +public class KhangSlides { //Create and assign motors - public Slides(HardwareMap hardwareMap) { + public KhangSlides(HardwareMap hardwareMap) { rSMotor = hardwareMap.get(DcMotor.class, "Right Slide Motor"); // rSMotor.setDirection(DcMotorSimple.Direction.REVERSE); rSMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); 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 4b993e3..61e0a75 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,5 +1,7 @@ package org.firstinspires.ftc.teamcode.hardware; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.UP; + import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -13,18 +15,23 @@ public class Robot { public SampleMecanumDrive drive; public Camera camera; public Intake intake; - public Slides slides; + public KhangSlides slides; public Arm arm; + public double macroStartTime = 0; + public int macroState = 0; + public int runningMacro = 0; // 0 = no macro | 1 = low macro | 2 = mid macro | 3 = high macro | 4 = pickup macro + public int lastMacro = 0; + private boolean camEnabled = true; //Name the objects public Robot(HardwareMap hardwareMap) { arm = new Arm(hardwareMap); drive = new SampleMecanumDrive(hardwareMap); - slides = new Slides(hardwareMap); + slides = new KhangSlides(hardwareMap); camera = new Camera(hardwareMap); camera.initTargetingCamera(); - intake = new Intake(hardwareMap); + intake = new Intake(hardwareMap, UP); camEnabled = true; } @@ -39,6 +46,66 @@ public class Robot { return String.format("position: %s", slide.getCurrentPosition()); } +// public void resetMacroNoDunk(int pos, double runTime) { +// switch(macroState) { +// case(0): +// macroStartTime = runTime; +// macroState++; +// break; +// case(1): +// if (runTime > macroStartTime) { +// macroState ++; +// } +// break; +// case(2): +// macroStartTime = runTime; +// slides.setTarget(pos); +// macroState = 0; +// runningMacro = 0; +// lastMacro = 0; +// } +// } +// +// public void resetMacro(int pos, double runTime) { +// switch(macroState) { +// case(0): +// macroStartTime = runTime; +// macroState++; +// break; +// case(1): +// if (runTime > macroStartTime) { +// macroState ++; +// } +// break; +// case(2): +// macroStartTime = runTime; +// slides.setTarget(pos); +// macroState = 0; +// runningMacro = 0; +// lastMacro = 0; +// } +// } +// +// public void resetMacroEnd(int pos, double runTime) { +// switch(macroState) { +// case(0): +// macroStartTime = runTime; +// macroState++; +// break; +// case(1): +// if (runTime > macroStartTime) { +// macroState ++; +// } +// break; +// case(2): +// macroStartTime = runTime; +// slides.setTarget(pos); +// macroState = 0; +// runningMacro = 0; +// lastMacro = 0; +// } +// } + public Arm getArm() { return this.arm; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SlidePos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SlidePos.java index 286a6f1..a4f944c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SlidePos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SlidePos.java @@ -3,6 +3,6 @@ package org.firstinspires.ftc.teamcode.hardware; public class SlidePos { public enum SPosition { - DOWN, TAPE1, TAPE2, TAPE3, MAX; + DOWN, TAPE1, TAPE2, TAPE3, MAX, MANUAL; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/robby/Slides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/robby/Slides.java new file mode 100644 index 0000000..2b17e37 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/robby/Slides.java @@ -0,0 +1,126 @@ +package org.firstinspires.ftc.teamcode.hardware.robby; + +import com.acmerobotics.dashboard.config.Config; +import com.arcrobotics.ftclib.controller.PIDController; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +@Config +public class Slides { + private DcMotor slide; + private DcMotor slide2; + + public static double p = 0.0014; + public static double i = 0.02; + public static double d = 0; + public static double f = 0.01; + public static double pTolerance = 20; + public static PIDController controller = new PIDController(p, i, d); + + public static int[] heights = {0, (int)(190/4.0), 2*(int)(190/4.0), 3*(int)(190/4.0), 4*(int)(180/4)}; + + public static int heightOffset = 0; + public static int targetMin = -10; + public static int targetMax = 770; + + public static int highPos = 720 + heightOffset; // ALSO DEFINED IN UPDATE SLIDES + public static int midPos = 350 + heightOffset; // ALSO DEFINED IN UPDATE SLIDES + public static int lowPos = heightOffset; // ALSO DEFINED IN UPDATE SLIDES + public static int pickupPos = 220 + heightOffset; // ALSO DEFINED IN UPDATE SLIDES + public static int downPos = heightOffset; + + private int target = 0; + + public static int manualSpeed = 20; + public static int zeroPower = 5; + + public enum Position { HIGH, MEDIUM, LOW, PICKUP, DOWN } + + public Slides(HardwareMap hardwareMap) { + slide = hardwareMap.get(DcMotor.class, "slide"); + slide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + slide.setMode(DcMotor.RunMode.RUN_USING_ENCODER); +// slide.setDirection(DcMotorSimple.Direction.REVERSE); + slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + slide2 = hardwareMap.get(DcMotor.class, "slide2"); + slide2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + slide2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + slide2.setDirection(DcMotorSimple.Direction.REVERSE); + slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + public void setTarget(int pos) { + target = Math.min(Math.max(pos, targetMin), targetMax); + } + + public void setTarget(Position pos) { + if (pos == Position.HIGH) { + target = Math.min(Math.max(highPos, targetMin), targetMax); + } else if (pos == Position.MEDIUM) { + target = Math.min(Math.max(midPos, targetMin), targetMax); + } else if (pos == Position.LOW) { + target = Math.min(Math.max(lowPos, targetMin), targetMax); + } else if (pos == Position.PICKUP) { + target = Math.min(Math.max(pickupPos, targetMin), targetMax); + } else if (pos == Position.DOWN) { + target = Math.min(Math.max(downPos, targetMin), targetMax); + } + } + + public void increaseTarget(double increase) { + target += (int) (increase * manualSpeed); + target = Math.min(targetMax, Math.max(targetMin, target)); + } + + public int getTarget() { + return target; + } + + public boolean atTarget() { + return controller.atSetPoint(); + } + + public void cancel() { + target = slide.getCurrentPosition(); + } + + public void targetReset() { + target = targetMin; + } + + public void update(double runTime) { +// highPos = 720 + heightOffset; +// midPos = 350 + heightOffset; +// lowPos = heightOffset; +// pickupPos = 20 + heightOffset; +// downPos = heightOffset;// TODO add these back in + +// if (target == 0) { +// slide.setPower(0); +// slide2.setPower(0); +// } else { +// if (target < 5) { +// slide.setPower(0); +// slide2.setPower(0); +// } else { + double pid, ff; + controller.setPID(p, i, d); + controller.setTolerance(pTolerance); + + pid = controller.calculate(slide.getCurrentPosition(), target); + ff = f; + slide.setPower(pid + ff); + + pid = controller.calculate(slide2.getCurrentPosition(), target); + ff = f; + slide2.setPower(pid + ff); +// } +// } + } + + 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(), heightOffset); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/AbstractTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/AbstractTeleOp.java index d0dda3d..b3a91f1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/AbstractTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/AbstractTeleOp.java @@ -1,150 +1,304 @@ -package org.firstinspires.ftc.teamcode.opmode.teleop; - -import static java.lang.Math.PI; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; - -import org.firstinspires.ftc.teamcode.controller.Controller; -import org.firstinspires.ftc.teamcode.hardware.Robot; - -@Config -public abstract class AbstractTeleOp extends OpMode { - private Robot robot; - Controller driver1; - Controller driver2; - - public static double drivebaseThrottle = 0.6; - public static double drivebaseTurbo = 1.0; - public static int heightIncrement = 20; - - Pose2d robot_pos; - double robot_x, robot_y, robot_heading; - - // auto align variables - double headingPID; - public static double headingP = 0.01; - public static double headingI = 0.03; - public static double headingD = 0.0005; - - double strafePID; - public static double strafeP = 0.05; - public static double strafeI = 0; - public static double strafeD = 0.01; - - double robot_y_pos; - boolean fixed90Toggle = false; - boolean fixed0Toggle = false; - -// public static double robot_width = 12; -// public static double robot_length = 12; -// public static double robot_radius = 6; +//package org.firstinspires.ftc.teamcode.opmode.teleop; // -// public static double groundJuncRadius = 6; -// public static double coneRadius = 4; +//import static org.firstinspires.ftc.teamcode.opmode.teleop.Team.BLUE; +//import static org.firstinspires.ftc.teamcode.opmode.teleop.Team.RED; +//import static java.lang.Math.PI; // -// public static double armWait = 0.2; - -// private double timeSinceOpened = 0; // for claw -// private double timeSinceClosed = 0; - -// private int delayState = 0; // for arm -// private double delayStart = 0; -// private boolean doArmDelay = false; // for arm - private boolean isAutoClose = true; - - @Override - public void init() { - robot = new Robot(hardwareMap); - driver1 = new Controller(gamepad1); - driver2 = new Controller(gamepad2); - } - - @Override - public void init_loop() { -// if (robot.camera.getFrameCount() < 1) { -// telemetry.addLine("Initializing Robot..."); -// } else { -// telemetry.addLine("Initialized"); +//import com.acmerobotics.dashboard.config.Config; +//import com.acmerobotics.roadrunner.geometry.Pose2d; +//import com.arcrobotics.ftclib.controller.PIDController; +//import com.qualcomm.robotcore.eventloop.opmode.OpMode; +// +//import org.firstinspires.ftc.teamcode.controller.Controller; +//import org.firstinspires.ftc.teamcode.hardware.Arm; +//import org.firstinspires.ftc.teamcode.hardware.Robot; +//import org.firstinspires.ftc.teamcode.hardware.robby.Slides; +// +//@Config +//public abstract class AbstractTeleOp extends OpMode { +// private Robot robot; +// public Team team; +// Controller driver1; +// Controller driver2; +// +// public static double normal = 0.5; +// public static double vazrooooming = 1.0; +// public static int heightIncrement = 20; +// +// Pose2d robot_pos; +// double robot_x, robot_y, robot_heading; +// +// // auto align variables +// double headingPID; +// public static double headingP = 0.01; +// public static double headingI = 0.03; +// public static double headingD = 0.0005; +// public static PIDController headingController = new PIDController(headingP, headingI, headingD); +// +// double strafePID; +// public static double strafeP = 0.05; +// public static double strafeI = 0; +// public static double strafeD = 0.01; +// public static PIDController strafeController = new PIDController(strafeP, strafeI, strafeD); +// +// double robot_y_pos; +// boolean fixed90Toggle = false; +// boolean fixed0Toggle = false; +// +//// public static double robot_width = 12; +//// public static double robot_length = 12; +//// public static double robot_radius = 6; +//// +//// public static double groundJuncRadius = 6; +//// public static double coneRadius = 4; +//// +//// public static double armWait = 0.2; +// +//// private double timeSinceOpened = 0; // for claw +//// private double timeSinceClosed = 0; +// +// // private int delayState = 0; // for arm +//// private double delayStart = 0; +//// private boolean doArmDelay = false; // for arm +// private boolean isAutoClose = true; +// +// @Override +// public void init() { +// robot = new Robot(hardwareMap); +// driver1 = new Controller(gamepad1); +// driver2 = new Controller(gamepad2); +// if (team == RED) { +// driver1.setColor(255, 0, 0); +// driver2.setColor(255, 0, 0); +// } else if (team == BLUE) { +// driver1.setColor(0, 0, 255); +// driver2.setColor(0, 0, 255); // } - telemetry.addLine("Initialized"); - telemetry.addLine(robot.getTelemetry()); - telemetry.update(); - } - - private int getQuadrant(double angle) { - if (0 < angle && angle < PI/2.0) { - return 1; - } else if (PI/2.0 < angle && angle < PI) { - return 2; - } else if (PI < angle && angle < 3*PI/2.0) { - return 3; - } else { - return 4; - } - } - - @Override - public void loop() { - // robot position update - //robot_pos = robot.drive.getPoseEstimate(); - //robot_x = robot_pos.getX(); - //robot_y = robot_pos.getY(); - //robot_heading = robot_pos.getHeading(); // in radians - - // driver 1 controls - //driver1.update(); - //driver2.update(); - double x = -driver1.getLeftStick().getY(); - double y = driver1.getLeftStick().getX(); - double z = -driver1.getRightStick().getX(); - -// figure out if a toggle is present - if (driver1.getRightBumper().isPressed()) { - if (!fixed90Toggle) { - //robot_y_pos = robot.drive.getPoseEstimate().getX(); - } - fixed90Toggle = true; - } else { - fixed90Toggle = false; - } - - if (!fixed90Toggle && driver1.getLeftBumper().isPressed()) { - if (!fixed0Toggle) { - //robot_y_pos = robot.drive.getPoseEstimate().getY(); - } - fixed0Toggle = true; - } else { - fixed0Toggle = false; - } - - telemetry.addLine("Wanted Position: "+robot_y_pos); - telemetry.addLine("Actual Position: "+robot_y); - telemetry.addLine("PID: "+strafePID); - - // turbo - if (driver1.getLeftBumper().isPressed() || driver1.getRightBumper().isPressed()) { - x *= drivebaseTurbo; - y *= drivebaseTurbo; - z *= drivebaseTurbo; - } else { - x *= drivebaseThrottle; - y *= drivebaseThrottle; - z *= drivebaseThrottle; - } - - // actually set power - if (fixed0Toggle || fixed90Toggle) { - //robot.drive.setWeightedDrivePower(new Pose2d(x, 0, headingPID)); - } else { - //robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); - } - - // update and telemetry - //robot.update(getRuntime()); - - telemetry.addLine(robot.getTelemetry()); - telemetry.update(); - } -} +// +// PoseStorage.AutoJustEnded = false; +// } +// +// @Override +// public void init_loop() { +// ////init when camera is on and responding +//// if (robot.camera.getFrameCount() < 1) { +//// telemetry.addLine("Initializing Robot..."); +//// } else { +//// telemetry.addLine("Initialized"); +//// } +// telemetry.addLine("Initialized"); +// telemetry.addLine(robot.getTelemetry()); +// telemetry.update(); +// } +// +// //find out what quarter you are in +// private int getQuadrant(double angle) { +// if (0 < angle && angle < PI/2.0) { +// return 1; +// } else if (PI/2.0 < angle && angle < PI) { +// return 2; +// } else if (PI < angle && angle < 3*PI/2.0) { +// return 3; +// } else { +// return 4; +// } +// } +// +// @Override +// public void loop() { +// // robot position update +// robot_pos = robot.drive.getPoseEstimate(); +// robot_x = robot_pos.getX(); +// robot_y = robot_pos.getY(); +// robot_heading = robot_pos.getHeading(); // in radians +// +// // driver 1 controls +// driver1.update(); +// driver2.update(); +// +// //create and set X, Y, and Z for drive inputs +// double x = -gamepad1.left_stick_y; +// double y = -gamepad1.left_stick_x; +// double z = -gamepad1.right_stick_x; +// +//// figure out if a toggle is present +// if (driver1.getRightBumper().isPressed()) { +// if (!fixed90Toggle) { +// robot_y_pos = robot.drive.getPoseEstimate().getX(); +// } +// fixed90Toggle = true; +// } else { +// fixed90Toggle = false; +// } +// +// if (!fixed90Toggle && driver1.getLeftBumper().isPressed()) { +// if (!fixed0Toggle) { +// robot_y_pos = robot.drive.getPoseEstimate().getY(); +// } +// fixed0Toggle = true; +// } else { +// fixed0Toggle = false; +// } +// +// // heading pid +// headingController.setPID(headingP, headingI, headingD); +// int quadrant = getQuadrant(robot_heading); +// if (fixed0Toggle) { +// switch (quadrant) { +// case 1: +// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 0);//- right +// break; +// case 2: +// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 180);//+ left +// break; +// case 3: +// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 180);//- right +// break; +// case 4: +// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 360);//+ left +// break; +// } +// } else if (fixed90Toggle) { +// switch (quadrant) { +// case 1: +// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 90);//- right +// break; +// case 2: +// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 90);//+ left +// break; +// case 3: +// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 270);//- right +// break; +// case 4: +// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 270);//+ left +// break; +// } +// } else { +// headingPID = 0; +// } +// +// // strafe pid +// strafeController.setPID(strafeP, strafeI, strafeD); +// if (fixed0Toggle) { +// strafePID = strafeController.calculate(robot_y, robot_y_pos); +// } else if (fixed90Toggle) { +// strafePID = strafeController.calculate(robot_x, robot_y_pos); +// } else { +// strafePID = 0; +// } +// +// telemetry.addLine("Wanted Position: "+robot_y_pos); +// telemetry.addLine("Actual Position: "+robot_y); +// telemetry.addLine("PID: "+strafePID); +// +// // turbo +// if (driver1.getLeftBumper().isPressed() || driver1.getRightBumper().isPressed()) { +// x *= vazrooooming; +// y *= vazrooooming; +// z *= vazrooooming; +// } else { +// x *= normal; +// y *= normal; +// z *= normal; +// } +// +// // actually set power +// if (fixed0Toggle || fixed90Toggle) { +// robot.drive.setWeightedDrivePower(new Pose2d(x, 0, headingPID)); +// } else { +// robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); +// } +// +// // driver 2 controls +// // increment heights +// if (driver2.getDUp().isJustPressed()) { +// Slides.heightOffset += heightIncrement; +// robot.slides.setTarget(robot.slides.getTarget() + heightIncrement); +// } else if (driver2.getDDown().isJustPressed()) { +// Slides.heightOffset -= heightIncrement; +// robot.slides.setTarget(robot.slides.getTarget() - heightIncrement); +// } +// +// switch (robot.runningMacro) { +// case(0): // manual mode +// +// //track pad is rad +// // left right dpad is turn wrist +// robot.slides.increaseTarget(driver2.getLeftStick().getY()); +//// if (Math.abs(driver2.getRightStick().getY()) > 0.05) { // close claw if anything is moved +//// robot.claw.close(); +//// } +// +//// if (driver2.getRightBumper().isJustPressed()) { +//// robot.arm.goToScore(); +//// } +//// if (driver2.getLeftBumper().isJustPressed()) { +//// robot.arm.goToIntake(); +//// } +// +// // retract all the time +// if (driver2.getLeftStickButton().isJustPressed()) { +// robot.runningMacro = 5; +// } +// +// // to cancel macro and allow manual movement +//// if (driver2.getLeftStick().getY() > 0.05 || driver2.getRightStick().getY() > 0.05) { +//// robot.runningMacro = 0; +//// robot.lastMacro = 0; +//// } +// +// if (driver2.getTouchpad().isJustPressed()) { +// robot.runningMacro = 4; +// } else if (driver2.getX().isJustPressed()) { // high position [closed, bring up, bring out] +// robot.runningMacro = 3; +// } else if (driver2.getY().isJustPressed()) { // middle position [middle goal level] +// robot.runningMacro = 2; +// } else if (driver2.getB().isJustPressed() && !driver2.getStart().isJustPressed()) { // low position [low goal level] +// robot.runningMacro = 1; +// } else if (driver2.getA().isJustPressed()) { +// if (robot.lastMacro == 0 || robot.lastMacro == 4) { // if not running any macros or picking up cones +// } else { // otherwise, I need to undo a macro +// robot.runningMacro = 5; +// } +// } else { +// if (driver2.getRightBumper().isJustPressed()) { +// isAutoClose = false; +// } +// if (driver2.getLeftBumper().isJustPressed()) { +// } +// } +// break; +// case(1): +//// robot.extendMacro(Slides.Position.LOW, Arm.APosition.SCORE, getRuntime()); +// break; +// case(2): +//// robot.extendMacro(Slides.Position.MEDIUM, Arm.APosition.SCORE, getRuntime()); +// break; +// case(3): +//// robot.extendMacro(Slides.Position.HIGH, Arm.APosition.SCORE, getRuntime()); +// break; +// case(4): +//// robot.extendMacro(Slides.Position.PICKUP, Arm.APosition.SDOWN, getRuntime()); +// break; +// case (5): // macro reset +// robot.resetMacroNoDunk(0, getRuntime()); +// driver1.rumble(100); +// break; +// } +// +// // cancel the macros +// if (driver2.getRightStickButton().isJustPressed()) { +// robot.runningMacro = 0; +// robot.lastMacro = 0; +// robot.macroState = 0; +// robot.slides.cancel(); +// } +// +// // update and telemetry +// robot.update(getRuntime()); +// +// telemetry.addLine(robot.getTelemetry()); +// telemetry.addLine(String.format("Last Macro: %s\nRunning Macro: %s\nMacroState: %s", robot.lastMacro, robot.runningMacro, robot.macroState)); +// telemetry.update(); +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeam.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeam.java new file mode 100644 index 0000000..cc710f1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeam.java @@ -0,0 +1,11 @@ +//package org.firstinspires.ftc.teamcode.opmode.teleop; +//import com.qualcomm.robotcore.eventloop.opmode.Disabled; +//import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +// +//@TeleOp(name = "Opmode For Team Blue", group = "Competition") +//public class BlueTeam extends AbstractTeleOp { +// +// public BlueTeam() { +// team = Team.BLUE; +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Drivebase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Drivebase.java new file mode 100644 index 0000000..ed6edda --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Drivebase.java @@ -0,0 +1,47 @@ +package org.firstinspires.ftc.teamcode.opmode.teleop; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +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.Robot; + +@TeleOp(name = "Drivebase Only", group = "OpModes") +public class Drivebase extends OpMode { + //turbo mode + public static double normal = 0.5; + public static double turbo = 1; + //create robot instance + private Robot robot; + //create controller 1 (driver) + private Controller controller1; + @Override + public void init() { + this.robot = new Robot(hardwareMap); + controller1 = new Controller(gamepad1); + telemetry.addLine("Started"); + //update to make sure we receive last line of code + telemetry.update(); + } + + @Override + public void loop() { + //create and set X, Y, and Z for drive inputs + double x = -gamepad1.left_stick_y; + double y = -gamepad1.left_stick_x; + double z = -gamepad1.right_stick_x; + robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); + + //turbo activation + if (controller1.getA().isJustPressed()){ + x *= turbo; + y *= turbo; + z *= turbo; + } else if (controller1.getA().isJustReleased()){ + x *= normal; + y *= normal; + z *= normal; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/ExperimentalKhangMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/ExperimentalKhangMain.java index 69ef3f4..198019b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/ExperimentalKhangMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/ExperimentalKhangMain.java @@ -1,259 +1,257 @@ -package org.firstinspires.ftc.teamcode.opmode.teleop; - -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.hardware.Servo; -import com.qualcomm.robotcore.util.ElapsedTime; - -import org.firstinspires.ftc.teamcode.controller.Controller; -import org.firstinspires.ftc.teamcode.hardware.ArmPos; -import org.firstinspires.ftc.teamcode.hardware.DoorPos; -import org.firstinspires.ftc.teamcode.hardware.Down; -import org.firstinspires.ftc.teamcode.hardware.Hieght; -import org.firstinspires.ftc.teamcode.hardware.HopperPos; -import org.firstinspires.ftc.teamcode.hardware.Intake; -import org.firstinspires.ftc.teamcode.hardware.Robot; -import org.firstinspires.ftc.teamcode.hardware.SlidePos; - -@Config -@TeleOp(name = "experimental opmode", group = "OpModes") -public class ExperimentalKhangMain extends OpMode { - - //keep track of runtime for advanced macros - private ElapsedTime runTime = new ElapsedTime(); - - //create and set start position of intake - private Intake.Position Currentpos = Intake.Position.UP; - //inform if slides are in lowest position or not - private Down.DownArm DownQuestion = Down.DownArm.YES; - //create and set start position of slides - private SlidePos.SPosition CurrentSpos = SlidePos.SPosition.DOWN; - //create and set start position of arm - private ArmPos.APosition CurrentApos = ArmPos.APosition.SDOWN; - //create and set start position of door, default close - private DoorPos.DoorPosition CurrentDpos = DoorPos.DoorPosition.CLOSE; - //create and set start position of hopper - private HopperPos.hopperPos hopperpos = HopperPos.hopperPos.DOWN; - //create manual slide height varable and set it to hold position as start - private Hieght.height CurrentHeight = Hieght.height.HOLD; - - //robot position stuff - Pose2d robot_pos; - double robot_x, robot_y, robot_heading; - - //create robot instance - private Robot robot; - //create servo for plane - private Servo planeServo; - //create controller 1 (driver) - private Controller controller1; - //create controller 2 (macro user and one with hard life) - private Controller controller2; - //set starting slide height to 0 - private double sHeight = 0; - - @Override - public void init() { - - //reset runtime when opmode is initialized - runTime.reset(); - - //make each servo, motor, and controller and configure them - planeServo = hardwareMap.get(Servo.class, "Plane Servo"); - this.robot = new Robot(hardwareMap); - controller1 = new Controller(gamepad1); - controller2 = new Controller(gamepad2); - //feedback to driver hub to know if init was successful - telemetry.addLine("Started"); - //update to amke sure we receive last lien of code - telemetry.update(); - } - - @Override - public void loop() { - - // robot position update - robot_pos = robot.drive.getPoseEstimate(); - robot_x = robot_pos.getX(); - robot_y = robot_pos.getY(); - robot_heading = robot_pos.getHeading(); // in radians - - // Calculate the runtime in seconds - double currentTime = runTime.seconds(); - - //set start of macro runtime - double macroStartTime = 0; - - //create and set X, Y, and Z for drive inputs - double x = -gamepad1.left_stick_y; - double y = -gamepad1.left_stick_x; - double z = -gamepad1.right_stick_x; - robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); - - //set intake to be pressure reactant to right trigger - robot.intake.setDcMotor(gamepad2.right_trigger); - //manual slide height control - double sHeight = gamepad2.right_stick_y; - //activate intake or not - double intakeON = gamepad2.right_trigger; - //graph input of Y joystick to make macros for slides - double sY = -gamepad2.left_stick_y; - //graph of X - double sX = gamepad2.left_stick_x; - - //reset value to set slides to starting value - double reset = gamepad2.left_trigger; - - //variable to determine shoot drone or not - double shoot = gamepad1.right_trigger; - - //update variables to constantly feed position each component should be in - //intake - robot.intake.setpos(Currentpos); - //slide macro - robot.slides.setSPos(CurrentSpos); - //slide manual -// robot.slides.setHeight(CurrentHeight); - //arm position - robot.arm.setPos(CurrentApos); - //door open or not - robot.arm.openDoor(CurrentDpos); - //update - controller2.update(); - - //manual slide input reader -// if (sHeight >= 0.2) { -// CurrentHeight = Hieght.height.ASCEND; -// } else if (sHeight <= -0.2) { -// CurrentHeight = Hieght.height.DESCEND; +//package org.firstinspires.ftc.teamcode.opmode.teleop; +// +//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.hardware.Servo; +//import com.qualcomm.robotcore.util.ElapsedTime; +// +//import org.firstinspires.ftc.teamcode.controller.Controller; +//import org.firstinspires.ftc.teamcode.hardware.DoorPos; +//import org.firstinspires.ftc.teamcode.hardware.Down; +//import org.firstinspires.ftc.teamcode.hardware.Hieght; +//import org.firstinspires.ftc.teamcode.hardware.HopperPos; +//import org.firstinspires.ftc.teamcode.hardware.Intake; +//import org.firstinspires.ftc.teamcode.hardware.Robot; +// +//@Config +//@TeleOp(name = "experimental opmode", group = "OpModes") +//public class ExperimentalKhangMain extends OpMode { +// +// //keep track of runtime for advanced macros +// private ElapsedTime runTime = new ElapsedTime(); +// +// //create and set start position of intake +// private Intake.Position Currentpos = Intake.Position.UP; +// //inform if slides are in lowest position or not +// private Down.DownArm DownQuestion = Down.DownArm.YES; +// //create and set start position of slides +// private SlidePos.SPosition CurrentSpos = SlidePos.SPosition.DOWN; +// //create and set start position of arm +// private ArmPos.APosition CurrentApos = ArmPos.APosition.SDOWN; +// //create and set start position of door, default close +// private DoorPos.DoorPosition CurrentDpos = DoorPos.DoorPosition.CLOSE; +// //create and set start position of hopper +// private HopperPos.hopperPos hopperpos = HopperPos.hopperPos.DOWN; +// //create manual slide height varable and set it to hold position as start +// private Hieght.height CurrentHeight = Hieght.height.HOLD; +// +// //robot position stuff +// Pose2d robot_pos; +// double robot_x, robot_y, robot_heading; +// +// //create robot instance +// private Robot robot; +// //create servo for plane +// private Servo planeServo; +// //create controller 1 (driver) +// private Controller controller1; +// //create controller 2 (macro user and one with hard life) +// private Controller controller2; +// //set starting slide height to 0 +// private double sHeight = 0; +// +// @Override +// public void init() { +// +// //reset runtime when opmode is initialized +// runTime.reset(); +// +// //make each servo, motor, and controller and configure them +// planeServo = hardwareMap.get(Servo.class, "Plane Servo"); +// this.robot = new Robot(hardwareMap); +// controller1 = new Controller(gamepad1); +// controller2 = new Controller(gamepad2); +// //feedback to driver hub to know if init was successful +// telemetry.addLine("Started"); +// //update to amke sure we receive last lien of code +// telemetry.update(); +// } +// +// @Override +// public void loop() { +// +// // robot position update +// robot_pos = robot.drive.getPoseEstimate(); +// robot_x = robot_pos.getX(); +// robot_y = robot_pos.getY(); +// robot_heading = robot_pos.getHeading(); // in radians +// +// // Calculate the runtime in seconds +// double currentTime = runTime.seconds(); +// +// //set start of macro runtime +// double macroStartTime = 0; +// +// //create and set X, Y, and Z for drive inputs +// double x = -gamepad1.left_stick_y; +// double y = -gamepad1.left_stick_x; +// double z = -gamepad1.right_stick_x; +// robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); +// +// //set intake to be pressure reactant to right trigger +// robot.intake.setDcMotor(gamepad2.right_trigger); +// //manual slide height control +// double sHeight = gamepad2.right_stick_y; +// //activate intake or not +// double intakeON = gamepad2.right_trigger; +// //graph input of Y joystick to make macros for slides +// double sY = -gamepad2.left_stick_y; +// //graph of X +// double sX = gamepad2.left_stick_x; +// +// //reset value to set slides to starting value +// double reset = gamepad2.left_trigger; +// +// //variable to determine shoot drone or not +// double shoot = gamepad1.right_trigger; +// +// //update variables to constantly feed position each component should be in +// //intake +// robot.intake.setpos(Currentpos); +// //slide macro +// robot.slides.setSPos(CurrentSpos); +// //slide manual +//// robot.slides.setHeight(CurrentHeight); +// //arm position +// robot.arm.setPos(CurrentApos); +// //door open or not +// robot.arm.openDoor(CurrentDpos); +// //update +// controller2.update(); +// +// //manual slide input reader +//// if (sHeight >= 0.2) { +//// CurrentHeight = Hieght.height.ASCEND; +//// } else if (sHeight <= -0.2) { +//// CurrentHeight = Hieght.height.DESCEND; +//// } else { +//// CurrentHeight = Hieght.height.HOLD; +//// } +// +// //read values to determine if plane should shoot or not +// if (shoot >= 0.9) { +// planeServo.setPosition(0.2); // } else { -// CurrentHeight = Hieght.height.HOLD; +// planeServo.setPosition(0); // } - - //read values to determine if plane should shoot or not - if (shoot >= 0.9) { - planeServo.setPosition(0.2); - } else { - planeServo.setPosition(0); - } - - //make door rise as intake goes on - if (intakeON >= 0.01) { - CurrentDpos = DoorPos.DoorPosition.OPEN; - } else { - CurrentDpos = DoorPos.DoorPosition.CLOSE; - } - - if (intakeON >= 0.35) { - Currentpos = Intake.Position.STACK1; - } else { - Currentpos = Intake.Position.UP; - } - - //reset slide position - if (reset >= 0.2) { - CurrentSpos = CurrentSpos.DOWN; - } - -// //control position of hopper -// if (controller2.getLeftBumper().isJustPressed()) { -// hopperpos = HopperPos.hopperPos.UP; -// } else if (controller2.getLeftBumper().isJustReleased()) { -// hopperpos = HopperPos.hopperPos.DOWN; +// +// //make door rise as intake goes on +// if (intakeON >= 0.01) { +// CurrentDpos = DoorPos.DoorPosition.OPEN; +// } else { +// CurrentDpos = DoorPos.DoorPosition.CLOSE; // } - - //shift intake up one pixel - if (controller2.getDLeft().isJustPressed()) { - //prevent from going higher than highest legal value - if (Currentpos != Intake.Position.UP) { - Currentpos = Currentpos.nextPosition(); - } - } - - //shift intake down one pixel - if (controller2.getDRight().isJustPressed()) { - //prevent from going lower than lowest value - if (Currentpos != Intake.Position.STACK1) { - Currentpos = Currentpos.previousPosition(); - } - } - - //set intake to max position - if (controller2.getDUp().isJustPressed()) { - Currentpos = Currentpos.UP; - } - - //set intake to lowest position - if (controller2.getDDown().isJustPressed()) { - Currentpos = Currentpos.STACK1; - } - -// //arm safety pause going up -// if (controller2.getA().isJustPressed()){ -// CurrentApos = CurrentApos.SAFTEYUP; +// +// if (intakeON >= 0.35) { +// Currentpos = Intake.Position.STACK1; +// } else { +// Currentpos = Intake.Position.UP; +// } +// +// //reset slide position +// if (reset >= 0.2) { +// CurrentSpos = CurrentSpos.DOWN; +// } +// +//// //control position of hopper +//// if (controller2.getLeftBumper().isJustPressed()) { +//// hopperpos = HopperPos.hopperPos.UP; +//// } else if (controller2.getLeftBumper().isJustReleased()) { +//// hopperpos = HopperPos.hopperPos.DOWN; +//// } +// +// //shift intake up one pixel +// if (controller2.getDLeft().isJustPressed()) { +// //prevent from going higher than highest legal value +// if (Currentpos != Intake.Position.UP) { +// Currentpos = Currentpos.nextPosition(); +// } +// } +// +// //shift intake down one pixel +// if (controller2.getDRight().isJustPressed()) { +// //prevent from going lower than lowest value +// if (Currentpos != Intake.Position.STACK1) { +// Currentpos = Currentpos.previousPosition(); +// } +// } +// +// //set intake to max position +// if (controller2.getDUp().isJustPressed()) { +// Currentpos = Currentpos.UP; +// } +// +// //set intake to lowest position +// if (controller2.getDDown().isJustPressed()) { +// Currentpos = Currentpos.STACK1; +// } +// +//// //arm safety pause going up +//// if (controller2.getA().isJustPressed()){ +//// CurrentApos = CurrentApos.SAFTEYUP; +//// DownQuestion = DownQuestion.NO; +//// } +// +// if (controller2.getA().isJustPressed()) { +// macroStartTime = currentTime; +// CurrentSpos = CurrentSpos.TAPE1; +// if (macroStartTime + 1000 <= currentTime) { +// CurrentApos = CurrentApos.SCORE; +// } +// } +// +// //arm safety pause going down +// if (controller2.getX().isJustPressed()) { +// CurrentApos = CurrentApos.SAFTEYDOWN; // DownQuestion = DownQuestion.NO; // } - - if (controller2.getA().isJustPressed()) { - macroStartTime = currentTime; - CurrentSpos = CurrentSpos.TAPE1; - if (macroStartTime + 1000 <= currentTime) { - CurrentApos = CurrentApos.SCORE; - } - } - - //arm safety pause going down - if (controller2.getX().isJustPressed()) { - CurrentApos = CurrentApos.SAFTEYDOWN; - DownQuestion = DownQuestion.NO; - } - - //arm all the way up - if (controller2.getB().isJustPressed()) { - CurrentApos = CurrentApos.SCORE; - DownQuestion = DownQuestion.NO; - } - - //arm all the way down - if (controller2.getY().isJustPressed()) { - CurrentApos = CurrentApos.SDOWN; - DownQuestion = DownQuestion.YES; - } - - //set slides to max when left joystick is up - if (sY >= 0.5) { - CurrentSpos = CurrentSpos.MAX; - } - - //set slides to tape 1 level when left joystick is down - if (sY <= -0.5) { - CurrentSpos = CurrentSpos.TAPE1; - } - - //set slides to tape 3 when left joystick is right - if (sX >= 0.2) { - CurrentSpos = CurrentSpos.TAPE3; - } - - //set slides to tape 2 when left joystick is left - if (sX <= -0.2) { - CurrentSpos = CurrentSpos.TAPE2; - } - - //set slides all the way down when left bumper gets pressed - if (controller2.getLeftBumper().isJustPressed()) { - CurrentSpos = CurrentSpos.DOWN; - } - - //set intake all teh way up when right bumper is pressed - if (controller2.getRightBumper().isJustPressed()) { - Currentpos = Currentpos.UP; - } - - // update the runtime on the telemetry - telemetry.addData("Runtime", currentTime); - telemetry.update(); - } -} +// +// //arm all the way up +// if (controller2.getB().isJustPressed()) { +// CurrentApos = CurrentApos.SCORE; +// DownQuestion = DownQuestion.NO; +// } +// +// //arm all the way down +// if (controller2.getY().isJustPressed()) { +// CurrentApos = CurrentApos.SDOWN; +// DownQuestion = DownQuestion.YES; +// } +// +// //set slides to max when left joystick is up +// if (sY >= 0.5) { +// CurrentSpos = CurrentSpos.MAX; +// } +// +// //set slides to tape 1 level when left joystick is down +// if (sY <= -0.5) { +// CurrentSpos = CurrentSpos.TAPE1; +// } +// +// //set slides to tape 3 when left joystick is right +// if (sX >= 0.2) { +// CurrentSpos = CurrentSpos.TAPE3; +// } +// +// //set slides to tape 2 when left joystick is left +// if (sX <= -0.2) { +// CurrentSpos = CurrentSpos.TAPE2; +// } +// +// //set slides all the way down when left bumper gets pressed +// if (controller2.getLeftBumper().isJustPressed()) { +// CurrentSpos = CurrentSpos.DOWN; +// } +// +// //set intake all teh way up when right bumper is pressed +// if (controller2.getRightBumper().isJustPressed()) { +// Currentpos = Currentpos.UP; +// } +// +// // update the runtime on the telemetry +// telemetry.addData("Runtime", currentTime); +// telemetry.update(); +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/KhangMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Meet1TeleOp.java similarity index 89% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/KhangMain.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Meet1TeleOp.java index db5051c..f4f9162 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/KhangMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Meet1TeleOp.java @@ -6,7 +6,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.teamcode.controller.Controller; -import org.firstinspires.ftc.teamcode.hardware.ArmPos; +import org.firstinspires.ftc.teamcode.hardware.Arm; import org.firstinspires.ftc.teamcode.hardware.DoorPos; import org.firstinspires.ftc.teamcode.hardware.Down; import org.firstinspires.ftc.teamcode.hardware.Hieght; @@ -18,7 +18,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; @Config @TeleOp(name = "Meet 1 TeleOp", group = "OpModes") -public class KhangMain extends OpMode { +public class Meet1TeleOp extends OpMode { //turbo mode public static double normal = 0.5; @@ -34,7 +34,7 @@ public class KhangMain extends OpMode { //create and set start position of slides private SlidePos.SPosition CurrentSpos = SlidePos.SPosition.DOWN; //create and set start position of arm - private ArmPos.APosition CurrentApos = ArmPos.APosition.SDOWN; + private Arm.APosition CurrentApos = Arm.APosition.SDOWN; //create and set start position of door, default close private DoorPos.DoorPosition CurrentDpos = DoorPos.DoorPosition.CLOSE; //create and set start position of hopper @@ -65,7 +65,7 @@ public class KhangMain extends OpMode { controller2 = new Controller(gamepad2); //feedback to driver hub to know if init was successful telemetry.addLine("Started"); - //update to amke sure we receive last lien of code + //update to make sure we receive last line of code telemetry.update(); } @@ -85,11 +85,11 @@ public class KhangMain extends OpMode { robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); //turbo activation - if (controller1.getRightBumper().isJustPressed()){ + if (controller1.getA().isJustPressed()){ x *= turbo; y *= turbo; z *= turbo; - } else { + } else if (controller1.getA().isJustReleased()){ x *= normal; y *= normal; z *= normal; @@ -105,6 +105,7 @@ public class KhangMain extends OpMode { double sY = -gamepad2.left_stick_y; //graph of X double sX = gamepad2.left_stick_x; + double manualSY = gamepad2.right_stick_y; //reset value to set slides to starting value double reset = gamepad2.left_trigger; @@ -145,9 +146,14 @@ public class KhangMain extends OpMode { //make door rise as intake goes on if (intakeON >= 0.01) { CurrentDpos = DoorPos.DoorPosition.OPEN; - Currentpos = Intake.Position.STACK1; } else { CurrentDpos = DoorPos.DoorPosition.CLOSE; + } + + //make intake go to stack 1 when pressed down + if (intakeON >= 0.25) { + Currentpos = Intake.Position.STACK1; + } else { Currentpos = Intake.Position.UP; } @@ -233,15 +239,19 @@ public class KhangMain extends OpMode { CurrentSpos = CurrentSpos.TAPE2; } - //set slides all the way down when left bumper gets pressed - if (controller2.getLeftBumper().isJustPressed()) { - CurrentSpos = CurrentSpos.DOWN; + if (manualSY >=0.1) { + CurrentSpos = CurrentSpos.MANUAL; } - //set intake all teh way up when right bumper is pressed - if (controller2.getRightBumper().isJustPressed()) { - Currentpos = Currentpos.UP; - } +// //set slides all the way down when left bumper gets pressed +// if (controller2.getLeftBumper().isJustPressed()) { +// CurrentSpos = CurrentSpos.DOWN; +// } +// +// //set intake all teh way up when right bumper is pressed +// if (controller2.getRightBumper().isJustPressed()) { +// Currentpos = Currentpos.UP; +// } // update the runtime on the telemetry telemetry.addData("Runtime", currentTime); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/PoseStorage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/PoseStorage.java new file mode 100644 index 0000000..cec0763 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/PoseStorage.java @@ -0,0 +1,9 @@ +package org.firstinspires.ftc.teamcode.opmode.teleop; + + +import com.acmerobotics.roadrunner.geometry.Pose2d; + +public class PoseStorage { + public static Pose2d currentPose = new Pose2d(); + public static boolean AutoJustEnded = false; +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeam.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeam.java new file mode 100644 index 0000000..2b76160 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeam.java @@ -0,0 +1,11 @@ +//package org.firstinspires.ftc.teamcode.opmode.teleop; +// +//import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +// +//@TeleOp(name = "Opmode For Team Red", group = "Competition") +//public class RedTeam extends AbstractTeleOp { +// +// public RedTeam() { +// team = Team.RED; +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Team.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Team.java new file mode 100644 index 0000000..684cd69 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Team.java @@ -0,0 +1,6 @@ +package org.firstinspires.ftc.teamcode.opmode.teleop; + + public enum Team { + RED, BLUE + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java index cd773cb..6df8529 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java @@ -148,7 +148,4 @@ public class TargetingPipeline extends OpenCvPipeline { public Detection getBlue() { return this.blue; } - public Detection getBlack() { - return this.black; - } } \ No newline at end of file