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 89cbb8c..7e9755a 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,56 +1,56 @@ -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 com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.HardwareMap; - -public class Drive { - private DcMotor frontLeft; - private DcMotor frontRight; - private DcMotor backLeft; - private 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.setDirection(DcMotor.Direction.REVERSE); - this.frontRight.setDirection(DcMotor.Direction.FORWARD); - this.backLeft.setDirection(DcMotor.Direction.REVERSE); - this.backRight.setDirection(DcMotor.Direction.FORWARD); - this.frontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - this.frontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - this.backLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - this.backRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - } - - public void setInput(double x, double y, double z) { - // instantiate motor power variables - double flPower, frPower, blPower, brPower; - - flPower = x + y + z; - frPower = -x + y - z; - blPower = -x + y + z; - brPower = x + y - z; - - double max = Math.max(Math.max(flPower, frPower), Math.max(blPower, brPower)); - if (max > 1) { - flPower /= max; - frPower /= max; - blPower /= max; - brPower /= max; - } - - // actually set the motor powers - frontLeft.setPower(flPower); - frontRight.setPower(frPower); - backLeft.setPower(blPower); - backRight.setPower(brPower); - } -} \ No newline at end of file +//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 com.qualcomm.robotcore.hardware.DcMotor; +//import com.qualcomm.robotcore.hardware.HardwareMap; +// +//public class Drive { +// private DcMotor frontLeft; +// private DcMotor frontRight; +// private DcMotor backLeft; +// private 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.setDirection(DcMotor.Direction.REVERSE); +// this.frontRight.setDirection(DcMotor.Direction.FORWARD); +// this.backLeft.setDirection(DcMotor.Direction.REVERSE); +// this.backRight.setDirection(DcMotor.Direction.FORWARD); +// this.frontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// this.frontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// this.backLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// this.backRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// } +// +// public void setInput(double x, double y, double z) { +// // instantiate motor power variables +// double flPower, frPower, blPower, brPower; +// +// flPower = x + y + z; +// frPower = -x + y - z; +// blPower = -x + y + z; +// brPower = x + y - z; +// +// double max = Math.max(Math.max(flPower, frPower), Math.max(blPower, brPower)); +// if (max > 1) { +// flPower /= max; +// frPower /= max; +// blPower /= max; +// brPower /= max; +// } +// +// // actually set the motor powers +// frontLeft.setPower(flPower); +// frontRight.setPower(frPower); +// backLeft.setPower(blPower); +// backRight.setPower(brPower); +// } +//} \ No newline at end of file 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 d2d6269..72a8806 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 @@ -60,7 +60,7 @@ public class Robot { this.arm = new Arm().init(hardwareMap); this.wrist = new Wrist().init(hardwareMap); this.claw = new Claw().init(hardwareMap); -// this.camera = new Camera(hardwareMap); + this.camera = new Camera(hardwareMap); this.plane = new Plane().init(hardwareMap); this.slides = new Slides().init(hardwareMap); // this.led = new Led().init(hardwareMap); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java index e282dcc..6dec286 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java @@ -71,7 +71,7 @@ public class DriveConstants { public static double MAX_VEL = 80; public static double MAX_ACCEL = 80; public static double MAX_ANG_VEL = Math.toRadians(360); - public static double MAX_ANG_ACCEL = Math.toRadians(660); + public static double MAX_ANG_ACCEL = Math.toRadians(360); /* * Adjust the orientations here to match your robot. See the FTC SDK documentation for details. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java index e750a9d..76b4ffc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java @@ -59,9 +59,9 @@ import java.util.List; * Simple mecanum drive hardware implementation for REV hardware. */ @Config -public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(20, 0, 1.5); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(10, 0, 2); +public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive { + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(3, 0, 2.5); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(12, 0, .5); public static double LATERAL_MULTIPLIER = 1; @@ -113,8 +113,8 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr this.leftFront.setDirection(DcMotor.Direction.REVERSE); - this.rightFront.setDirection(DcMotor.Direction.FORWARD); this.leftRear.setDirection(DcMotor.Direction.REVERSE); + this.rightFront.setDirection(DcMotor.Direction.FORWARD); this.rightRear.setDirection(DcMotor.Direction.FORWARD); this.leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); this.rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -332,8 +332,8 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr this.setWeightedDrivePower( new Pose2d( - gamepad1.left_stick_y * speedScale, - gamepad1.left_stick_x * speedScale, + -gamepad1.left_stick_y * speedScale, + -gamepad1.left_stick_x * speedScale, -gamepad1.right_stick_x * turnScale )); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java index 70be980..7628101 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java @@ -62,11 +62,11 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer { this.drive = drive; - parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightRear")); - perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftFront")); + parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftFront")); + perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightRear")); // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) - parallelEncoder.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/hardware/roadrunner/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TurnTest.java index 2874259..d22efd1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TurnTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TurnTest.java @@ -36,13 +36,12 @@ public class TurnTest extends LinearOpMode { if (controller.wasJustPressed(GamepadKeys.Button.B)) { TrajectoryBuilder builder = drive.trajectoryBuilder(drive.getPoseEstimate()); - if (zig) { - zig = false; - builder.forward(DISTANCE); - } else { - zig = true; - builder.forward(-DISTANCE); - } + builder.forward(DISTANCE); + drive.followTrajectory(builder.build()); + } + if (controller.wasJustPressed(GamepadKeys.Button.Y)) { + TrajectoryBuilder builder = drive.trajectoryBuilder(drive.getPoseEstimate()); + builder.back(DISTANCE); drive.followTrajectory(builder.build()); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java index e35e384..da0e91b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java @@ -7,16 +7,14 @@ 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.hardware.roadrunner.drive.DriveConstants; -import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; @Autonomous(name = "autoRed") public class AutoRed extends LinearOpMode { protected Pose2d initialPosition; private Robot robot; - private String randomization; - private String parkLocation; + private String randomization = "CENTER"; + private String parkLocation = "LEFT"; //Pose2ds //Preloads @@ -56,19 +54,13 @@ public class AutoRed extends LinearOpMode { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); switch (randomization) { case "LEFT": - builder.lineToLinearHeading(LEFT_BOARD, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); + builder.lineToLinearHeading(LEFT_BOARD); break; case "CENTER": - builder.lineToLinearHeading(CENTER_BOARD, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); + builder.lineToLinearHeading(CENTER_BOARD); break; case "RIGHT": - builder.lineToLinearHeading(RIGHT_BOARD, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); + builder.lineToLinearHeading(RIGHT_BOARD); break; } builder.addTemporalMarker(.2, robot.getArm()::armScore); @@ -114,18 +106,22 @@ public class AutoRed extends LinearOpMode { // Do super fancy chinese shit while (!this.isStarted()) { this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); - randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); - parkLocation(); +// randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); +// parkLocation(); + randomization = "CENTER"; this.telemetry.addData("Park Position", parkLocation); this.telemetry.update(); } - - scorePreloadOne(); - boardScore(); - sleep(250); - this.robot.getClaw().open(); - sleep(250); - park(); + while (!isStopRequested()) { + robot.update(); + scorePreloadOne(); + boardScore(); + sleep(250); + this.robot.getClaw().open(); + sleep(250); + park(); + stop(); + } } } 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 e7c10ef..0531c87 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 @@ -37,18 +37,8 @@ public class Constants { public static final Detection INVALID_DETECTION = new Detection(new Size(0, 0), 0); // Hardware Name Constants - public static final String WHEEL_FRONT_LEFT = "frontLeft"; - public static final String WHEEL_FRONT_RIGHT = "frontRight"; - public static final String WHEEL_BACK_LEFT = "backLeft"; - public static final String WHEEL_BACK_RIGHT = "backRight"; - public static final String ARM = "wobbler"; + public static final String CLAW = "claw"; - public static final String INTAKE = "intake"; - public static final String INTAKE_SECONDARY = "secondary"; - public static final String INTAKE_SHIELD = "shield"; - public static final String SHOOTER = "wheel"; - public static final String PUSHER = "pusher"; - 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 LEFTHANG = "leftHang";