diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index bb5a7b0..92673eb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -72,6 +72,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; +import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; import androidx.annotation.NonNull; @@ -137,7 +138,7 @@ public class Auto_LT_Close extends LinearOpMode { public static double intake3Time = 4.2; public static double flywheel0Time = 3.5; - public static double pickup1Speed = 23; + public static double pickup1Speed = 15; // ---- SECOND SHOT / PICKUP ---- public static double shoot1Vel = 2300; public static double shootAllVelocity = 2500; @@ -214,12 +215,28 @@ public class Auto_LT_Close extends LinearOpMode { ticker++; robot.transferServo.setPosition(transferServo_out); - turret.manualSetTurret(turretShootPos); - drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + TELE.addData("Velocity", flywheel.getVelo()); TELE.addData("Hood", robot.hood.getPosition()); TELE.addData("motif", motif_id); @@ -504,6 +521,9 @@ public class Auto_LT_Close extends LinearOpMode { teleStart = drive.localizer.getPose(); + TELE.addData("Full?", spindexer.isFull()); + TELE.update(); + return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); } }; @@ -780,11 +800,6 @@ public class Auto_LT_Close extends LinearOpMode { servos = new Servos(hardwareMap); - robot.limelight.start(); - - robot.limelight.pipelineSwitch(1); - - turret = new Turret(robot, TELE, robot.limelight); turret.manualSetTurret(turrDefault); @@ -796,6 +811,8 @@ public class Auto_LT_Close extends LinearOpMode { robot.transferServo.setPosition(transferServo_out); + limelightUsed = false; + TrajectoryActionBuilder shoot0 = null; TrajectoryActionBuilder pickup1 = null; TrajectoryActionBuilder shoot1 = null; @@ -831,6 +848,11 @@ public class Auto_LT_Close extends LinearOpMode { ballCycles--; } + if (gamepad2.squareWasPressed()){ + robot.limelight.start(); + robot.limelight.pipelineSwitch(1); + } + if (redAlliance) { robot.light.setPosition(0.28); @@ -1177,6 +1199,8 @@ public class Auto_LT_Close extends LinearOpMode { teleStart = drive.localizer.getPose(); + flywheel.manageFlywheel(0); + TELE.addLine("finished"); TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index fa0a85d..8e9709a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -763,6 +763,8 @@ public class Auto_LT_Far extends LinearOpMode { teleStart = drive.localizer.getPose(); + flywheel.manageFlywheel(0); + TELE.addLine("finished"); TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index 43e18df..bb7d29b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -8,45 +8,40 @@ public class Front_Poses { public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1; - public static double rx2a = 41, ry2a = 18, rh2a = 140; - public static double rx2b = 19, ry2b = 40, rh2b = 140; - - - public static double rx3a = 55, ry3a = 39, rh3a = 140; - - public static double rx3aG = 60, ry3aG = 34, rh3aG = 140; - - public static double rx3b = 36, ry3b = 58, rh3b = 140.1; - - public static double rx4a = 80, ry4a = 48, rh4a = 140; - public static double rx4b = 45, ry4b = 83, rh4b = 140.1; - public static double bx1 = 20, by1 = 0.5, bh1 = 0.1; + + public static double rx2a = 41, ry2a = 18, rh2a = 140; public static double bx2a = 41, by2a = -18, bh2a = -140; + + public static double rx2b = 23, ry2b = 36, rh2b = 140.1; public static double bx2b = 19, by2b = -40, bh2b = -140.1; + public static double rx3a = 55, ry3a = 39, rh3a = 140; public static double bx3a = 55, by3a = -39, bh3a = -140; - public static double bx3b = 41, by3b = -59, bh3b = -140.1; + + public static double rx3aG = 60, ry3aG = 34, rh3aG = 140; public static double bx3aG = 55, by3aG = -43, bh3aG = -140; + public static double rx3b = 36, ry3b = 58, rh3b = 140.1; + public static double bx3b = 41, by3b = -59, bh3b = -140.1; + + public static double rx4a = 75, ry4a = 53, rh4a = 140; public static double bx4a = 75, by4a = -53, bh4a = -140; + + public static double rx4b = 50, ry4b = 78, rh4b = 140.1; public static double bx4b = 47, by4b = -85, bh4b = -140.1; + public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; public static double rShootX = 40, rShootY = 10, rShootH = 50; - public static double rxPrep = 45, ryPrep = 10, rhPrep = 50; - - public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50; - - public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50; - public static double bShootX = 40, bShootY = 0, bShootH = -50; + + public static double rxPrep = 45, ryPrep = 10, rhPrep = 50; public static double bxPrep = 45, byPrep = -10, bhPrep = -50; + public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50; + public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50; public static Pose2d teleStart = new Pose2d(0, 0, 0); - public static Pose2d teleEnd = new Pose2d(0, 0, 0); - - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index e32e81f..c09cafd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -97,7 +97,10 @@ public class TeleopV3 extends LinearOpMode { tController.setTolerance(0.001); Turret turret = new Turret(robot, TELE, robot.limelight); + limelightUsed = true; + robot.light.setPosition(1); + while (opModeInInit()) { robot.limelight.start(); if (redAlliance) { @@ -107,9 +110,6 @@ public class TeleopV3 extends LinearOpMode { } } - limelightUsed= true; - - waitForStart(); if (isStopRequested()) return; @@ -186,6 +186,8 @@ public class TeleopV3 extends LinearOpMode { } //SHOOTER: + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); //HOOD: @@ -225,8 +227,6 @@ public class TeleopV3 extends LinearOpMode { drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); } - - if (enableSpindexerManager) { //if (!shootAll) { spindexer.processIntake(); @@ -328,7 +328,7 @@ public class TeleopV3 extends LinearOpMode { // TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); // TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp); - TELE.addData("Voltage", robot.voltage.getVoltage()); + TELE.addData("Voltage", robot.voltage.getVoltage()); // returns alleged recorded voltage (not same as driver hub) TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index d45ffc0..63e85bc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -26,12 +26,11 @@ public class ShooterTest extends LinearOpMode { public static double parameter = 0.0; // --- CONSTANTS YOU TUNE --- - //TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED public static double Velocity = 0.0; public static double P = 255.0; public static double I = 0.0; public static double D = 0.0; - public static double F = 7.5; + public static double F = 90; public static double transferPower = 1.0; public static double hoodPos = 0.501; public static double turretPos = 0.501; @@ -73,11 +72,13 @@ public class ShooterTest extends LinearOpMode { while (opModeIsActive()) { + double voltage = robot.voltage.getVoltage(); + if (mode == 0) { rightShooter.setPower(parameter); leftShooter.setPower(parameter); } else if (mode == 1) { - flywheel.setPIDF(P, I, D, F); + flywheel.setPIDF(P, I, D, F / voltage); flywheel.manageFlywheel((int) Velocity); } @@ -109,7 +110,6 @@ public class ShooterTest extends LinearOpMode { //intake = false; //reject = false; - // TODO: Change starting position based on desired order to shoot green ball //spindexPos = spindexer_intakePos1; if (getRuntime() - shootStamp < 3.5) { @@ -150,6 +150,7 @@ public class ShooterTest extends LinearOpMode { TELE.addData("Power", robot.shooter1.getPower()); TELE.addData("Steady?", flywheel.getSteady()); TELE.addData("Position", robot.shooter1.getCurrentPosition()); + TELE.addData("Voltage", voltage); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index f5716a4..4ce0461 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -34,7 +34,7 @@ public class Robot { public double shooterPIDF_P = 255.0; public double shooterPIDF_I = 0.0; public double shooterPIDF_D = 0.0; - public double shooterPIDF_F = 7.5; + public double shooterPIDF_F = 90; public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001}; public DcMotorEx shooter1; public DcMotorEx shooter2;