diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java deleted file mode 100644 index 0e41422..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java +++ /dev/null @@ -1,89 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.arcrobotics.ftclib.gamepad.GamepadEx; -import com.arcrobotics.ftclib.gamepad.GamepadKeys; -import com.qualcomm.robotcore.hardware.DcMotorEx; - -import org.firstinspires.ftc.teamcode.utils.Robot; - -import java.util.Objects; - -public class Drivetrain { - - private final GamepadEx gamepad; - private final DcMotorEx fl; - private final DcMotorEx fr; - private final DcMotorEx bl; - private final DcMotorEx br; - public MultipleTelemetry TELE; - private String Mode = "Default"; - private double defaultSpeed = 0.7; - - private double slowSpeed = 0.3; - - public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1) { - - this.fl = robot.frontLeft; - this.fr = robot.frontRight; - this.br = robot.backRight; - this.bl = robot.backLeft; - - this.gamepad = gamepad1; - - this.TELE = tele; - - } - - public void setMode(String mode) { - this.Mode = mode; - } - - public void setDefaultSpeed(double speed) { - this.defaultSpeed = speed; - } - - public void setSlowSpeed(double speed) { - this.slowSpeed = speed; - } - - public void RobotCentric(double fwd, double strafe, double turn, double turbo) { - - double y = -fwd; // Remember, Y stick value is reversed - double x = strafe * 1.1; // Counteract imperfect strafing - double rx = turn; - - // Denominator is the largest motor power (absolute value) or 1 - // This ensures all the powers maintain the same ratio, - // but only if at least one is out of the range [-1, 1] - double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); - double frontLeftPower = (y + x + rx) / denominator; - double backLeftPower = (y - x + rx) / denominator; - double frontRightPower = (y - x - rx) / denominator; - double backRightPower = (y + x - rx) / denominator; - - fl.setPower(frontLeftPower * turbo); - bl.setPower(backLeftPower * turbo); - fr.setPower(frontRightPower * turbo); - br.setPower(backRightPower * turbo); - - } - - public void update() { - - if (Objects.equals(Mode, "Default")) { - - RobotCentric( - gamepad.getRightY(), - gamepad.getRightX(), - gamepad.getLeftX(), - (gamepad.getTrigger( - GamepadKeys.Trigger.RIGHT_TRIGGER) * (1 - defaultSpeed) - - gamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) * slowSpeed - + defaultSpeed - ) - ); - } - - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java deleted file mode 100644 index 96bd384..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java +++ /dev/null @@ -1,162 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.Pose2d; -import com.qualcomm.robotcore.hardware.DcMotorEx; - -import org.firstinspires.ftc.teamcode.utils.Robot; - -@Config -public class Shooter { - - // ================================================================ - // ------------------- DASHBOARD CONSTANTS ----------------------- - // ================================================================ - - public static int mode = 0; // 0 = manual, 1 = velocity PID, 2 = autoTrack - public static double parameter = 0.0; // manual: power, vel: target RPM, auto: target RPM - - public static double MAX_RPM = 2500; - public static double kP = 0.01; - public static double maxStep = 0.2; - - public static double transferPower = 0.0; - public static double hoodPos = 0.501; - - // ================================================================ - // ------------- AUTO TRACK TUNING CONSTANTS ------------------- - // ================================================================ - - // z offset between shooter and goal - public static double dz = 30; // inches - - // Quadratic fit for shooter velocity vs distance - // v = A*d^2 + B*d + C - public static double A = 0.0004; - public static double B = 0.9; - public static double C = 1200; - - // Hood angle trig model - // hood = HOOD_A * atan(d * HOOD_B) + HOOD_C - public static double HOOD_A = 0.42; - public static double HOOD_B = 0.012; - public static double HOOD_C = 0.22; - - // ================================================================ - // ---------------------- INTERNAL STATE ------------------------ - // ================================================================ - - private DcMotorEx leftShooter, rightShooter, encoder; - - private double lastRevolutions = 0.0; - private double lastTime = 0.0; - - private MultipleTelemetry TELE; - private Robot robot; - - // ================================================================ - // --------------------------- INIT ------------------------------- - // ================================================================ - - public void init(Robot robot, MultipleTelemetry TELE) { - this.robot = robot; - this.TELE = TELE; - - leftShooter = robot.shooter1; - rightShooter = robot.shooter2; - encoder = robot.shooter1; - - lastTime = 0.0; - lastRevolutions = 0.0; - } - - // ================================================================ - // -------------------------- UPDATE ------------------------------ - // ================================================================ - - public void update(double runtimeSec) { - - double kF = 1.0 / MAX_RPM; - - double rev = encoder.getCurrentPosition() / 2048.0; - double velocity = -60 * (rev - lastRevolutions) / (runtimeSec - lastTime); - - TELE.addLine("Mode: 0=Manual, 1=Vel, 2=AutoTrack"); - TELE.addData("Parameter", parameter); - TELE.addData("Velocity", velocity); - - if (mode == 0) { - // Manual - leftShooter.setPower(parameter); - rightShooter.setPower(parameter); - } - else if (mode == 1 || mode == 2) { - // Velocity PID (shared logic) - double feed = kF * parameter; - double error = parameter - velocity; - double correction = kP * error; - - correction = Math.max(-maxStep, Math.min(maxStep, correction)); - - double finalPower = Math.max(0, Math.min(1, feed + correction)); - - leftShooter.setPower(finalPower); - rightShooter.setPower(finalPower); - } - - robot.hood.setPosition(hoodPos); - robot.transfer.setPower(transferPower); - - lastTime = runtimeSec; - lastRevolutions = rev; - - TELE.update(); - } - - // ================================================================ - // ------------------------ AUTO TRACK ---------------------------- - // ================================================================ - - public void autoTrack(Pose2d robotPose, Pose2d goalPose) { - - mode = 2; // Auto tracking → velocity PID - - // Compute 3D distance - double dx = goalPose.position.x - robotPose.position.x; - double dy = goalPose.position.y - robotPose.position.y; - - double distance = Math.sqrt(dx*dx + dy*dy + dz*dz); - - // ---- Velocity Fit ---- - double targetVelocity = A * distance * distance + B * distance + C; - parameter = targetVelocity; - - // ---- Hood Fit ---- - hoodPos = HOOD_A * Math.atan(distance * HOOD_B) + HOOD_C; - - // Telemetry - TELE.addLine("AUTO TRACK ACTIVE"); - TELE.addData("Distance", distance); - TELE.addData("Target Velocity", targetVelocity); - TELE.addData("Hood", hoodPos); - } - - // ================================================================ - // --------------------- USER CALL METHODS ------------------------ - // ================================================================ - - public void setManualPower(double p) { - mode = 0; - parameter = p; - } - - public void setVelocity(double rpm) { - mode = 1; - parameter = rpm; - } - - public void setHood(double pos) { hoodPos = pos; } - - public void setTransfer(double p) { transferPower = p; } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java index ba7cfa5..d6a526e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java @@ -1,7 +1,9 @@ package org.firstinspires.ftc.teamcode.teleop; +import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; -import static org.firstinspires.ftc.teamcode.tests.ShooterTest.*; +import static org.firstinspires.ftc.teamcode.tests.ShooterTest.kP; +import static org.firstinspires.ftc.teamcode.tests.ShooterTest.maxStep; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -9,6 +11,7 @@ import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Robot; import java.util.ArrayList; @@ -16,16 +19,13 @@ import java.util.List; public class TeleopV2 extends LinearOpMode { + public static double vel = 3000; + public static double hood = 0.5; Robot robot; MultipleTelemetry TELE; - boolean intake = false; boolean reject = false; - private double lastEncoderRevolutions = 0.0; - private double lastTimeStamp = 0.0; - - private double vel = 3000; - + int ticker = 0; List allHubs = hardwareMap.getAll(LynxModule.class); List s1G = new ArrayList<>(); List s2G = new ArrayList<>(); @@ -33,6 +33,15 @@ public class TeleopV2 extends LinearOpMode { List s1 = new ArrayList<>(); List s2 = new ArrayList<>(); List s3 = new ArrayList<>(); + private double lastEncoderRevolutions = 0.0; + private double lastTimeStamp = 0.0; + private double velo1, velo; + private double stamp1, stamp, initPos; + private boolean shootAll = false; + double desiredTurretAngle = 180; + + MecanumDrive drive = new MecanumDrive(hardwareMap, teleStart); + @Override public void runOpMode() throws InterruptedException { @@ -50,7 +59,7 @@ public class TeleopV2 extends LinearOpMode { if (isStopRequested()) return; while (opModeIsActive()) { - //DRIVETRAIN: + //DRIVETRAIN: double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing @@ -67,7 +76,7 @@ public class TeleopV2 extends LinearOpMode { robot.frontRight.setPower(frontRightPower); robot.backRight.setPower(backRightPower); - //INTAKE: + //INTAKE: if (gamepad1.rightBumperWasPressed()) { intake = true; @@ -96,7 +105,7 @@ public class TeleopV2 extends LinearOpMode { robot.intake.setPower(0); } - //COLOR: + //COLOR: double s1D = robot.color1.getDistance(DistanceUnit.MM); double s2D = robot.color2.getDistance(DistanceUnit.MM); @@ -151,38 +160,73 @@ public class TeleopV2 extends LinearOpMode { boolean green2 = s2.get(s2.size() - 1); boolean green3 = s3.get(s3.size() - 1); - //SHOOTER: + //SHOOTER: - double kF = 1.0 / MAX_RPM; // baseline feedforward + double penguin = 0; - double encoderRevolutions = (double) robot.shooterEncoder.getCurrentPosition() / 2048; + if (ticker % 8 == 0) { + penguin = (double) robot.shooterEncoder.getCurrentPosition() / 2048; + double stamp = getRuntime(); + velo1 = -60 * ((penguin - initPos) / (stamp - stamp1)); + initPos = penguin; + stamp1 = stamp; + } - double velocity = -60 * (encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp); + velo = velo1; - double velPID; + double feed = vel / 4500; - // --- FEEDFORWARD BASE POWER --- - double feed = kF * vel; // Example: vel=2500 → feed=0.5 + if (vel > 500) { + feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18; + } // --- PROPORTIONAL CORRECTION --- - double error = vel - velocity; + double error = vel - velo1; double correction = kP * error; // limit how fast power changes (prevents oscillation) correction = Math.max(-maxStep, Math.min(maxStep, correction)); // --- FINAL MOTOR POWER --- - velPID = feed + correction; + double powPID = feed + correction; // clamp to allowed range - velPID = Math.max(0, Math.min(1, velPID)); + powPID = Math.max(0, Math.min(1, powPID)); - robot.shooter1.setPower(velPID); - robot.shooter2.setPower(velPID); + if (vel - velo > 1000) { + powPID = 1; + } else if (velo - vel > 1000) { + powPID = 0; + } + + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + + robot.hood.setPosition(hood); //TODO: ADD CODE TO CHANGE VARIABLE VEL BASED ON POSITION - //MISC: + //TODO: ADD CODE TO CHANGE VARIABLE HOOD ANGLE BASED ON POSITION + + + + + //SHOOT ALL: + + if (gamepad2.rightBumperWasPressed()){ + shootAll = true; + } + + if (shootAll){ + intake = false; + reject = false; + + } + + //MISC: + + drive.updatePoseEstimate(); + for (LynxModule hub : allHubs) { hub.clearBulkCache(); @@ -192,8 +236,13 @@ public class TeleopV2 extends LinearOpMode { TELE.addData("Spin2Green", s2.get(s2.size() - 1)); TELE.addData("Spin3Green", s3.get(s3.size() - 1)); + TELE.addData("pose", drive.localizer.getPose()); + TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); + TELE.update(); + ticker++; + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.txt similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.txt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.txt similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.txt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java index 2a74ca3..b0ae734 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java @@ -6,7 +6,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.teamcode.subsystems.AprilTagWebcam; +import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam; import org.firstinspires.ftc.teamcode.utils.Robot; @Config diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.txt similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.txt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TrackingTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TrackingTest.java deleted file mode 100644 index 5b125c7..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TrackingTest.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.firstinspires.ftc.teamcode.tests; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.arcrobotics.ftclib.gamepad.GamepadEx; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.Gamepad; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; -import org.firstinspires.ftc.teamcode.utils.Robot; - -public class TrackingTest extends LinearOpMode { - - Robot robot; - - Drivetrain drivetrain; - - MultipleTelemetry TELE; - - GamepadEx g1; - - - - @Override - public void runOpMode() throws InterruptedException { - - robot = new Robot(hardwareMap); - - TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - g1 = new GamepadEx(gamepad1); - - drivetrain = new Drivetrain(robot, TELE, g1); - - - waitForStart(); - if(isStopRequested()) return; - while (opModeIsActive()){ - drivetrain.update(); - } - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.txt similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.txt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagWebcam.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AprilTagWebcam.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagWebcam.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AprilTagWebcam.java index 4cacde5..f473595 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagWebcam.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AprilTagWebcam.java @@ -1,14 +1,11 @@ -package org.firstinspires.ftc.teamcode.subsystems; +package org.firstinspires.ftc.teamcode.utils; import android.util.Size; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;