diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java index c10685d..48a3ba8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java @@ -22,28 +22,28 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; -import org.firstinspires.ftc.teamcode.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utilsv2.Flywheel; import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; -import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utilsv2.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Spindexer; import org.firstinspires.ftc.teamcode.utils.Targeting; -import org.firstinspires.ftc.teamcode.utils.Turret; +import org.firstinspires.ftc.teamcode.utilsv2.Turret; import java.util.List; @Config -@Autonomous (preselectTeleOp = "TeleopV3") +@Autonomous (preselectTeleOp = "TeleopV4") public class Auto12BallPedroPathing extends LinearOpMode { Robot robot; MultipleTelemetry TELE; - Flywheel flywheel; - Targeting targeting; - Targeting.Settings targetingSettings; +// Flywheel flywheel; +// Targeting targeting; +// Targeting.Settings targetingSettings; Follower follower; - Turret turret; - Spindexer spindexer; - Servos servos; +// Turret turret; +// Spindexer spindexer; +// Servos servos; MeasuringLoopTimes loopTimes; // Wait Times @@ -222,10 +222,10 @@ public class Auto12BallPedroPathing extends LinearOpMode { driveShoot(PathState.WAIT_SHOOT3, currentTime); break; case WAIT_SHOOT3: - if (spindexer.shootAllComplete()){ - spindexer.resetSpindexer(); - TELE.addLine("Done Auto"); - } +// if (spindexer.shootAllComplete()){ +// spindexer.resetSpindexer(); +// TELE.addLine("Done Auto"); +// } break; default: break; @@ -237,21 +237,21 @@ public class Auto12BallPedroPathing extends LinearOpMode { private void intakePowerDown(double stamp, double currentTime) { double pow = (stamp - currentTime) / 2; // adjust denominator to see how much time to adjust if (pow < -1) {pow = 0;} - spindexer.setIntakePower(pow); +// spindexer.setIntakePower(pow); } private void driveShoot(PathState nextState, double currentTime){ if (!follower.isBusy()){ pathState = nextState; timeStamp = currentTime; - spindexer.prepareShootAllContinous(); +// spindexer.prepareShootAllContinous(); } } private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) { - if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime) { - spindexer.resetSpindexer(); + if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() || +// spindexer.resetSpindexer(); pathState = nextState; follower.followPath(nextPath, true); - spindexer.setIntakePower(1); +// spindexer.setIntakePower(1); } } private void drivePickup(PathState nextState, PathChain nextPath) { @@ -286,24 +286,25 @@ public class Auto12BallPedroPathing extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - robot = new Robot(hardwareMap); + Robot.resetInstance(); + robot = Robot.getInstance(hardwareMap); List allHubs = hardwareMap.getAll(LynxModule.class); for (LynxModule hub : allHubs) { hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); } TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - flywheel = new Flywheel(hardwareMap); - targeting = new Targeting(); - targetingSettings = new Targeting.Settings(0,0); +// flywheel = new Flywheel(hardwareMap); +// targeting = new Targeting(); +// targetingSettings = new Targeting.Settings(0,0); follower = Constants.createFollower(hardwareMap); follower.setStartingPose(new Pose(72,72,0)); - turret = new Turret(robot, TELE, robot.limelight); - spindexer = new Spindexer(hardwareMap); - servos = new Servos(hardwareMap); +// turret = new Turret(robot, TELE, robot.limelight); +// spindexer = new Spindexer(hardwareMap); +// servos = new Servos(hardwareMap); loopTimes = new MeasuringLoopTimes(); loopTimes.init(); - robot.light.setPosition(Color.LightRed); +// robot.light.setPosition(Color.LightRed); boolean initializeRobot = false; while (opModeInInit()){ @@ -311,11 +312,11 @@ public class Auto12BallPedroPathing extends LinearOpMode { if (gamepad1.crossWasPressed() && !initializeRobot){ Color.redAlliance = !Color.redAlliance; - if (Color.redAlliance){ - robot.light.setPosition(Color.LightRed); - } else { - robot.light.setPosition(Color.LightBlue); - } +// if (Color.redAlliance){ +// robot.light.setPosition(Color.LightRed); +// } else { +// robot.light.setPosition(Color.LightBlue); +// } double[] xPoses = {startPoseX, shoot0X, drivePickup1X, pickup1X, shoot1X, @@ -338,8 +339,8 @@ public class Auto12BallPedroPathing extends LinearOpMode { buildPaths(); sleep(2000); - turret.setTurret(turrDefault); - servos.setSpinPos(spinStartPos); +// turret.setTurret(turrDefault); +// servos.setSpinPos(spinStartPos); } TELE.addData("Red Alliance?", Color.redAlliance); @@ -352,27 +353,27 @@ public class Auto12BallPedroPathing extends LinearOpMode { if (isStopRequested()) return; - robot.transfer.setPower(1); +// robot.transfer.setPower(1); limelightUsed = false; while (opModeIsActive()){ follower.update(); pathStateMachine(); Pose currentPose = follower.getPose(); - teleStartPoseX = currentPose.getX(); - teleStartPoseY = currentPose.getY(); - teleStartPoseH = Math.toDegrees(currentPose.getHeading()); - - turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH))); - targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate); - - double voltage = robot.voltage.getVoltage(); - flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); - flywheel.manageFlywheel(targetingSettings.flywheelRPM); - servos.setHoodPos(targetingSettings.hoodAngle); - - if (driveToShoot()){servos.setSpinPos(spinStartPos);} - else {spindexer.processIntake();} +// teleStartPoseX = currentPose.getX(); +// teleStartPoseY = currentPose.getY(); +// teleStartPoseH = Math.toDegrees(currentPose.getHeading()); +// +// turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH))); +// targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate); +// +// double voltage = robot.voltage.getVoltage(); +// flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); +// flywheel.manageFlywheel(targetingSettings.flywheelRPM); +// servos.setHoodPos(targetingSettings.hoodAngle); +// +// if (driveToShoot()){servos.setSpinPos(spinStartPos);} +// else {spindexer.processIntake();} for (LynxModule hub : allHubs) { hub.clearBulkCache(); 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 new file mode 100644 index 0000000..599865d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java @@ -0,0 +1,366 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.PathChain; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.utilsv2.Flywheel; +import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; +import org.firstinspires.ftc.teamcode.utilsv2.Robot; +import org.firstinspires.ftc.teamcode.utilsv2.Turret; + +import java.util.List; + +@Config +@Autonomous (preselectTeleOp = "TeleopV4") +public class Auto12Ball_Back_Sorted extends LinearOpMode { + Robot robot; + MultipleTelemetry TELE; + Follower follower; + MeasuringLoopTimes loopTimes; + + // Wait Times + public static double shootTime = 2; + public static double openGateTime = 1.5; + + // Extra Variables + public static double intakePower = 0.3; + double shootX, shootY, shootH; + + // Initialize path state machine + private enum PathState { + PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0, + PICKUP1, DRIVE_OPENGATE, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1, + DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2, + DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3 + } + PathState pathState = PathState.PUSHBOT; + + // Poses + public static double startPoseX = 84, startPoseY = 7, startPoseH = 90; + public static double pushBotX = 94, pushBotY = 9, pushBotH = 100; + public static double shoot0ControlX = 88.29667812142038, shoot0ControlY = 52.03493699885454; + public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0; + public static double pickup1ControlX = 109.29381443298968, pickup1ControlY = 82.70618556701031; + public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0; + public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165; + public static double openGateX = 129, openGateY = 74, openGateH = 0; + public static double shoot1ControlX = 112, shoot1ControlY = 75; + public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12; + public static double drivePickup2X = 102, 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 = 91, shoot2Y = 80, shoot2H = -50; + public static double drivePickup3X = 102, 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 = 105, shoot3H = -80; + Pose startPose, pushBot, shoot0Control, shoot0, + pickup1Control, pickup1, openGateControl, openGate, shoot1Control, shoot1, + drivePickup2, pickup2, shoot2Control, shoot2, + drivePickup3, pickup3, shoot3Control, shoot3; + private void initializePoses(){ + startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH)); + pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH)); + shoot0Control = new Pose(shoot0ControlX, shoot0ControlY); + shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H)); + pickup1Control = new Pose(pickup1ControlX, pickup1ControlY); + pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H)); + openGateControl = new Pose(openGateControlX, openGateControlY); + openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH)); + shoot1Control = new Pose(shoot1ControlX, shoot1ControlY); + shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H)); + drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H)); + pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H)); + shoot2Control = new Pose(shoot2ControlX, shoot2ControlY); + shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H)); + drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H)); + pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H)); + shoot3Control = new Pose(shoot3ControlX, shoot3ControlY); + shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H)); + } + + //Building Paths + PathChain startPose_pushBot, pushBot_shoot0, + shoot0_pickup1, pickup1_openGate, openGate_shoot1, + shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2, + shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3; + private void buildPaths(){ + startPose_pushBot = follower.pathBuilder() + .addPath(new BezierLine(startPose, pushBot)) + .setLinearHeadingInterpolation(startPose.getHeading(), pushBot.getHeading()) + .build(); + + pushBot_shoot0 = follower.pathBuilder() + .addPath(new BezierCurve(pushBot, shoot0Control, shoot0)) + .setLinearHeadingInterpolation(pushBot.getHeading(), shoot0.getHeading()) + .build(); + + shoot0_pickup1 = follower.pathBuilder() + .addPath(new BezierCurve(shoot0, pickup1Control, pickup1)) + .setLinearHeadingInterpolation(shoot0.getHeading(), pickup1.getHeading()) + .build(); + + pickup1_openGate = follower.pathBuilder() + .addPath(new BezierCurve(pickup1, openGateControl, openGate)) + .setLinearHeadingInterpolation(pickup1.getHeading(), openGate.getHeading()) + .build(); + + openGate_shoot1 = follower.pathBuilder() + .addPath(new BezierCurve(openGate, shoot1Control, shoot1)) + .setLinearHeadingInterpolation(openGate.getHeading(), shoot1.getHeading()) + .build(); + + shoot1_drivePickup2 = follower.pathBuilder() + .addPath(new BezierLine(shoot1, drivePickup2)) + .setLinearHeadingInterpolation(shoot1.getHeading(), drivePickup2.getHeading()) + .build(); + + drivePickup2_pickup2 = follower.pathBuilder() + .addPath(new BezierLine(drivePickup2, pickup2)) + .setLinearHeadingInterpolation(drivePickup2.getHeading(), pickup2.getHeading()) + .build(); + + pickup2_shoot2 = follower.pathBuilder() + .addPath(new BezierCurve(pickup2, shoot2Control, shoot2)) + .setLinearHeadingInterpolation(pickup2.getHeading(), shoot2.getHeading()) + .build(); + + shoot2_drivePickup3 = follower.pathBuilder() + .addPath(new BezierLine(shoot2, drivePickup3)) + .setLinearHeadingInterpolation(shoot2.getHeading(), drivePickup3.getHeading()) + .build(); + + drivePickup3_pickup3 = follower.pathBuilder() + .addPath(new BezierLine(drivePickup3, pickup3)) + .setLinearHeadingInterpolation(drivePickup3.getHeading(), pickup3.getHeading()) + .build(); + + pickup3_shoot3 = follower.pathBuilder() + .addPath(new BezierCurve(pickup3, shoot3Control, shoot3)) + .setLinearHeadingInterpolation(pickup3.getHeading(), shoot3.getHeading()) + .build(); + } + + //Path State Machine + private boolean startAuto = true; + private double timeStamp = 0; + private void pathStateMachine(){ + double currentTime = (double) System.currentTimeMillis() / 1000; + switch(pathState){ + case PUSHBOT: + if (startAuto){ + follower.followPath(startPose_pushBot, true); + startAuto = false; + shootX = shoot0X; + shootY = shoot0Y; + shootH = shoot0H; + } + if (!follower.isBusy()){ + follower.followPath(pushBot_shoot0, true); + pathState = PathState.DRIVE_SHOOT0; + } + break; + case DRIVE_SHOOT0: + if (!follower.isBusy()){ + timeStamp = currentTime; + pathState = PathState.WAIT_SHOOT0; + } + break; + case WAIT_SHOOT0: + if (currentTime - timeStamp > shootTime){ + follower.followPath(shoot0_pickup1, intakePower, false); + pathState = PathState.PICKUP1; + } + break; + case PICKUP1: + if (!follower.isBusy()){ + follower.followPath(pickup1_openGate, true); + pathState = PathState.OPENGATE; + shootX = shoot1X; + shootY = shoot1Y; + shootH = shoot1H; + } + break; + case DRIVE_OPENGATE: + if (!follower.isBusy()){ + pathState = PathState.OPENGATE; + timeStamp = currentTime; + } + break; + case OPENGATE: + if (currentTime - timeStamp > openGateTime){ + follower.followPath(openGate_shoot1, true); + pathState = PathState.DRIVE_SHOOT1; + } + break; + case DRIVE_SHOOT1: + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT1; + timeStamp = currentTime; + } + break; + case WAIT_SHOOT1: + if (currentTime - timeStamp > shootTime){ + follower.followPath(shoot1_drivePickup2, true); + pathState = PathState.DRIVE_PICKUP2; + } + break; + case DRIVE_PICKUP2: + if (!follower.isBusy()) { + follower.followPath(drivePickup2_pickup2, intakePower, false); + pathState = PathState.PICKUP2; + } + break; + case PICKUP2: + if (!follower.isBusy()){ + follower.followPath(pickup2_shoot2, true); + pathState = PathState.DRIVE_SHOOT2; + } + shootX = shoot2X; + shootY = shoot2Y; + shootH = shoot2H; + break; + case DRIVE_SHOOT2: + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT2; + timeStamp = currentTime; + } + break; + case WAIT_SHOOT2: + if (currentTime - timeStamp > shootTime){ + follower.followPath(shoot2_drivePickup3, true); + pathState = PathState.DRIVE_PICKUP3; + } + break; + case DRIVE_PICKUP3: + if (!follower.isBusy()){ + follower.followPath(drivePickup3_pickup3, intakePower, false); + pathState = PathState.PICKUP3; + } + break; + case PICKUP3: + if (!follower.isBusy()){ + follower.followPath(pickup3_shoot3, true); + pathState = PathState.DRIVE_SHOOT3; + } + shootX = shoot3X; + shootY = shoot3Y; + shootH = shoot3H; + break; + case DRIVE_SHOOT3: + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT3; + } + break; + case WAIT_SHOOT3: + // add line here to say "done auto' + break; + default: + break; + } + TELE.update(); // use for debugging + } + + // Used for changing alliance + private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);} + private double adjustHeadingBasedOnAlliance(double heading){ + heading = 180 - heading; + while (heading > 180) {heading-=360;} + while (heading <= -180) {heading+=360;} + return heading; + } + + @Override + public void runOpMode() throws InterruptedException { + Robot.resetInstance(); + robot = Robot.getInstance(hardwareMap); + List allHubs = hardwareMap.getAll(LynxModule.class); + for (LynxModule hub : allHubs) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(new Pose(72,72,0)); + loopTimes = new MeasuringLoopTimes(); + loopTimes.init(); + + boolean initializeRobot = false; + while (opModeInInit()){ + follower.update(); + + if (gamepad1.crossWasPressed() && !initializeRobot){ + Color.redAlliance = !Color.redAlliance; + + double[] xPoses = {startPoseX, pushBotX, shoot0ControlX, shoot0X, + pickup1ControlX, pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X, + drivePickup2X, pickup2X, shoot2ControlX, shoot2X, + drivePickup3X, pickup3X, shoot3ControlX, shoot3X}; + + double[] headings = {startPoseH, pushBotH, shoot0H, + pickup1H, openGateH, shoot1H, + drivePickup2H, pickup2H, shoot2H, + drivePickup3H, pickup3H, shoot3H}; + + 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()){ + initializeRobot = true; + initializePoses(); + follower.setPose(startPose); + buildPaths(); + sleep(2000); + } + + TELE.addData("Red Alliance?", Color.redAlliance); + TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot); + TELE.addData("Start Pose", follower.getPose()); + TELE.update(); + } + + waitForStart(); + + if (isStopRequested()) return; + + limelightUsed = false; + + while (opModeIsActive()){ + follower.update(); + pathStateMachine(); + Pose currentPose = follower.getPose(); + teleStartPoseX = currentPose.getX(); + teleStartPoseY = currentPose.getY(); + teleStartPoseH = Math.toDegrees(currentPose.getHeading()); + + for (LynxModule hub : allHubs) { + hub.clearBulkCache(); + } + loopTimes.loop(); + + 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.update(); + } + } +} \ No newline at end of file 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 748524e..268c78c 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 @@ -18,9 +18,12 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; public class Constants { public static FollowerConstants followerConstants = new FollowerConstants() .mass(14.37888) - .forwardZeroPowerAcceleration(-29.512) - .lateralZeroPowerAcceleration(-72.872) - .centripetalScaling(0); + .forwardZeroPowerAcceleration(-30.322) + .lateralZeroPowerAcceleration(-60.876) + .translationalPIDFCoefficients(new PIDFCoefficients(0.15, 0, 0.015, 0.02)) + .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02)) + .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.02, 0, 0.00001, 0.6, 0.015)) + .centripetalScaling(0.0005); public static MecanumConstants driveConstants = new MecanumConstants() .maxPower(1) @@ -32,15 +35,15 @@ public class Constants { .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE) .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD) .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD) - .xVelocity(64.675) - .yVelocity(49.583); + .xVelocity(84.376) + .yVelocity(64.052); public static double breakingStrength = 1; public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, breakingStrength, 1); public static PinpointConstants localizerConstants = new PinpointConstants() - .forwardPodY(-3.676) - .strafePodX(3.780) + .forwardPodY(3.7795) + .strafePodX(-3.676) .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 55cc2fc..7d55931 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 @@ -416,7 +416,7 @@ class ForwardVelocityTuner extends OpMode { if (!end) { - if (Math.abs(follower.getPose().getX()) > (DISTANCE + 72)) { + if (Math.abs(follower.getPose().getX()) > (DISTANCE)) { end = true; stopRobot(); } else { @@ -524,7 +524,7 @@ class LateralVelocityTuner extends OpMode { drawCurrentAndHistory(); if (!end) { - if (Math.abs(follower.getPose().getY()) > (DISTANCE + 72)) { + if (Math.abs(follower.getPose().getY()) > (DISTANCE)) { end = true; stopRobot(); } else { 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 0dc08ab..f6ee1ad 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 @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.tests; +import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -8,7 +9,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.NormalizedRGBA; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utilsv2.Robot; @Config @TeleOp @@ -50,8 +51,9 @@ public class Hardware_Tester extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - robot = new Robot(hardwareMap); - TELE = new MultipleTelemetry(); + Robot.resetInstance(); + robot = Robot.getInstance(hardwareMap); + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); @@ -122,7 +124,8 @@ public class Hardware_Tester extends LinearOpMode { // Sensor Data -// TELE.addData("Beam Break 1?", robot.beam1.isPressed()); + TELE.addData("Beam Break insideBeam?", robot.insideBeam.isPressed()); + TELE.addData("Beam Break outsideBeam?", robot.outsideBeam.isPressed()); // TELE.addData("Beam Break 2?", robot.beam2.isPressed()); // TELE.addData("Beam Break 3?", robot.beam3.isPressed()); 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 3ab756f..bc366be 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 @@ -20,16 +20,20 @@ public class NewShooterTest extends LinearOpMode { Robot robot; Flywheel flywheel; Turret turret; + MultipleTelemetry TELE; + public static boolean intake = true; public static boolean shoot = false; public static double intakePower = 1.0; public static double transferShootPower = -1; - public static double transferIntakePower = -1.0; + public static double transferIntakePower = -1; public static double turretPos = 0.51; public static double hoodPos = 0.51; public static double flywheel_velo = 0; + public static double shooterP = 255, shooterI = 0, shooterD = 0, shooterF = 75; + private enum ShootState { IDLE, WAIT_GATE, @@ -46,15 +50,15 @@ public class NewShooterTest extends LinearOpMode { robot = Robot.getInstance(hardwareMap); + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + flywheel = new Flywheel(robot); turret = new Turret(robot); Shooter shooter = new Shooter( robot, - new MultipleTelemetry( - telemetry, - FtcDashboard.getInstance().getTelemetry() - ), + TELE, Constants.createFollower(hardwareMap), true, turret, @@ -72,6 +76,9 @@ public class NewShooterTest extends LinearOpMode { robot.setHoodPos(hoodPos); shooter.setTurretPosition(turretPos); shooter.setFlywheelVelocity(flywheel_velo); + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage); + robot.setSpinPos(ServoPositions.spindexer_A2); if (intake && !shoot) { @@ -85,9 +92,7 @@ public class NewShooterTest extends LinearOpMode { robot.setIntakePower(intakePower); robot.setTransferServoPos( ServoPositions.transferServo_out); - } - - else if (shoot) { + } else if (shoot) { robot.setIntakePower(intakePower); @@ -129,6 +134,13 @@ public class NewShooterTest extends LinearOpMode { } } + TELE.addData("Flywheel Velocity1", (robot.shooter1.getVelocity() * 60) / 28); + TELE.addData("Flywheel Velocity2", (robot.shooter2.getVelocity() * 60) / 28); + TELE.addData("Flywheel Averag Velocity", flywheel.getAverageVelocity()); + TELE.addData("PIDF Coefficients", Flywheel.shooterPIDF); + TELE.addData("Power", flywheel.getShooterPower()); + TELE.update(); + shooter.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java index a5cfada..a4dc26e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java @@ -1,8 +1,8 @@ package org.firstinspires.ftc.teamcode.utilsv2; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.PIDFCoefficients; import org.firstinspires.ftc.teamcode.utilsv2.Robot; @@ -12,7 +12,12 @@ import java.util.LinkedList; public class Flywheel { Robot robot; - public PIDFCoefficients shooterPIDF1, shooterPIDF2; +// public PIDFCoefficients shooterPIDF1, shooterPIDF2; + public static PIDFCoefficients shooterPIDF; + public static double shooterPIDF_P = 255; + public static double shooterPIDF_I = 0.0; + public static double shooterPIDF_D = 0.0; + public static double shooterPIDF_F = 75; private double velo = 0.0; private double velo1 = 0.0; @@ -27,10 +32,8 @@ public class Flywheel { private final LinkedList velocityHistory = new LinkedList<>(); public Flywheel(Robot rob) { - robot = rob; - shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F); - shooterPIDF2 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F); + shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12); } public double getVelo() { @@ -54,28 +57,24 @@ public class Flywheel { } // Set the robot PIDF for the next cycle. - private double prevF = 0; - - public static double voltagePIDFDifference = 0.8; public void setPIDF(double p, double i, double d, double f) { - shooterPIDF1.p = p; - shooterPIDF1.i = i; - shooterPIDF1.d = d; - shooterPIDF1.f = f; + shooterPIDF.p = p; + shooterPIDF.i = i; + shooterPIDF.d = d; + shooterPIDF.f = f; - shooterPIDF2.p = p; - shooterPIDF2.i = i; - shooterPIDF2.d = d; - shooterPIDF2.f = f; + robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + } + private double prevF = 0; + + public static double voltagePIDFDifference = 0.8; + public void setF(double f){ if (Math.abs(prevF - f) > voltagePIDFDifference) { - - robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); - - robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); - + shooterPIDF.f = f; + robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); prevF = f; } } @@ -108,6 +107,7 @@ public class Flywheel { averageVelocity = sum / velocityHistory.size(); } + double power; public void manageFlywheel(double commandedVelocity) { if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) { @@ -115,8 +115,8 @@ public class Flywheel { } robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); - - robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); + power = robot.shooter1.getPower(); + robot.shooter2.setPower(power); velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); @@ -128,4 +128,5 @@ public class Flywheel { steady = (Math.abs(commandedVelocity - averageVelocity) < 50); } + public double getShooterPower(){return power;} } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java index 224cdca..7cbedc0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.utilsv2; +import android.view.View; + import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.robotcore.hardware.AnalogInput; @@ -19,6 +21,7 @@ public class Robot { // Singleton instance private static Robot instance; + /** * Returns the existing Robot instance or creates one if it doesn't exist. */ @@ -45,11 +48,6 @@ public class Robot { public DcMotorEx backRight; public DcMotorEx intake; public DcMotorEx transfer; - public PIDFCoefficients shooterPIDF; - public static double shooterPIDF_P = 255; - public static double shooterPIDF_I = 0.0; - public static double shooterPIDF_D = 0.0; - public static double shooterPIDF_F = 75; // public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001}; public DcMotorEx shooter1; public DcMotorEx shooter2; @@ -65,7 +63,6 @@ public class Robot { public Servo spin2; public TouchSensor insideBeam; public TouchSensor outsideBeam; - public RevColorSensorV3 revSensor; public VoltageSensor voltage; @@ -106,13 +103,10 @@ public class Robot { shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); shooter1.setDirection(DcMotorSimple.Direction.REVERSE); - shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F); +// shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12); shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); - shooter1.setVelocity(0); - shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); - shooter2.setVelocity(0); +// shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); hood = hardwareMap.get(Servo.class, "hood");