diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index 66c0654..bb1dd5b 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java @@ -2,7 +2,9 @@ package opmodes; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_DELTA; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_LIFT_DELTA; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_DELTA; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_DELTA; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MAX; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -21,10 +23,12 @@ public class MainTeleOp extends OpMode { private boolean previousScrewReset = false; private boolean previousSlideUp = false; private boolean previousSlideDown = false; + private double currentScrewPosition = 1f; @Override public void init() { telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + this.clawArmPosition = PICKUP_ARM_MAX; this.robot = new Robot(this.hardwareMap); telemetry.addData("Status", "Initialized"); @@ -33,10 +37,13 @@ public class MainTeleOp extends OpMode { @Override public void loop() { // Drive - double x = gamepad1.left_stick_x; + double x = -gamepad1.left_stick_x; double y = -gamepad1.left_stick_y; - double z = gamepad1.right_stick_x; - this.robot.getDrive().setInput(x, y, z); + double z = -gamepad1.right_stick_x; + this.robot.getDrive().setInput(0, y, z); + + this.telemetry.addLine(this.robot.getDrive().getTelemetry()); + this.telemetry.update(); // Button Mappings boolean openClaw = gamepad2.b; // B @@ -57,23 +64,19 @@ public class MainTeleOp extends OpMode { // Claw if (openClaw) { this.robot.getClaw().open(); - } else { + } else if (!clawUp && !clawDown){ this.robot.getClaw().close(); } if (clawUp) { - this.clawArmPosition += CLAW_ARM_DELTA; + this.clawArmPosition = Math.min(1, this.clawArmPosition + CLAW_ARM_DELTA); this.robot.getClaw().setArmPosition(clawArmPosition); } else if (clawDown) { - this.clawArmPosition -= CLAW_ARM_DELTA; + this.robot.getClaw().open(); + this.clawArmPosition = Math.max(0, this.clawArmPosition - CLAW_ARM_DELTA); this.robot.getClaw().setArmPosition(clawArmPosition); } - // Robot Lift - if (raiseRobotLift) { - this.robot.getLift().raise(); - } else if (liftRobot) { - this.robot.getLift().lift(); - } +/* // Gantry if (!previousScrewArmToggle && screwArmToggle) { @@ -85,7 +88,8 @@ public class MainTeleOp extends OpMode { screwArmPos = !screwArmPos; } if (!previousScrewDeposit && screwDeposit) { - this.robot.getGantry().deposit(); + this.currentScrewPosition += GANTRY_SCREW_DELTA; + this.robot.getGantry().setScrew(currentScrewPosition); } else if (!previousScrewReset && screwReset) { this.robot.getGantry().resetScrew(); } @@ -99,10 +103,17 @@ public class MainTeleOp extends OpMode { this.robot.getGantry().setX(this.robot.getGantry().getX() - GANTRY_X_DELTA); } - + // Robot Lift + if (raiseRobotLift) { + this.robot.getLift().raise(); + } else if (liftRobot) { + this.robot.getLift().lift(); + } + */ this.previousSlideUp = slideUp; this.previousScrewArmToggle = screwArmToggle; this.previousScrewDeposit = screwDeposit; this.previousScrewReset = screwReset; + } } 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 9261fa1..ca79a12 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 @@ -1,7 +1,7 @@ package org.firstinspires.ftc.teamcode.hardware; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.WEBCAM_NAME; import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION; -import static org.firstinspires.ftc.teamcode.util.Constants.TARGETING_WEBCAM; import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_HEIGHT; import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_ROTATION; import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_WIDTH; @@ -26,7 +26,7 @@ public class Camera { public void initTargetingCamera() { int targetingCameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); - this.targetingCamera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, TARGETING_WEBCAM), targetingCameraMonitorViewId); + this.targetingCamera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, WEBCAM_NAME), targetingCameraMonitorViewId); this.targetingPipeline = new TargetingPipeline(); targetingCamera.setPipeline(targetingPipeline); targetingCamera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java index 6a92832..ca299a2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java @@ -21,6 +21,8 @@ public class Claw { this.armLeft = hardwareMap.get(Servo.class, CLAW_ARM_LEFT_NAME); this.armRight = hardwareMap.get(Servo.class, CLAW_ARM_RIGHT_NAME); this.armRight.setDirection(Servo.Direction.REVERSE); + this.setArmPosition(PICKUP_ARM_MAX); + this.close(); } public void open() { @@ -40,6 +42,7 @@ public class Claw { } public void setArmPosition(double target) { + target = Math.min(PICKUP_ARM_MAX, Math.max(PICKUP_ARM_MIN, target)); this.armLeft.setPosition(target); this.armRight.setPosition(target); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java index d673664..6215c72 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java @@ -1,26 +1,28 @@ package org.firstinspires.ftc.teamcode.hardware; -import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_LEFT; -import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_RIGHT; -import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_LEFT; -import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_RIGHT; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.BACK_LEFT_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.BACK_RIGHT_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.FRONT_LEFT_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.FRONT_RIGHT_NAME; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; +import java.util.Locale; + public class Drive { - private DcMotor frontLeft; - private DcMotor frontRight; - private DcMotor backLeft; - private DcMotor backRight; + private final DcMotor frontLeft; + private final DcMotor frontRight; + private final DcMotor backLeft; + private final DcMotor backRight; public Drive(HardwareMap hardwareMap) { // Drive - this.frontLeft = hardwareMap.get(DcMotor.class, WHEEL_FRONT_LEFT); - this.frontRight = hardwareMap.get(DcMotor.class, WHEEL_FRONT_RIGHT); - this.backLeft = hardwareMap.get(DcMotor.class, WHEEL_BACK_LEFT); - this.backRight = hardwareMap.get(DcMotor.class, WHEEL_BACK_RIGHT); + this.frontLeft = hardwareMap.get(DcMotor.class, FRONT_LEFT_NAME); + this.frontRight = hardwareMap.get(DcMotor.class, FRONT_RIGHT_NAME); + this.backLeft = hardwareMap.get(DcMotor.class, BACK_LEFT_NAME); + this.backRight = hardwareMap.get(DcMotor.class, BACK_RIGHT_NAME); this.frontLeft.setDirection(DcMotor.Direction.REVERSE); this.frontRight.setDirection(DcMotor.Direction.FORWARD); this.backLeft.setDirection(DcMotor.Direction.REVERSE); @@ -55,6 +57,15 @@ public class Drive { backRight.setPower(brPower); } + public String getTelemetry() { + double flPower = this.frontLeft.getPower(); + double frPower = this.frontRight.getPower(); + double blPower = this.backLeft.getPower(); + double brPower = this.backRight.getPower(); + + return String.format("FL: %f, FR: %f, BL: %f, BR: %f", flPower, frPower, blPower, brPower); + } + public void setInput(Gamepad gamepad1, Gamepad gamepad2) { double x = gamepad1.left_stick_x; double y = -gamepad1.left_stick_y; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java index 0192f3f..d5adaad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java @@ -22,6 +22,7 @@ public class Gantry extends Slide { this.xServo = hardwareMap.get(Servo.class, GANTRY_X_NAME); this.armServo = hardwareMap.get(Servo.class, GANTRY_ARM_NAME); this.screwServo = hardwareMap.get(Servo.class, GANTRY_SCREW_NAME); + this.screwServo.setPosition(1); } public void setX(double x) { @@ -41,12 +42,12 @@ public class Gantry extends Slide { } public void resetScrew() { - this.currentScrewIndex = 0; - this.screwServo.setPosition(GANTRY_SCREW_POSITIONS[currentScrewIndex]); + this.screwServo.setPosition(0); } - public void deposit() { - this.screwServo.setPosition(GANTRY_SCREW_POSITIONS[this.currentScrewIndex--]); + public void setScrew(double target) { + double clamped = Math.min(1, Math.max(0, target)); + this.screwServo.setPosition(clamped); } public void center() { 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 52b0cfe..22c2aef 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,8 +8,11 @@ import lombok.Getter; public class Robot { +// @Getter +// private MecanumDrive drive; + @Getter - private MecanumDrive drive; + private Drive drive; @Getter private Gantry gantry; @@ -25,9 +28,10 @@ public class Robot { } private void init(HardwareMap hardwareMap) { - this.drive = new MecanumDrive(hardwareMap); - this.gantry = new Gantry(hardwareMap); +// this.drive = new MecanumDrive(hardwareMap); + this.drive = new Drive(hardwareMap); +// this.gantry = new Gantry(hardwareMap); this.claw = new Claw(hardwareMap); - this.lift = new RobotLift(hardwareMap); +// this.lift = new RobotLift(hardwareMap); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java index 2855961..8ee0d15 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java @@ -4,7 +4,12 @@ import com.acmerobotics.dashboard.config.Config; @Config public class RobotConstants { - public static final String SLIDE_MOTOR_NAME = "slide"; + public static final String FRONT_LEFT_NAME = "frontLeft"; + public static final String FRONT_RIGHT_NAME = "frontRight"; + public static final String BACK_LEFT_NAME = "backLeft"; + public static final String BACK_RIGHT_NAME = "backRight"; + public static final String LEFT_SLIDE_MOTOR_NAME = "slideLeft"; + public static final String RIGHT_SLIDE_MOTOR_NAME = "slideRight"; public static final String CLAW_ARM_LEFT_NAME = "clawArmLeft"; public static final String CLAW_ARM_RIGHT_NAME = "clawArmRight"; public static final String CLAW_NAME = "claw"; @@ -13,16 +18,17 @@ public class RobotConstants { public static final String GANTRY_SCREW_NAME = "screw"; public static final String ROBOT_LIFT_ROTATION_NAME = "liftRotation"; public static final String ROBOT_LIFT_LIFT_NAME = "liftLift"; + public static final String WEBCAM_NAME = "webcam"; // Slide public static int SLIDE_UP = 100; // Arm - public static double PICKUP_ARM_MIN = 0; - public static double PICKUP_ARM_MAX = 1; - public static double CLAW_MIN = 0; - public static double CLAW_MAX = 1; - public static double CLAW_ARM_DELTA = 0.05; + public static double PICKUP_ARM_MIN = 0.19; + public static double PICKUP_ARM_MAX = 0.75; + public static double CLAW_MIN = 0.9; + public static double CLAW_MAX = 0.6; + public static double CLAW_ARM_DELTA = 0.005; // Gantry public static double GANTRY_ARM_MIN = 0; @@ -31,6 +37,7 @@ public class RobotConstants { public static int GANTRY_LIFT_DELTA = 50; public static double GANTRY_X_DELTA = 0.01; public static double GANTRY_CENTER = 0.5; + public static double GANTRY_SCREW_DELTA = 0.01; // Robot Lift public static int LIFT_ROTATION_UP = 100; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java index 07326d6..458e930 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java @@ -1,30 +1,40 @@ package org.firstinspires.ftc.teamcode.hardware; -import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SLIDE_MOTOR_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LEFT_SLIDE_MOTOR_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.RIGHT_SLIDE_MOTOR_NAME; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; public class Slide { - protected final DcMotor lift; + protected final DcMotor left; + protected final DcMotor right; protected Slide(HardwareMap hardwareMap) { - this.lift = hardwareMap.get(DcMotor.class, SLIDE_MOTOR_NAME); - this.lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - this.lift.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + this.left = hardwareMap.get(DcMotor.class, LEFT_SLIDE_MOTOR_NAME); + this.left.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.left.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + this.right = hardwareMap.get(DcMotor.class, RIGHT_SLIDE_MOTOR_NAME); + this.right.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.right.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } public void setTarget(int target) { - this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); - this.lift.setTargetPosition(target); - this.lift.setPower(1); + this.left.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.left.setTargetPosition(target); + this.left.setPower(1); + + this.right.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.right.setTargetPosition(target); + this.right.setPower(1); } public void setInput(double x) { - this.lift.setPower(x); + } public int getSlidePosition() { - return this.lift.getCurrentPosition(); + return this.left.getCurrentPosition(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java index b08629b..9702f5e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java @@ -8,7 +8,7 @@ import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.teamcode.util.Encoder; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.Encoder; import java.util.Arrays; import java.util.List; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java index f0ddbbe..5dbb502 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java @@ -73,17 +73,6 @@ public final class LogFiles { public double mecHeadingI = MecanumDrive.HEADING_PID.kI; public double mecHeadingD = MecanumDrive.HEADING_PID.kD; public double mecLateralMultiplier = MecanumDrive.LATERAL_MULTIPLIER; - - public double tankAxialP = SampleTankDrive.AXIAL_PID.kP; - public double tankAxialI = SampleTankDrive.AXIAL_PID.kI; - public double tankAxialD = SampleTankDrive.AXIAL_PID.kD; - public double tankCrossTrackP = SampleTankDrive.CROSS_TRACK_PID.kP; - public double tankCrossTrackI = SampleTankDrive.CROSS_TRACK_PID.kI; - public double tankCrossTrackD = SampleTankDrive.CROSS_TRACK_PID.kD; - public double tankHeadingP = SampleTankDrive.HEADING_PID.kP; - public double tankHeadingI = SampleTankDrive.HEADING_PID.kI; - public double tankHeadingD = SampleTankDrive.HEADING_PID.kD; - public double trackingTicksPerRev = StandardTrackingWheelLocalizer.TICKS_PER_REV; public double trackingWheelRadius = StandardTrackingWheelLocalizer.WHEEL_RADIUS; public double trackingGearRatio = StandardTrackingWheelLocalizer.GEAR_RATIO;