diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java index b0311cc..5a105ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java @@ -16,8 +16,8 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.ServoPositions; -import org.firstinspires.ftc.teamcode.constants.TeleStart; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.teleop.TeleopV4; import org.firstinspires.ftc.teamcode.utilsv2.*; import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; @@ -37,8 +37,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { SpindexerTransferIntake spindexer; // Wait Times - public static double sortedShootTime = 2; - public static double rapidWaitTime = 0.25; + public static double sortedShootTime = 2.6; + public static double rapidWaitTime = 0.5; public static double rapidShootTime = 0.8; public static double openGateTime = 2.5; public static double pushTime = 2; @@ -57,23 +57,23 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { PathState pathState = PathState.PUSHBOT; // Poses - public static double startPoseX = 91.5, startPoseY = 15, startPoseH = 90; - public static double pushBotX = 97, pushBotY = 18, pushBotH = 100; - public static double shoot0ControlX = 94.29667812142038, shoot0ControlY = 55.03493699885454; - public static double shoot0X = 95, shoot0Y = 83, shoot0H = 0; - public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0; - public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165; - public static double openGateX = 131, openGateY = 74, openGateH = 0; - public static double shoot1ControlX = 112, shoot1ControlY = 75; - public static double shoot1X = 95, shoot1Y = 83, shoot1H = 0; - public static double drivePickup2X = 98, drivePickup2Y = 58.5, drivePickup2H = 0; - public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0; - public static double shoot2ControlX = 102, shoot2ControlY = 63; - public static double shoot2X = 95, shoot2Y = 83, shoot2H = 0; - public static double drivePickup3X = 98, drivePickup3Y = 34.5, drivePickup3H = 0; - public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0; - public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446; - public static double shoot3X = 84, shoot3Y = 120, shoot3H = -90; + public static double startPoseX = 12, startPoseY = -64, startPoseH = 90; + public static double pushBotX = 19, pushBotY = -63, pushBotH = 100; + public static double shoot0ControlX = 16.29667812142038, shoot0ControlY = -19.67493699885454; + public static double shoot0X = 19, shoot0Y = 10, shoot0H = 0; + public static double pickup1X = 54, pickup1Y = 10, pickup1H = 0; + public static double openGateControlX = 37.184421534937, openGateControlY = 2.24455899198165; + public static double openGateX = 59, openGateY = 2, openGateH = 0; + public static double shoot1ControlX = 40, shoot1ControlY = 3; + public static double shoot1X = 19, shoot1Y = 10, shoot1H = 0; + public static double drivePickup2X = 26, drivePickup2Y = -14, drivePickup2H = 0; + public static double pickup2X = 61, pickup2Y = -14.5, pickup2H = 0; + public static double shoot2ControlX = 30, shoot2ControlY = -9; + public static double shoot2X = 19, shoot2Y = 10, shoot2H = 0; + public static double drivePickup3X = 26, drivePickup3Y = -37.5, drivePickup3H = 0; + public static double pickup3X = 61, pickup3Y = -37.5, pickup3H = 0; + public static double shoot3ControlX = 25.62371134020621, shoot3ControlY = -38.813287514318446; + public static double shoot3X = 12, shoot3Y = 40, shoot3H = -90; double[] xPoses = {startPoseX, pushBotX, shoot0ControlX, shoot0X, pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X, drivePickup2X, pickup2X, shoot2ControlX, shoot2X, @@ -193,7 +193,6 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { if (!follower.isBusy() || currentTime - timeStamp > pushTime){ follower.followPath(pushBot_shoot0, true); pathState = PathState.DRIVE_SHOOT0; - timeStamp = currentTime; } break; case DRIVE_SHOOT0: @@ -201,6 +200,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { timeStamp = currentTime; pathState = PathState.WAIT_SHOOT0; spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); + } else if (follower.isBusy()){ + timeStamp = currentTime; } break; case WAIT_SHOOT0: @@ -220,6 +221,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { shootY = shoot1Y; shootH = shoot1H; timeStamp = currentTime; + shooter.setFlywheelVelocity(2300); + robot.setHoodPos(0.68); } break; case OPENGATE: @@ -271,8 +274,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { break; case DRIVE_PICKUP3: if (!follower.isBusy()){ - shooter.setFlywheelVelocity(2300); - robot.setHoodPos(0.68); + shooter.setFlywheelVelocity(2200); + robot.setHoodPos(0.75); follower.followPath(drivePickup3_pickup3, intakePower, false); pathState = PathState.PICKUP3; } @@ -302,7 +305,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { } // Used for changing alliance - private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);} + private double adjustXPoseBasedOnAlliance(double pose) {return -pose;} private double adjustHeadingBasedOnAlliance(double heading){ heading = 180 - heading; while (heading > 180) {heading-=360;} @@ -321,7 +324,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); follower = Constants.createFollower(hardwareMap); sleep(1000); - follower.setStartingPose(new Pose(72,72,0)); + follower.setStartingPose(new Pose(0,0,0)); loopTimes = new MeasuringLoopTimes(); loopTimes.init(); turret = new Turret(robot); @@ -344,9 +347,14 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { if (gamepad1.crossWasPressed() && !initializeRobot){ Color.redAlliance = !Color.redAlliance; shooter.setRedAlliance(Color.redAlliance); + } - for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);} - for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);} + if (!initializeRobot){ + if ((Color.redAlliance && xPoses[0] < 0) + || (!Color.redAlliance && xPoses[0] > 0)){ + for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);} + for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);} + } } if (gamepad1.triangleWasPressed()){ @@ -385,10 +393,11 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR); shooter.update(robot.voltage.getVoltage()); - follower.update(); + if (!isStopRequested()){ + follower.update(); + } pathStateMachine(); - Pose currentPose = follower.getPose(); - TeleStart.teleStart = currentPose; + TeleopV4.teleStart = follower.getPose(); spindexer.update(); @@ -400,9 +409,9 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); - TELE.addData("X:", currentPose.getX()); - TELE.addData("Y:", currentPose.getY()); - TELE.addData("H:", currentPose.getHeading()); + TELE.addData("X:", TeleopV4.teleStart.getX()); + TELE.addData("Y:", TeleopV4.teleStart.getY()); + TELE.addData("H:", TeleopV4.teleStart.getHeading()); TELE.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TeleStart.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TeleStart.java deleted file mode 100644 index c6d9801..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TeleStart.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.firstinspires.ftc.teamcode.constants; - -import com.pedropathing.geometry.Pose; - -public class TeleStart { - public static Pose teleStart = new Pose(72,72,0); -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index d7a8aad..b56d7db 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -42,8 +42,8 @@ public class Constants { public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, breakingStrength, 1); public static PinpointConstants localizerConstants = new PinpointConstants() - .forwardPodY(3.7795) - .strafePodX(-3.676) + .forwardPodY(-3.7795) + .strafePodX(-3.769) .distanceUnit(DistanceUnit.INCH) .hardwareMapName("pinpoint") .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index 7d55931..d2701b1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -148,7 +148,7 @@ class LocalizationTest extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72,72, 0)); + follower.setStartingPose(new Pose(0,0, 0)); } /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */ @@ -1272,7 +1272,7 @@ class CentripetalTuner extends OpMode { public void start() { follower.activateAllPIDFs(); forwards = new Path(new BezierCurve(new Pose(0,0), new Pose(Math.abs(DISTANCE) ,0), new Pose(Math.abs(DISTANCE) ,DISTANCE ))); - backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) ,0), new Pose(0,0))); + backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 0,DISTANCE + 0), new Pose(Math.abs(DISTANCE) ,0), new Pose(0,0))); backwards.setTangentHeadingInterpolation(); backwards.reverseHeadingInterpolation(); @@ -1382,14 +1382,14 @@ class Circle extends OpMode { public void start() { circle = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(72, 72), new Pose(RADIUS + 72, 72), new Pose(RADIUS + 72, RADIUS + 72))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) - .addPath(new BezierCurve(new Pose(RADIUS + 72, RADIUS + 72), new Pose(RADIUS + 72, (2 * RADIUS) + 72), new Pose(72, (2 * RADIUS) + 72))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) - .addPath(new BezierCurve(new Pose(72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, RADIUS + 72))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) - .addPath(new BezierCurve(new Pose(-RADIUS + 72, RADIUS + 72), new Pose(-RADIUS + 72, 72), new Pose(72, 72))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(0, 0), new Pose(RADIUS + 0, 0), new Pose(RADIUS + 0, RADIUS + 0))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS + 0)) + .addPath(new BezierCurve(new Pose(RADIUS + 0, RADIUS + 0), new Pose(RADIUS + 0, (2 * RADIUS) + 0), new Pose(0, (2 * RADIUS) + 0))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS + 0)) + .addPath(new BezierCurve(new Pose(0, (2 * RADIUS) + 0), new Pose(-RADIUS + 0, (2 * RADIUS) + 0), new Pose(-RADIUS + 0, RADIUS + 0))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS + 0)) + .addPath(new BezierCurve(new Pose(-RADIUS + 0, RADIUS + 0), new Pose(-RADIUS + 0, 0), new Pose(0, 0))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS + 0)) .build(); follower.followPath(circle); } @@ -1406,7 +1406,7 @@ class Circle extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72, 72)); + follower.setStartingPose(new Pose(0, 0)); } /** @@ -1639,8 +1639,8 @@ class OffsetsTuner extends OpMode { telemetry.addLine("Total Angle: " + follower.getTotalHeading()); telemetry.addLine("The following values are the offsets in inches that should be applied to your localizer."); - telemetry.addLine("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0)); - telemetry.addLine("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0)); + telemetry.addLine("strafeX: " + ((0.0-follower.getPose().getX()) / 2.0)); + telemetry.addLine("forwardY: " + ((0.0-follower.getPose().getY()) / 2.0)); telemetry.update(); drawCurrentAndHistory(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index def390a..64ae441 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -9,7 +9,6 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.constants.Color; -import org.firstinspires.ftc.teamcode.constants.TeleStart; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; import org.firstinspires.ftc.teamcode.utilsv2.*; @@ -31,7 +30,8 @@ public class TeleopV4 extends LinearOpMode { ParkTilter parkTilter; MeasuringLoopTimes loopTimes; - public static Pose relocalizePose = new Pose(128, 83, 0); + public static Pose relocalizePose = new Pose(56, 11, 0); + public static Pose teleStart = new Pose(0,0,0); @Override public void runOpMode() throws InterruptedException { @@ -47,9 +47,9 @@ public class TeleopV4 extends LinearOpMode { commander = new VelocityCommander(); drivetrain = new Drivetrain(robot, TELE); follower = Constants.createFollower(hardwareMap); - follower.setStartingPose(TeleStart.teleStart); + follower.setStartingPose(teleStart); sleep(500); - follower.setPose(TeleStart.teleStart); + follower.setPose(teleStart); follower.update(); flywheel = new Flywheel(robot); @@ -73,6 +73,8 @@ public class TeleopV4 extends LinearOpMode { limelightUsed = true; TELE.addLine("Initialization is done"); + TELE.addData("Starting Position", follower.getPose()); + TELE.addData("TELE START", teleStart); TELE.update(); waitForStart(); @@ -89,10 +91,12 @@ public class TeleopV4 extends LinearOpMode { if (gamepad1.crossWasPressed()){ if (Color.redAlliance){ - relocalizePose = new Pose(128, 83, 0); + relocalizePose = new Pose(57.5, 5, 0); } else { - relocalizePose = new Pose(16, 83, 180); + relocalizePose = new Pose(-57.5, 5, Math.toRadians(180)); } + follower.setPose(relocalizePose); + sleep(500); gamepad1.rumble(100); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java index 170c7f2..0dc9c5e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java @@ -131,7 +131,7 @@ public class Hardware_Tester extends LinearOpMode { NormalizedRGBA revColor = robot.revSensor.getNormalizedColors(); TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.CM)); - TELE.addData("REV Green", revColor.green / (revColor.red + revColor.blue + revColor.green)); + TELE.addData("REV Green", revColor.blue / (revColor.green + revColor.blue + revColor.red)); TELE.addData("Voltage Sensor", robot.voltage.getVoltage()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java index fedc78a..bce8280 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java @@ -48,9 +48,9 @@ public class NewShooterTest extends LinearOpMode { commander = new VelocityCommander(); drivetrain = new Drivetrain(robot, TELE); follower = Constants.createFollower(hardwareMap); - follower.setStartingPose(new Pose(72,72,0)); + follower.setStartingPose(new Pose(0,0,0)); sleep(500); - follower.setPose(new Pose(72,72,0)); + follower.setPose(new Pose(0,0,0)); flywheel = new Flywheel(robot); turret = new Turret(robot); @@ -86,11 +86,11 @@ public class NewShooterTest extends LinearOpMode { if (gamepad1.crossWasPressed()){ if (Color.redAlliance){ - TeleopV4.relocalizePose = new Pose(128, 83, 0); + TeleopV4.relocalizePose = new Pose(56, 11, 0); } else { - TeleopV4.relocalizePose = new Pose(16, 83, 180); + TeleopV4.relocalizePose = new Pose(-56, 11, 180); } - gamepad1.rumble(100); + follower.setPose(TeleopV4.relocalizePose); gamepad1.rumble(100); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java index 45d5fdc..6dea819 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -6,6 +6,8 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.pedropathing.follower.Follower; +import org.firstinspires.ftc.teamcode.constants.Color; + @Config public class Shooter { @@ -42,14 +44,14 @@ public class Shooter { this.red = input; if (this.red) { - goalX = 144; - turretGoalX = 140; + goalX = 72; + turretGoalX = 68; } else { - goalX = 0; - turretGoalX = 8; + goalX = -72; + turretGoalX = -68; } - goalY = 144; - turretGoalY = 132; + goalY = 72; + turretGoalY = 68; } private double flywheelVelocity = 0.0; @@ -89,7 +91,7 @@ public class Shooter { private final double shooterDistFromCenter = 1.545; public void update(double voltage) { - + setRedAlliance(Color.redAlliance); switch (state) { case NOTHING: break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java index a939e92..9b43763 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java @@ -53,8 +53,8 @@ public class SpindexerTransferIntake { } int[] shootOrder = {0, 1, 2}; - private final double sensorDistanceThreshold = 6.0; - private final long pulseTime = 100; // ms + final double sensorDistanceThreshold = 5.3; + final long pulseTime = 100; // ms private DesiredPattern desiredPattern = DesiredPattern.GPP; @@ -203,8 +203,8 @@ public class SpindexerTransferIntake { private RapidMode rapidMode = RapidMode.INTAKE; private SortedIntakeStates sortedIntakeStates = SortedIntakeStates.IDLE; private BallStates[] ballColors = {BallStates.UNKNOWN, BallStates.UNKNOWN, BallStates.UNKNOWN}; - private final double greenThresh = 0.39; - private final double spinMovementTime = 250; + final double greenThresh = 0.39; + final double spinMovementTime = 250; /** * Time when current state was entered. @@ -571,7 +571,7 @@ public class SpindexerTransferIntake { break; case WAIT_FOR_1: - if (shootStateTime() > 250) { + if (shootStateTime() > 400) { setShootState( SortedShootState.SHOOT_1 @@ -618,7 +618,7 @@ public class SpindexerTransferIntake { break; case WAIT_FOR_2: - if (shootStateTime() > 250) { + if (shootStateTime() > 400) { setShootState( SortedShootState.SHOOT_2 @@ -664,7 +664,7 @@ public class SpindexerTransferIntake { break; case WAIT_FOR_3: - if (shootStateTime() > 250) { + if (shootStateTime() > 400) { setShootState( SortedShootState.SHOOT_3