From 4625c3f784c2f30477ac927903c9d520b7ce3e5d Mon Sep 17 00:00:00 2001 From: Robert Teal Date: Tue, 19 Dec 2023 23:19:07 -0600 Subject: [PATCH] tweaks all around, deleted a lot of unused files, started a cycle auto --- .../ftc/teamcode/hardware/Arm.java | 90 ++++-- .../ftc/teamcode/hardware/ArmPos.java | 8 - .../ftc/teamcode/hardware/Camera.java | 15 +- .../ftc/teamcode/hardware/DoorPos.java | 7 - .../ftc/teamcode/hardware/Down.java | 7 - .../ftc/teamcode/hardware/DroneLauncher.java | 2 +- .../ftc/teamcode/hardware/Height.java | 7 - .../ftc/teamcode/hardware/HopperPos.java | 7 - .../ftc/teamcode/hardware/Intake.java | 7 +- .../ftc/teamcode/hardware/KhangSlides.java | 138 -------- .../ftc/teamcode/hardware/Robot.java | 22 +- .../ftc/teamcode/hardware/SlidePos.java | 8 - .../teamcode/hardware/{robby => }/Slides.java | 14 +- .../opmode/{teleop => }/PoseStorage.java | 2 +- .../opmode/autonomous/AbstractAuto.java | 60 +++- .../opmode/autonomous/BlueLeftAuto.java | 2 +- .../opmode/autonomous/RedRightAuto.java | 98 ++++++ .../ftc/teamcode/opmode/autonomous/Step.java | 11 +- .../opmode/teleop/AbstractTeleOp.java | 304 ------------------ .../ftc/teamcode/opmode/teleop/BlueTeam.java | 11 - .../opmode/teleop/ExperimentalKhangMain.java | 257 --------------- .../{NewTeleop.java => MainTeleOp.java} | 41 ++- .../teamcode/opmode/teleop/Meet1TeleOp.java | 260 --------------- .../ftc/teamcode/opmode/teleop/RedTeam.java | 11 - .../ftc/teamcode/opmode/teleop/Team.java | 6 - .../roadrunner/drive/DriveConstants.java | 7 +- .../drive/TwoWheelTrackingLocalizer.java | 2 +- 27 files changed, 283 insertions(+), 1121 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/ArmPos.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DoorPos.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Down.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Height.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/HopperPos.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/KhangSlides.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SlidePos.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/{robby => }/Slides.java (91%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/{teleop => }/PoseStorage.java (76%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedRightAuto.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/AbstractTeleOp.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeam.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/ExperimentalKhangMain.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/{NewTeleop.java => MainTeleOp.java} (75%) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Meet1TeleOp.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeam.java delete 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 0c04bb9..e40b5cb 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,79 +5,102 @@ 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 { - //make each servo - private Servo dServo; private Servo rAServo; private Servo lAServo; + private Servo doorServo; private Servo wristServo; private Slides.Position pos = Slides.Position.DOWN; private PController armController; private double armControllerTarget; - public static double ARM_KP = 0.01; + public static double armTolerance = 0.03; + public static double armSpeed = 0.022; + private double armPos; + private double armTempPos; + private double doorPos; + private double wristPos; + public static double ARM_KP = 0.08; + + public static double doorOpenPos = 0.09; + public static double doorClosePos = 0.26; + + public static double armStart = 0.24; + public static double armScore = 0.95; + public static double wristStart = 0.29; + public static double wristScore = 0.5; public enum Position { - INTAKE, SCORE; + INTAKE, SCORE + } + + public enum DoorPosition { + OPEN, CLOSE } public Arm(HardwareMap hardwareMap) { wristServo = hardwareMap.get(Servo.class, "Wrist Servo"); - dServo = hardwareMap.get(Servo.class, "Door Servo"); + doorServo = 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); + doorServo.setDirection(Servo.Direction.REVERSE); // wristServo.setDirection(Servo.Direction.REVERSE); this.armController = new PController(ARM_KP); setArmPos(Position.INTAKE); + lAServo.setPosition(armStart); + rAServo.setPosition(armStart); +// armTempPos = armPos; + setWristPos(Position.INTAKE); + setDoor(DoorPosition.CLOSE); } - public static double doorOpenpos = 0.1; - public static double doorClosePos = 0.85; - - public static double armStart = 0.15; - public static double armScore = 1; - public static double wristStart = 0.93; - public static double wristScore = 0.98; - public void setArmPos(Position pos) { this.armControllerTarget = pos == Position.INTAKE ? armStart : armScore; +// if (pos == Position.INTAKE) { +// armPos = armStart; +// } else if (pos == Position.SCORE) { +// armPos = armScore; +// } + } + public boolean armAtPosition() { + return armController.atSetPoint(); +// return armTempPos == armPos; } public void setWristPos(Position pos) { if (pos == Position.INTAKE) { - wristServo.setPosition(wristStart); + wristPos = wristStart; } else if (pos == Position.SCORE) { - wristServo.setPosition(wristScore); + wristPos = wristScore; } } - public void setDoor(DoorPos.DoorPosition pos) { - if (pos == DoorPos.DoorPosition.OPEN) { - dServo.setPosition(doorOpenpos); - } else if (pos == DoorPos.DoorPosition.CLOSE) { - dServo.setPosition(doorClosePos); + public void setDoor(DoorPosition pos) { + if (pos == DoorPosition.OPEN) { + doorPos = doorOpenPos; + } else if (pos == DoorPosition.CLOSE) { + doorPos = doorClosePos; } } public String getTelemetry() { - return "Wrist Servo: "+wristServo.getPosition()+"Left Arm Servo: "+lAServo.getPosition()+"Right Arm Servo: "+rAServo.getPosition()+"Door Servo: "+dServo.getPosition(); + return String.format("Wrist: %s\nDoor: %s\nLeft Arm: %s\nRight Arm: %s\nError: %s", + wristServo.getPosition(), doorServo.getPosition(), lAServo.getPosition(), rAServo.getPosition(), armController.getPositionError()); + //return "Wrist Servo: "+wristServo.getPosition()+"\nLeft Arm Servo: "+lAServo.getPosition()+"\nRight Arm Servo: "+rAServo.getPosition()+"\nDoor Servo: "+ doorServo.getPosition(); } public void update() { this.armController.setSetPoint(this.armControllerTarget); - this.armController.setTolerance(0.001); + this.armController.setTolerance(armTolerance); this.armController.setP(ARM_KP); double output = 0; @@ -86,6 +109,23 @@ public class Arm { this.lAServo.setPosition(this.lAServo.getPosition() + output); this.rAServo.setPosition(this.lAServo.getPosition() + output); + } else { + lAServo.setPosition(armControllerTarget); + rAServo.setPosition(armControllerTarget); } +// if (Math.abs(armTempPos-armPos) <= armTolerance) { +// armTempPos = armPos; +// } else { +// if (armTempPos < armPos) { +// armTempPos += armSpeed; +// } else if (armTempPos > armPos) { +// armTempPos -= armSpeed; +// } +// } +// lAServo.setPosition(armTempPos); +// rAServo.setPosition(armTempPos); + + doorServo.setPosition(doorPos); + wristServo.setPosition(wristPos); } } 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 deleted file mode 100644 index f078725..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/ArmPos.java +++ /dev/null @@ -1,8 +0,0 @@ -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/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java index 9465b6f..4df8526 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 @@ -17,20 +17,22 @@ import org.openftc.apriltag.AprilTagDetection; import org.openftc.easyopencv.OpenCvCamera; import org.openftc.easyopencv.OpenCvCameraFactory; +import com.acmerobotics.dashboard.FtcDashboard; + import java.util.ArrayList; // Class for the camera public class Camera { + private HardwareMap hardwareMap; + private OpenCvCamera targetingCamera; + private TargetingPipeline targetingPipeline; + private boolean targetingCameraInitialized; private float decimation; private boolean needToSetDecimation; int numFramesWithoutDetection = 0; private boolean signalWebcamInitialized; private OpenCvCamera signalWebcam; - private HardwareMap hardwareMap; - private OpenCvCamera targetingCamera; - private TargetingPipeline targetingPipeline; - private boolean targetingCameraInitialized; AprilTagDetectionPipeline aprilTagDetectionPipeline; ArrayList detections; static final double FEET_PER_METER = 3.28084; @@ -57,6 +59,7 @@ public class Camera { public void onOpened() { targetingCamera.startStreaming(WEBCAM_WIDTH, WEBCAM_HEIGHT, WEBCAM_ROTATION); targetingCameraInitialized = true; + FtcDashboard.getInstance().startCameraStream(signalWebcam, 0); } @Override @@ -85,8 +88,8 @@ public class Camera { //return frame rate public int getFrameCount() { - if (signalWebcamInitialized) { - return signalWebcam.getFrameCount(); + if (targetingCameraInitialized) { + return targetingCamera.getFrameCount(); } else { return 0; } 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 deleted file mode 100644 index bae4fc3..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DoorPos.java +++ /dev/null @@ -1,7 +0,0 @@ -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 deleted file mode 100644 index 520a7ff..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Down.java +++ /dev/null @@ -1,7 +0,0 @@ -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/DroneLauncher.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DroneLauncher.java index 7f28e53..4bd9274 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DroneLauncher.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DroneLauncher.java @@ -11,7 +11,7 @@ public class DroneLauncher { public static double launchPos = 0.5; public DroneLauncher(HardwareMap hardwareMap) { - this.servo = hardwareMap.get(Servo.class, Constants.droneLauncher); + this.servo = hardwareMap.get(Servo.class, "Drone Launcher"); this.servo.setPosition(initPos); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Height.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Height.java deleted file mode 100644 index 5a3dd49..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Height.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.firstinspires.ftc.teamcode.hardware; - -public class Height { - 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 deleted file mode 100644 index d9f0a3d..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/HopperPos.java +++ /dev/null @@ -1,7 +0,0 @@ -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 3e519e7..f3afaf8 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 @@ -1,5 +1,8 @@ package org.firstinspires.ftc.teamcode.hardware; +import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE; +import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN; + import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -36,9 +39,9 @@ public class Intake { public static double motorPower = 0; public Intake(HardwareMap hardwareMap, Position up) { - lServo = hardwareMap.get(Servo.class, "Left Servo"); + lServo = hardwareMap.get(Servo.class, "Left Intake Servo"); lServo.setDirection(Servo.Direction.REVERSE); - rServo = hardwareMap.get(Servo.class, "Right Servo"); + rServo = hardwareMap.get(Servo.class, "Right Intake Servo"); dcMotor = hardwareMap.get(DcMotor.class, "Intake Motor"); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/KhangSlides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/KhangSlides.java deleted file mode 100644 index ef90406..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/KhangSlides.java +++ /dev/null @@ -1,138 +0,0 @@ -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 KhangSlides { - - //Create and assign motors - public KhangSlides(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/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index f831541..9808845 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,15 +1,13 @@ package org.firstinspires.ftc.teamcode.hardware; import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.UP; -import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.CLOSE; -import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.OPEN; +import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE; +import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN; import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.teamcode.hardware.robby.Slides; import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.roadrunner.util.Encoder; @Config public class Robot { @@ -21,6 +19,7 @@ public class Robot { public Slides slides; public Arm arm; public DroneLauncher droneLauncher; + public double macroStartTime = 0; public int macroState = 0; public int runningMacro = 0; // 0 = no macro | 1 = tier 1 | 2 = tier 2 | 3 = tier 3 | 4 = return @@ -28,6 +27,10 @@ public class Robot { private boolean camEnabled = true; + public static double slideWait = 1.5; + public static double scoreWait = 1.25; + public static double armWait = 2.0; + //Name the objects public Robot(HardwareMap hardwareMap) { drive = new SampleMecanumDrive(hardwareMap); @@ -49,12 +52,11 @@ public class Robot { macroState ++; break; case(1): - if (runTime > macroStartTime + 1) { + if (runTime > macroStartTime + slideWait || slides.atTarget()) { macroState ++; } break; case(2): - macroStartTime = runTime; arm.setArmPos(Arm.Position.SCORE); arm.setWristPos(Arm.Position.SCORE); macroState = 0; @@ -71,9 +73,8 @@ public class Robot { arm.setDoor(OPEN); macroState++; break; - //Ind_sleeper case(1): - if (runTime > macroStartTime + 1) { + if (runTime > macroStartTime + scoreWait) { macroState ++; } break; @@ -84,12 +85,11 @@ public class Robot { macroState++; break; case(3): - if (runTime > macroStartTime + 1.1) { + if (/*runTime > macroStartTime + armWait || */arm.armAtPosition()) { macroState ++; } break; case(4): - macroStartTime = runTime; slides.setTarget(pos); macroState = 0; lastMacro = runningMacro; @@ -104,6 +104,6 @@ public class Robot { } public String getTelemetry() { - return "Telemetry?"; + return arm.getTelemetry(); } } \ No newline at end of file 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 deleted file mode 100644 index a4f944c..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SlidePos.java +++ /dev/null @@ -1,8 +0,0 @@ -package org.firstinspires.ftc.teamcode.hardware; - -public class SlidePos { - - public enum SPosition { - 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/Slides.java similarity index 91% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/robby/Slides.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java index 5eebc85..0a33c2a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/robby/Slides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.hardware.robby; +package org.firstinspires.ftc.teamcode.hardware; import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.controller.PIDController; @@ -19,12 +19,12 @@ public class Slides { public static PIDController controller = new PIDController(p, i, d); public static int targetMin = -10; - public static int targetMax = 1000; + public static int targetMax = 555; public static int down = 0; - public static int tier1 = 200; - public static int tier2 = 350; - public static int tier3 = 500; + public static int tier1 = 350; + public static int tier2 = 450; + public static int tier3 = 550; private int target = 0; @@ -36,13 +36,13 @@ public class Slides { slide = hardwareMap.get(DcMotor.class, "Right Slide Motor"); slide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); slide.setMode(DcMotor.RunMode.RUN_USING_ENCODER); -// slide.setDirection(DcMotorSimple.Direction.REVERSE); + slide.setDirection(DcMotorSimple.Direction.REVERSE); slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); slide2 = hardwareMap.get(DcMotor.class, "Left Slide Motor"); slide2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); slide2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - slide2.setDirection(DcMotorSimple.Direction.REVERSE); +// slide2.setDirection(DcMotorSimple.Direction.REVERSE); slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); } 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/PoseStorage.java similarity index 76% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/PoseStorage.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/PoseStorage.java index cec0763..73aa2d7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/PoseStorage.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/PoseStorage.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.opmode.teleop; +package org.firstinspires.ftc.teamcode.opmode; import com.acmerobotics.roadrunner.geometry.Pose2d; 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 49b31cd..b3cf8f9 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 @@ -3,8 +3,10 @@ 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.Arm; +import org.firstinspires.ftc.teamcode.hardware.Intake; import org.firstinspires.ftc.teamcode.hardware.Robot; -import org.firstinspires.ftc.teamcode.hardware.robby.Slides; +import org.firstinspires.ftc.teamcode.hardware.Slides; import org.firstinspires.ftc.teamcode.util.CameraPosition; import java.util.ArrayList; @@ -23,7 +25,7 @@ public abstract class AbstractAuto extends LinearOpMode { public void runOpMode() { // telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - telemetry.addLine("Initializing Robot..."); + telemetry.addLine("Initializing..."); telemetry.update(); setCameraPosition(); @@ -32,9 +34,9 @@ public abstract class AbstractAuto extends LinearOpMode { makeTrajectories(); -// while (robot.camera.getFrameCount() < 1) { -// idle(); -// } + while (robot.camera.getFrameCount() < 1) { + idle(); + } // wait for start while (!(isStarted() || isStopRequested())) { @@ -222,6 +224,54 @@ public abstract class AbstractAuto extends LinearOpMode { }); } + public void intakeStack(Intake.Position stackHeight1, Intake.Position stackHeight2) { + steps.add(new Step("Intaking Stack") { + @Override + public void start() { + robot.intake.setDcMotor(0.5); + robot.arm.setDoor(Arm.DoorPosition.OPEN); + } + + @Override + public void whileRunning() { + switch(macroState) { + case 0: + macroStart = currentRuntime; + robot.intake.setpos(stackHeight1); + macroState++; + break; + case 1: + if (currentRuntime > macroStart + 1.0) { + macroState++; + } + break; + case 2: + robot.intake.setpos(stackHeight2); + macroState++; + break; + case 3: + if (currentRuntime > macroStart + 1.0) { + macroState++; + } + break; + case 4: + macroState = 0; + } + } + + @Override + public void end() { + robot.intake.setDcMotor(0); + robot.arm.setDoor(Arm.DoorPosition.CLOSE); + } + + @Override + public boolean isFinished() { + return macroState == 0; + } + }); + } + public void runIntake(double power, double timeout) { steps.add(new Step("Running Intake" + timeout + " seconds", timeout) { @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java index b97e512..5640391 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java @@ -4,7 +4,7 @@ import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import org.firstinspires.ftc.teamcode.hardware.robby.Slides; +import org.firstinspires.ftc.teamcode.hardware.Slides; @Autonomous(name = "Blue Left Auto") public class BlueLeftAuto extends AbstractAuto { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedRightAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedRightAuto.java new file mode 100644 index 0000000..ebdf22c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedRightAuto.java @@ -0,0 +1,98 @@ +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 org.firstinspires.ftc.teamcode.hardware.Intake; +import org.firstinspires.ftc.teamcode.hardware.Slides; +import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; + +@Autonomous(name = "Red Right Auto") +public class RedRightAuto extends AbstractAuto { + public Trajectory scorePurple; + public Trajectory scoreYellow; + public Trajectory load1; + public Trajectory score1; + public Trajectory parkRobot; + + @Override + public void makeTrajectories() { + Pose2d start = new Pose2d(12, -61.5, Math.toRadians(90)); + + Pose2d dropMiddle = new Pose2d(12, -40.5, Math.toRadians(90)); + + Pose2d depositPreload = new Pose2d(46, -36, Math.toRadians(180)); + + Pose2d lineup1 = new Pose2d(12, -14, Math.toRadians(180)); + Pose2d pickup1 = new Pose2d(-58, -14, Math.toRadians(180)); + + Pose2d driveup1 = new Pose2d(12-6, -14, Math.toRadians(180)); + Pose2d deposit1 = new Pose2d(48-6, -36-4, Math.toRadians(180)); + + Pose2d park = new Pose2d(48, -12, Math.toRadians(180)); + + scorePurple = robot.drive.trajectoryBuilder(start) + .lineToLinearHeading(dropMiddle) + .build(); + + scoreYellow = robot.drive.trajectoryBuilder(scorePurple.end()) + .lineToLinearHeading(depositPreload) + .build(); + + load1 = robot.drive.trajectoryBuilder(scoreYellow.end()) + .splineToConstantHeading(lineup1.vec(), lineup1.getHeading(), + SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL) + ) + .lineToLinearHeading(pickup1, + SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL) + ) + .build(); + + score1 = robot.drive.trajectoryBuilder(load1.end(), true) + .lineToLinearHeading(driveup1, + SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL) + ) + .splineToConstantHeading(deposit1.vec(), deposit1.getHeading(), + SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL) + ) + .build(); + + parkRobot = robot.drive.trajectoryBuilder(score1.end()) + .lineToLinearHeading(park) + .build(); + + robot.drive.setPoseEstimate(start); + } + + @Override + public void initializeSteps(int location) { + followTrajectory(scorePurple); + runIntake(-0.4, 0.5); +// followAndExtend(scoreYellow, Slides.Position.TIER1); +// followAndRetract(load1, Slides.Position.DOWN); + followTrajectory(scoreYellow); + followTrajectory(load1); + intakeStack(Intake.Position.STACK5, Intake.Position.STACK4); + followAndExtend(score1, Slides.Position.TIER1); + followAndRetract(parkRobot, Slides.Position.DOWN); + } + + @Override + public void setAlliance() { + } + + @Override + public void setCameraPosition() { + } + + @Override + public boolean useCamera() { + return false; + } +} 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 index b6ea1f9..2deaa24 100644 --- 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 @@ -8,14 +8,9 @@ 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; + // macro variables + public double macroStart; + public int macroState; // Constructors public Step(String telemetry) { 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 deleted file mode 100644 index b3a91f1..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/AbstractTeleOp.java +++ /dev/null @@ -1,304 +0,0 @@ -//package org.firstinspires.ftc.teamcode.opmode.teleop; -// -//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; -// -//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); -// } -// -// 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 deleted file mode 100644 index cc710f1..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeam.java +++ /dev/null @@ -1,11 +0,0 @@ -//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/ExperimentalKhangMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/ExperimentalKhangMain.java deleted file mode 100644 index 198019b..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/ExperimentalKhangMain.java +++ /dev/null @@ -1,257 +0,0 @@ -//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 { -// 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; -//// } -// -// //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; -// } -// -// //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/NewTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java similarity index 75% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/NewTeleop.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java index 71317bb..6a76404 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/NewTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java @@ -1,7 +1,7 @@ package org.firstinspires.ftc.teamcode.opmode.teleop; -import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.CLOSE; -import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.OPEN; +import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE; +import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; @@ -11,14 +11,15 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.controller.Controller; import org.firstinspires.ftc.teamcode.hardware.Intake; import org.firstinspires.ftc.teamcode.hardware.Robot; -import org.firstinspires.ftc.teamcode.hardware.robby.Slides; +import org.firstinspires.ftc.teamcode.hardware.Slides; @Config @TeleOp(name = "Main TeleOp", group = "OpModes") -public class NewTeleop extends OpMode { +public class MainTeleOp extends OpMode { public static double normal = 0.5; public static double turbo = 1; + public static double intakeMax = 0.65; private Robot robot; private Controller controller1; @@ -30,9 +31,14 @@ public class NewTeleop extends OpMode { controller2 = new Controller(gamepad2); this.robot = new Robot(hardwareMap); - robot.intake.setpos(Intake.Position.STACK1); +// robot.intake.setpos(Intake.Position.STACK1); - telemetry.addLine("Started"); + while (robot.camera.getFrameCount() < 1) { + telemetry.addLine("Initializing..."); + telemetry.update(); + } + + telemetry.addLine("Initialized"); telemetry.update(); } @@ -55,7 +61,7 @@ public class NewTeleop extends OpMode { robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); // Driver 2 - robot.intake.setDcMotor(gamepad2.right_trigger); + robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax); if (controller2.getRightBumper().isJustPressed()) { robot.intake.incrementPos(); } @@ -64,7 +70,7 @@ public class NewTeleop extends OpMode { } // Drone launcher - if (controller1.getA().isPressed()) { + if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { this.robot.droneLauncher.launch(); } else { this.robot.droneLauncher.reset(); @@ -74,20 +80,21 @@ public class NewTeleop extends OpMode { switch (robot.runningMacro) { case (0): // manual mode robot.slides.increaseTarget(controller2.getLeftStick().getY()); - if (controller2.getX().isJustPressed()) { - robot.runningMacro = 1; - } else if (controller2.getY().isJustPressed()) { - robot.runningMacro = 2; - } else if (controller2.getB().isJustPressed()) { - robot.runningMacro = 3; - } else if (controller2.getA().isJustPressed()) { - robot.runningMacro = 4; - } if (robot.intake.getPower() >= 0.01) { robot.arm.setDoor(OPEN); } else { robot.arm.setDoor(CLOSE); } + + if (controller2.getX().isJustPressed()) { + robot.runningMacro = 1; + } else if (controller2.getY().isJustPressed()) { + robot.runningMacro = 2; + } else if (controller2.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { + robot.runningMacro = 3; + } else if (controller2.getA().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { + robot.runningMacro = 4; + } break; case (1): robot.extendMacro(Slides.Position.TIER1, getRuntime()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Meet1TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Meet1TeleOp.java deleted file mode 100644 index b6f67f5..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Meet1TeleOp.java +++ /dev/null @@ -1,260 +0,0 @@ -//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.Arm; -//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; -//import com.qualcomm.robotcore.util.ElapsedTime; -// -//@Config -//@TeleOp(name = "Meet 1 TeleOp", group = "OpModes") -//public class Meet1TeleOp extends OpMode { -// -// //turbo mode -// public static double normal = 0.5; -// public static double turbo = 1; -// -// //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 Arm.Position CurrentApos = Arm.Position.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() { -// -// //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 make sure we receive last line of code -// telemetry.update(); -// } -// -// @Override -// public void loop() { -// -// // 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)); -// -// //turbo activation -// if (controller1.getA().isJustPressed()){ -// x *= turbo; -// y *= turbo; -// z *= turbo; -// } else if (controller1.getA().isJustReleased()){ -// x *= normal; -// y *= normal; -// z *= normal; -// } -// -// //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; -// double manualSY = gamepad2.right_stick_y; -// -// //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.setArmPos(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 { -// planeServo.setPosition(0); -// } -// -// //make door rise as intake goes on -// if (intakeON >= 0.01) { -// CurrentDpos = DoorPos.DoorPosition.OPEN; -// } 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; -// } -// -// //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; -// } -// -// //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; -// } -// -// if (manualSY >=0.1) { -// CurrentSpos = CurrentSpos.MANUAL; -// } -// -//// //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/RedTeam.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeam.java deleted file mode 100644 index 2b76160..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeam.java +++ /dev/null @@ -1,11 +0,0 @@ -//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 deleted file mode 100644 index 684cd69..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Team.java +++ /dev/null @@ -1,6 +0,0 @@ -package org.firstinspires.ftc.teamcode.opmode.teleop; - - public enum Team { - RED, BLUE - } - 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 3e384fa..f3ca168 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 @@ -66,14 +66,11 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - // old is 35 35 120 120 - // new is 70 45 90 90 - public static double MAX_VEL = 60; - public static double MAX_ACCEL = 60; + public static double MAX_VEL = 50; + public static double MAX_ACCEL = 50; public static double MAX_ANG_VEL = Math.toRadians(120); public static double MAX_ANG_ACCEL = Math.toRadians(120); - public static double encoderTicksToInches(double ticks) { return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; } 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 d519761..6f702eb 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 @@ -63,7 +63,7 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer { parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backLeft")); // parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backRight")); - perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "Intake Motor")); + perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft")); // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) perpendicularEncoder.setDirection(Encoder.Direction.REVERSE);