From dc48b5ebf3dbb1ff64dd05424685721882342027 Mon Sep 17 00:00:00 2001 From: UntitledRice Date: Sat, 11 Nov 2023 18:39:00 -0600 Subject: [PATCH] Fix all errors, first working code used for meet 1, after meet added new keybinds and some minor changes plus lots fo math, logic, adn macros added. --- TeamCode/build.gradle | 2 + .../ftc/teamcode/hardware/Arm.java | 93 ++++++++ .../ftc/teamcode/hardware/ArmPos.java | 8 + .../ftc/teamcode/hardware/DoorPos.java | 7 + .../ftc/teamcode/hardware/Down.java | 7 + .../ftc/teamcode/hardware/Hieght.java | 7 + .../ftc/teamcode/hardware/HopperPos.java | 7 + .../ftc/teamcode/hardware/Intake.java | 15 +- .../ftc/teamcode/hardware/Robot.java | 16 +- .../ftc/teamcode/hardware/SlidePos.java | 8 + .../ftc/teamcode/hardware/Slides.java | 138 ++++++++++++ .../opmode/autonomous/AbstractAuto.java | 169 +++++++++++++- .../teamcode/opmode/autonomous/BotGoMove.java | 27 +++ .../autonomous/StartFromLeftCenterSpike.java | 40 ++-- .../ftc/teamcode/opmode/autonomous/Step.java | 53 +++++ .../teamcode/opmode/autonomous/TestAuto.java | 29 +++ .../ftc/teamcode/opmode/teleop/KhangMain.java | 206 +++++++++++++++--- .../roadrunner/drive/DriveConstants.java | 2 +- .../roadrunner/drive/SampleMecanumDrive.java | 14 +- .../drive/TwoWheelTrackingLocalizer.java | 8 +- .../ftc/teamcode/util/CameraPosition.java | 2 +- .../ftc/teamcode/util/Constants.java | 4 + build.common.gradle | 1 + 23 files changed, 770 insertions(+), 93 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/ArmPos.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DoorPos.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Down.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Hieght.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/HopperPos.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SlidePos.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BotGoMove.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/Step.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/TestAuto.java diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index dc69ae4..52b0465 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -42,4 +42,6 @@ dependencies { annotationProcessor files('lib/OpModeAnnotationProcessor.jar') implementation 'org.openftc:easyopencv:1.7.0' implementation 'com.acmerobotics.roadrunner:core:0.5.5' + implementation 'org.ftclib.ftclib:vision:2.0.1' + implementation 'org.ftclib.ftclib:core:2.0.1' } 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 new file mode 100644 index 0000000..d33c4f6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java @@ -0,0 +1,93 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import com.acmerobotics.dashboard.config.Config; +import com.arcrobotics.ftclib.controller.PController; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +@Config +public class Arm { + + //make each servo + private Servo dServo; + private Servo rAServo; + private Servo lAServo; + private Servo wristServo; + private SlidePos.SPosition pos = SlidePos.SPosition.DOWN; + private PController armController; + private double armControllerTarget; + private double ARM_KP = 0.001; + + public Arm(HardwareMap hardwareMap) { + wristServo = hardwareMap.get(Servo.class, "Wrist Servo"); + dServo = hardwareMap.get(Servo.class, "Door Servo"); + lAServo = hardwareMap.get(Servo.class, "Left Arm Servo"); + rAServo = hardwareMap.get(Servo.class, "Right Arm Servo "); +// lAServo.setDirection(Servo.Direction.REVERSE); + rAServo.setDirection(Servo.Direction.REVERSE); + dServo.setDirection(Servo.Direction.REVERSE); +// wristServo.setDirection(Servo.Direction.REVERSE); + } + + public static double armScorePos = 1; + public static double doorOpenpos = 0.5; + public static double doorClosePos = 0.85; + public static double startingWristPos = 0.735; + public static double startingArmPos = 0.325; + public static double safteyDownPos = 0.4; + public static double safteyUpPos = 0.9; + public static double wristScorePos = 0.98; + public static double safeWrist = 0.8; + + public void openDoor(DoorPos.DoorPosition pos) { + if (pos == DoorPos.DoorPosition.OPEN) { + dServo.setPosition(doorOpenpos); + } else if (pos == DoorPos.DoorPosition.CLOSE) { + dServo.setPosition(doorClosePos); + } + } + + public void setPos(ArmPos.APosition tape) { + if (tape == ArmPos.APosition.SDOWN) { + this.armControllerTarget = startingArmPos; + lAServo.setPosition(startingArmPos); + rAServo.setPosition(startingArmPos); + } else if (tape == ArmPos.APosition.SCORE) { + this.armControllerTarget = armScorePos; + lAServo.setPosition(armScorePos); + rAServo.setPosition(armScorePos); + wristServo.setPosition(wristScorePos); + } else if (tape == ArmPos.APosition.SAFTEYDOWN) { + lAServo.setPosition(safteyDownPos); + rAServo.setPosition(safteyDownPos); + wristServo.setPosition(wristScorePos); + } else if (tape == ArmPos.APosition.SAFTEYUP) { + lAServo.setPosition(safteyUpPos); + rAServo.setPosition(safteyUpPos); + wristServo.setPosition(wristScorePos); + } + } + + + public void setHopper(HopperPos.hopperPos hopper) { + if (hopper == HopperPos.hopperPos.UP) { + wristServo.setPosition(wristScorePos); + } else if (hopper == HopperPos.hopperPos.DOWN) { + wristServo.setPosition(startingWristPos); + } + } + + public String getTelemetry() { + return "Wrist Servo: "+wristServo.getPosition()+"Left Arm Servo: "+lAServo.getPosition()+"Right Arm Servo: "+rAServo.getPosition()+"Door Servo: "+dServo.getPosition(); + } + + public void update() { + armController.setP(0.01); + this.armController.setSetPoint(this.armControllerTarget); + + double output = this.armController.calculate(this.lAServo.getPosition()); + + this.lAServo.setPosition(this.lAServo.getPosition() + output); + this.rAServo.setPosition(this.rAServo.getPosition() + output); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/ArmPos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/ArmPos.java new file mode 100644 index 0000000..f078725 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/ArmPos.java @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.teamcode.hardware; + +public class ArmPos { + + public enum APosition { + SDOWN, SCORE, SAFTEYDOWN, SAFTEYUP; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DoorPos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DoorPos.java new file mode 100644 index 0000000..bae4fc3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DoorPos.java @@ -0,0 +1,7 @@ +package org.firstinspires.ftc.teamcode.hardware; + +public class DoorPos { + public enum DoorPosition { + OPEN, CLOSE + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Down.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Down.java new file mode 100644 index 0000000..520a7ff --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Down.java @@ -0,0 +1,7 @@ +package org.firstinspires.ftc.teamcode.hardware; + +public class Down { + public enum DownArm { + YES, NO + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Hieght.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Hieght.java new file mode 100644 index 0000000..3a28e4d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Hieght.java @@ -0,0 +1,7 @@ +package org.firstinspires.ftc.teamcode.hardware; + +public class Hieght { + public enum height { + ASCEND, HOLD, DESCEND, RESET + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/HopperPos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/HopperPos.java new file mode 100644 index 0000000..d9f0a3d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/HopperPos.java @@ -0,0 +1,7 @@ +package org.firstinspires.ftc.teamcode.hardware; + +public class HopperPos { + public enum hopperPos { + UP, DOWN + } +} 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 aaa90d7..3c8be72 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 @@ -6,12 +6,11 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @Config -public class Intake { // TODO done in theory, but need to get the actual servo positions &&&& do the Sensor stuff +public class Intake { private Servo rServo; private Servo lServo; private DcMotor dcMotor; private Position pos = Position.UP; - public enum Position { STACK1, STACK2, STACK3, STACK4, STACK5, UP; @@ -27,12 +26,12 @@ public class Intake { // TODO done in theory, but need to get the actual servo p } //Position - public static double stack1 = 0.49; - public static double stack2 = 0.50; - public static double stack3 = 0.51; - public static double stack4 = 0.52; - public static double stack5 = 0.53; - public static double up = 0.551; + public static double stack1 = 0; + public static double stack2 = 0.03; + public static double stack3 = 0.06; + public static double stack4 = 0.09; + public static double stack5 = 0.12; + public static double up = 0.30; public static double motorPower = 0; public Intake(HardwareMap hardwareMap) { 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 02dd6c2..4b993e3 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 @@ -8,31 +8,41 @@ import org.firstinspires.ftc.teamcode.roadrunner.util.Encoder; @Config public class Robot { - //set to public Drive to revert + + //Create each object public SampleMecanumDrive drive; public Camera camera; public Intake intake; + public Slides slides; + public Arm arm; private boolean camEnabled = true; + //Name the objects public Robot(HardwareMap hardwareMap) { - //change this to new Drive to revert + arm = new Arm(hardwareMap); drive = new SampleMecanumDrive(hardwareMap); + slides = new Slides(hardwareMap); camera = new Camera(hardwareMap); camera.initTargetingCamera(); intake = new Intake(hardwareMap); camEnabled = true; } + //Update on runtime and dive public void update(double runTime) { drive.update(); } + //Return position to control hub public String getTelemetry() { Encoder slide = null; return String.format("position: %s", slide.getCurrentPosition()); } - //set to public Drive to revert + public Arm getArm() { + return this.arm; + } + //return drive public SampleMecanumDrive getDrive() { return this.drive; } 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 new file mode 100644 index 0000000..286a6f1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SlidePos.java @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.teamcode.hardware; + +public class SlidePos { + + public enum SPosition { + DOWN, TAPE1, TAPE2, TAPE3, MAX; + } +} 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 new file mode 100644 index 0000000..52d06b5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java @@ -0,0 +1,138 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +@Config +public class Slides { + + //Create and assign motors + public Slides(HardwareMap hardwareMap) { + rSMotor = hardwareMap.get(DcMotor.class, "Right Slide Motor"); +// rSMotor.setDirection(DcMotorSimple.Direction.REVERSE); + rSMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rSMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + lSMotor = hardwareMap.get(DcMotor.class, "Left Slide Motor"); + lSMotor.setDirection(DcMotorSimple.Direction.REVERSE); + lSMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + lSMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + //make each motor + private DcMotor rSMotor; + private DcMotor lSMotor; + + //Value for motor power and speed slides go at + public static double rSPower = 0.5; + public static double lSPower = 0.5; + public static double downpos = 0; + public static double tape1pos = 200; + public static double tape2pos = 350; + public static double tape3pos = 500; + public static double maxpos = 1000; + + //Set default slide height to 0 aka starting position + public static double sHeight = 0; + public static double newSHeight = 0; + +// Do stuff to get to a position when asked to go to a position + public void setSPos(SlidePos.SPosition height) { + if (height == SlidePos.SPosition.DOWN) { + sHeight = 0; + lSMotor.setTargetPosition((int) downpos); + lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lSMotor.setPower(lSPower); + rSMotor.setTargetPosition((int) downpos); + rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rSMotor.setPower(rSPower); + } else if (height == SlidePos.SPosition.TAPE1) { + sHeight = 300; + lSMotor.setTargetPosition((int) tape1pos); + lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lSMotor.setPower(lSPower); + rSMotor.setTargetPosition((int) tape1pos); + rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rSMotor.setPower(rSPower); + } else if (height == SlidePos.SPosition.TAPE2) { + sHeight = 500; + lSMotor.setTargetPosition((int) tape2pos); + lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lSMotor.setPower(lSPower); + rSMotor.setTargetPosition((int) tape2pos); + rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rSMotor.setPower(rSPower); + } else if (height == SlidePos.SPosition.TAPE3) { + sHeight = 700; + lSMotor.setTargetPosition((int) tape3pos); + lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lSMotor.setPower(lSPower); + rSMotor.setTargetPosition((int) tape3pos); + rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rSMotor.setPower(rSPower); + } else if (height == SlidePos.SPosition.MAX) { + sHeight = 1000; + lSMotor.setTargetPosition((int) maxpos); + lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lSMotor.setPower(lSPower); + rSMotor.setTargetPosition((int) maxpos); + rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rSMotor.setPower(rSPower); + } + } + +// public void setHeight(Hieght.height send) { +// if (send == Hieght.height.ASCEND) { +// if (newSHeight <= 790) { +// newSHeight = newSHeight + 10; +// lSMotor.setTargetPosition((int) (newSHeight)); +// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// lSMotor.setPower(lSPower); +// rSMotor.setTargetPosition((int) (newSHeight)); +// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// rSMotor.setPower(rSPower); +// } else if (newSHeight >= 800) { +// newSHeight = 800; +// lSMotor.setTargetPosition((int) (newSHeight)); +// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// lSMotor.setPower(lSPower); +// rSMotor.setTargetPosition((int) (newSHeight)); +// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// rSMotor.setPower(rSPower); +// } +// } else if (send == Hieght.height.HOLD) { +// newSHeight = newSHeight + 0; +// lSMotor.setTargetPosition((int) (newSHeight)); +// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// lSMotor.setPower(lSPower); +// rSMotor.setTargetPosition((int) (newSHeight)); +// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// rSMotor.setPower(rSPower); +// } else if (send == Hieght.height.DESCEND) { +// if (newSHeight >= 10) { +// newSHeight = newSHeight - 10; +// lSMotor.setTargetPosition((int) (newSHeight)); +// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// lSMotor.setPower(lSPower); +// rSMotor.setTargetPosition((int) (newSHeight)); +// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// rSMotor.setPower(rSPower); +// } else if (newSHeight <= 0) { +// newSHeight = 0; +// lSMotor.setTargetPosition((int) (newSHeight)); +// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// lSMotor.setPower(lSPower); +// rSMotor.setTargetPosition((int) (newSHeight)); +// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); +// rSMotor.setPower(rSPower); +// } +// } +// } + + + //Return feed to the control hub for easy debug + public String getTelemetry() { + return "Left Slide Motor: "+lSMotor.getPower()+"Right Slide Motor: "+rSMotor.getPower(); + } +} 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 6d78026..f406f9d 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 @@ -1,29 +1,40 @@ package org.firstinspires.ftc.teamcode.opmode.autonomous; +import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.util.CameraPosition; import java.util.ArrayList; +import java.util.Locale; public abstract class AbstractAuto extends LinearOpMode { - public Robot robot; - private int teamElementLocation = 2; - private double currentRuntime; + private int teamElementLocation = 2; + private ArrayList steps; + private double currentRuntime; + public CameraPosition cameraPosition; + + // Main method to run all the steps for autonomous @Override public void runOpMode() { +// telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); telemetry.addLine("Initializing Robot..."); telemetry.update(); + + setCameraPosition(); + robot = new Robot(hardwareMap); + makeTrajectories(); while (robot.camera.getFrameCount() < 1) { idle(); } + // wait for start while (!(isStarted() || isStopRequested())) { currentRuntime = getRuntime(); robot.update(currentRuntime); @@ -34,17 +45,161 @@ public abstract class AbstractAuto extends LinearOpMode { } telemetry.addLine("Initialized"); - telemetry.addLine("Randomization: " + teamElementLocation); + telemetry.addLine("Randomization: "+teamElementLocation); + telemetry.addLine(robot.getTelemetry()); + telemetry.update(); + } + resetRuntime(); + + // build the first step + steps = new ArrayList<>(); + stopCamera(); + initializeSteps(teamElementLocation); + + int stepNumber = 0; + double stepTimeout; + Step step = steps.get(stepNumber); + stepTimeout = step.getTimeout() != -1 ? currentRuntime + step.getTimeout() : Double.MAX_VALUE; + step.start(); + + // run the remaining steps + while (opModeIsActive()) { + currentRuntime = getRuntime(); + // once a step finishes + if (step.isFinished() || currentRuntime >= stepTimeout) { + // do the finishing move + step.end(); + stepNumber++; + // if it was the last step break out of the while loop + if (stepNumber > steps.size() - 1) { + break; + } + // else continue to the next step + step = steps.get(stepNumber); + stepTimeout = step.getTimeout() != -1 ? currentRuntime + step.getTimeout() : Double.MAX_VALUE; + step.start(); + } + +// // update turret and slides position +// PoseStorage.slidesPosition = robot.actuators.getSlides(); +// PoseStorage.turretPosition = robot.actuators.getTurret(); +// PoseStorage.currentPose = robot.drive.getPoseEstimate(); + + // while the step is running display telemetry + step.whileRunning(); + robot.update(currentRuntime); + + telemetry.addLine(String.format(Locale.US, "Runtime: %.0f", currentRuntime)); + telemetry.addLine("Step " + (stepNumber + 1) + " of " + steps.size() + ", " + step.getTelemetry() + "\n"); telemetry.addLine(robot.getTelemetry()); telemetry.update(); } } - public abstract void setAlliance(); + // Load up all of the steps for the autonomous: to be overridden with the specific steps in the specific auto + public abstract void initializeSteps(int location); - public abstract void makeTrajectories(); + //methods to be implemented in the specific autos + public abstract void setAlliance(); public abstract void setCameraPosition(); public abstract boolean useCamera(); + + public abstract void makeTrajectories(); + + +// public abstract void setArm(Arm.Position armPos, Claw.Position clawPos); + + //other methods that do certain tasks + + public void turn(double degrees) { + steps.add(new Step("Following a trajectory") { + @Override + public void start() { + robot.drive.turn(degrees); + } + + @Override + public void whileRunning() { + robot.drive.update(); + } + + @Override + public void end() { + } + + @Override + public boolean isFinished() { + return !robot.drive.isBusy(); + } + }); + } + + public void followTrajectory(Trajectory trajectory) { + steps.add(new Step("Following a trajectory") { + @Override + public void start() { + robot.drive.followTrajectoryAsync(trajectory); + } + + @Override + public void whileRunning() { + robot.drive.update(); + } + + @Override + public void end() { + } + + @Override + public boolean isFinished() { + return !robot.drive.isBusy(); + } + }); + } + + // Functions to add steps + public void addDelay(double timeout) { + steps.add(new Step("Waiting for " + timeout + " seconds", timeout) { + @Override + public void start() { + } + + @Override + public void whileRunning() { + } + + @Override + public void end() { + } + + @Override + public boolean isFinished() { + return false; + } + }); + } + + public void stopCamera() { + steps.add(new Step("Stopping Signal Camera") { + @Override + public void start() { + robot.camera.stopTargetingCamera(); + } + + @Override + public void whileRunning() { + } + + @Override + public void end() { + } + + @Override + public boolean isFinished() { + return true; + } + }); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BotGoMove.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BotGoMove.java new file mode 100644 index 0000000..468e66c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BotGoMove.java @@ -0,0 +1,27 @@ +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 com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +@Autonomous +public class BotGoMove extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory myTrajectory = drive.trajectoryBuilder(new Pose2d()) + .forward(3.7) + .build(); + + waitForStart(); + + if(isStopRequested()) return; + + drive.followTrajectory(myTrajectory); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/StartFromLeftCenterSpike.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/StartFromLeftCenterSpike.java index c70d97d..c5fa7ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/StartFromLeftCenterSpike.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/StartFromLeftCenterSpike.java @@ -1,11 +1,12 @@ package org.firstinspires.ftc.teamcode.opmode.autonomous; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.teamcode.hardware.Camera; -import org.firstinspires.ftc.teamcode.hardware.Drive; +import org.firstinspires.ftc.teamcode.hardware.Intake; import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants; import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; @@ -14,24 +15,22 @@ import org.firstinspires.ftc.teamcode.util.CameraPosition; @Autonomous(name = "Start From Left Wall", group = "Left Start", preselectTeleOp = "Khang Main") public class StartFromLeftCenterSpike extends AbstractAuto { - //set to public Drive to revert - public SampleMecanumDrive drive; public Robot robot; - public Camera camera; - private boolean camEnabled = true; public CameraPosition cameraPosition; - private int teamElementLocation = 2; //Steps public Trajectory start; - public Trajectory step1; - public Trajectory step2; + + @Override + public void initializeSteps(int location) { + followTrajectory(start); + } @Override public void setAlliance() {} @Override public void setCameraPosition() { - cameraPosition = CameraPosition.CENTER; + cameraPosition = CameraPosition.FRONT; } @Override @@ -39,27 +38,22 @@ public class StartFromLeftCenterSpike extends AbstractAuto { return true; } - public void Robot(HardwareMap hardwareMap) { - //set to new Drive to revert - drive = new SampleMecanumDrive(hardwareMap); - camera = new Camera(hardwareMap); - camera.initTargetingCamera(); - camEnabled = true; + @Override + public void waitForStart() { + super.waitForStart(); } @Override public void makeTrajectories() { // positions - Pose2d start = new Pose2d(-65.125,-48,Math.toRadians(-90)); - Pose2d step1 = new Pose2d(-24,-48,Math.toRadians(0)); - Pose2d step2 = new Pose2d(-24,-48,Math.toRadians(0)); + Pose2d start = new Pose2d(-65.125,-48,Math.toRadians(90)); + Vector2d step1 = new Vector2d(-24,-48); + Pose2d step2 = new Pose2d(-24,-48,Math.toRadians(90)); - this.start = robot.drive.trajectoryBuilder(start) - .lineToSplineHeading(step1, - SampleMecanumDrive.getVelocityConstraint(60, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), - SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL) - ) +// this.start = robot.drive.trajectoryBuilder(start) + this.start = robot.drive.trajectoryBuilder(new Pose2d()) + .forward(3) .build(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/Step.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/Step.java new file mode 100644 index 0000000..b6ea1f9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/Step.java @@ -0,0 +1,53 @@ +package org.firstinspires.ftc.teamcode.opmode.autonomous; + + +import org.firstinspires.ftc.teamcode.vision.Detection; + +// Class for every step that the autonomous program will take +public abstract class Step { + private final double timeout; + private String telemetry; + + // variables when moving + public double x; + public double y; + public double stepTime; + double tempTime = stepTime; + + // variables when shooting + public Detection teamProp; + + // Constructors + public Step(String telemetry) { + this.telemetry = telemetry; + this.timeout = -1; + } + + public Step(String telemetry, double timeout) { + this.telemetry = telemetry; + this.timeout = timeout; + } + + // Abstract methods to be overrode + public abstract void start(); + + public abstract void whileRunning(); + + public abstract void end(); + + public abstract boolean isFinished(); + + // Return the timeout for the step + public double getTimeout() { + return timeout; + } + + public void setTelemetry(String telemetry) { + this.telemetry = telemetry; + } + + // Return the Telemetry for the step + public String getTelemetry() { + return telemetry; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/TestAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/TestAuto.java new file mode 100644 index 0000000..134ebfa --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/TestAuto.java @@ -0,0 +1,29 @@ +package org.firstinspires.ftc.teamcode.opmode.autonomous; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +@Config + +@Autonomous (name = "Start From Left Center Spike Test", group = "Testing", preselectTeleOp = "KhangMain") + +public class TestAuto extends LinearOpMode { + + private Robot robot; + + @Override + public void runOpMode() throws InterruptedException { + + this.robot = new Robot(hardwareMap); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + //bot is 15 in and a half so middle is 7.75 + //17.8 in so half is 8.9 in + Pose2d startPOS = new Pose2d(-63.1, -36.75, Math.toRadians(90)); + } +} 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/KhangMain.java index 1e52d99..ca74b90 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/KhangMain.java @@ -1,71 +1,207 @@ 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 org.firstinspires.ftc.teamcode.controller.Controller; -import org.firstinspires.ftc.teamcode.hardware.Camera; +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.roadrunner.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.hardware.SlidePos; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.Gamepad; -import com.qualcomm.robotcore.hardware.HardwareMap; - -@TeleOp(name = "Khang Main", group = "OpModes") +@Config +@TeleOp(name = "Meet 1 TeleOp", group = "OpModes") public class KhangMain extends OpMode { + //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; + //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() { + + //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() { -// this.robot.getDrive().setInput(gamepad1, gamepad2); - double x = gamepad1.left_stick_x; - double y = -gamepad1.left_stick_y; - double z = gamepad1.right_stick_x; - robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); + + //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); + //create variable for door open or close to open and close in sync with intake + double door = gamepad2.right_trigger; + + //manual slide height control + double sHeight = gamepad2.right_stick_y; + //set slides all the way down aka reset button + 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; + + //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); + //hopper position + robot.arm.setHopper(hopperpos); + //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 { + planeServo.setPosition(0); + } + + //make door rise as intake goes on + if (intakeON >= 0.01) { + CurrentDpos = DoorPos.DoorPosition.OPEN; + } else { + CurrentDpos = DoorPos.DoorPosition.CLOSE; + } + + //control position of hopper if (controller2.getLeftBumper().isJustPressed()) { - Currentpos = Currentpos.nextPosition(); + hopperpos = HopperPos.hopperPos.UP; + } else if (controller2.getLeftBumper().isJustReleased()) { + hopperpos = HopperPos.hopperPos.DOWN; } - if (controller2.getRightBumper().isJustPressed()) { - Currentpos = Currentpos.previousPosition(); + //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; + } - // Read pose -// Pose2d poseEstimate = drive.getPoseEstimate(); -// -//// Create a vector from the gamepad x/y inputs -//// Then, rotate that vector by the inverse of that heading -// Vector2d input = new Vector2d( -// -gamepad1.left_stick_y, -// -gamepad1.left_stick_x -// ).rotated(-poseEstimate.getHeading()); -// -//// Pass in the rotated input + right stick value for rotation -//// Rotation is not part of the rotated input thus must be passed in separately -// drive.setWeightedDrivePower( -// new Pose2d( -// input.getX(), -// input.getY(), -// -gamepad1.right_stick_x -// ) -// ); + //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; + } + + //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; + } + + if (sY >= 0.2) { + CurrentSpos = CurrentSpos.MAX; + } + + if (sY <= -0.2) { + CurrentSpos = CurrentSpos.TAPE1; + } + + if (sX >= 0.2) { + CurrentSpos = CurrentSpos.TAPE3; + } + + if (sX <= -0.2) { + CurrentSpos = CurrentSpos.TAPE2; + } + + if (gamepad2.left_trigger >= 0.35) { + CurrentSpos = CurrentSpos.DOWN; + } + +// if (gamepad2.left_stick_button = true) { +// CurrentSpos = CurrentSpos.DOWN; +// } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java index d2f6fb0..05f0c73 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java @@ -31,7 +31,7 @@ public class DriveConstants { * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients * from DriveVelocityPIDTuner. */ - public static final boolean RUN_USING_ENCODER = true; + public static final boolean RUN_USING_ENCODER = false; public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0, getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java index dee69ec..5e107ee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java @@ -136,7 +136,7 @@ public class SampleMecanumDrive extends MecanumDrive { // TODO: if desired, use setLocalizer() to change the localization method // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...)); - setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this)); +// setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this)); // setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap)); trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID); @@ -282,8 +282,8 @@ public class SampleMecanumDrive extends MecanumDrive { public void setMotorPowers(double v, double v1, double v2, double v3) { leftFront.setPower(v); leftRear.setPower(v1); - rightRear.setPower(v2); - rightFront.setPower(v3); + rightRear.setPower(-v2); + rightFront.setPower(-v3); } @Override @@ -315,12 +315,4 @@ public class SampleMecanumDrive extends MecanumDrive { public String getTelemetry() { return "Drive: "+getPoseEstimate(); } - - public void setInput(Gamepad gamepad1, Gamepad gamepad2) { - double x = gamepad1.left_stick_x; - double y = -gamepad1.left_stick_y; - double z = gamepad1.right_stick_x; - - setInput(gamepad1, gamepad2); - } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java index 5ea5411..5faa500 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java @@ -61,13 +61,13 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer { this.drive = drive; - parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "parallel")); +// parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "parallel")); // parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backRight")); - perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "perpendicular")); +// perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "perpendicular")); // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) - perpendicularEncoder.setDirection(Encoder.Direction.REVERSE); - parallelEncoder.setDirection(Encoder.Direction.REVERSE); +// perpendicularEncoder.setDirection(Encoder.Direction.REVERSE); +// parallelEncoder.setDirection(Encoder.Direction.REVERSE); } public static double encoderTicksToInches(double ticks) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CameraPosition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CameraPosition.java index 0e65173..ae94b41 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CameraPosition.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CameraPosition.java @@ -1,5 +1,5 @@ package org.firstinspires.ftc.teamcode.util; public enum CameraPosition { - LEFT, CENTER, RIGHT + FRONT, BACK } \ 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..3e66862 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 @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.util; +import androidx.annotation.VisibleForTesting; + import org.firstinspires.ftc.teamcode.vision.Detection; import org.opencv.core.Mat; import org.opencv.core.Point; @@ -51,4 +53,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 lServo = "lServo"; + public static final String rServo = "rServo"; } \ No newline at end of file diff --git a/build.common.gradle b/build.common.gradle index 12e6acb..dda61e7 100644 --- a/build.common.gradle +++ b/build.common.gradle @@ -121,5 +121,6 @@ android { } repositories { + mavenCentral() }