diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 50cc8cc..7aa9159 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double rapidFireBlocker_Closed = 0.3; + public static double rapidFireBlocker_Closed = 0.35; public static double rapidFireBlocker_Open = 0.5; public static double spindexBlocker_Closed = 0.31; 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 7d80708..a061c24 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 @@ -1,38 +1,56 @@ package org.firstinspires.ftc.teamcode.teleop; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; + 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.Pose; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.utilsv2.Drivetrain; -import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.utilsv2.*; @TeleOp @Config public class TeleopV4 extends LinearOpMode { Robot robot; Drivetrain drivetrain; + Shooter shooter; MultipleTelemetry TELE; + Follower follower; + SpindexerTransferIntake spindexerTransferIntake; + @Override public void runOpMode() throws InterruptedException { - robot = new Robot(hardwareMap); + robot = Robot.getInstance(hardwareMap); TELE = new MultipleTelemetry( FtcDashboard.getInstance().getTelemetry(), telemetry ); drivetrain = new Drivetrain(robot, TELE); + follower = Constants.createFollower(hardwareMap); + Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH)); + follower.setStartingPose(start); + + shooter = new Shooter(robot, TELE, follower, Color.redAlliance); + shooter.setState(Shooter.ShooterState.TRACK_GOAL); + spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE); + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); - drivetrain.setTelemetry(true); waitForStart(); if (isStopRequested()) return; - while (opModeIsActive()){ + while (opModeIsActive()) { //Drivetrain @@ -42,6 +60,40 @@ public class TeleopV4 extends LinearOpMode { gamepad1.left_stick_x ); + shooter.update(); + spindexerTransferIntake.update(); + + SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState(); + + if (gamepad1.xWasPressed() && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF || + state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT || + state == SpindexerTransferIntake.RapidMode.PULSE_OUT || + state == SpindexerTransferIntake.RapidMode.PULSE_IN || + state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) { + + spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); + } + + if (gamepad1.aWasPressed() && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) { + + spindexerTransferIntake.setRapidMode( + SpindexerTransferIntake.RapidMode.HOLD_BALLS + ); + } + + if (gamepad1.yWasPressed() + && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) { + + spindexerTransferIntake.setRapidMode( + SpindexerTransferIntake.RapidMode.INTAKE + ); + } + + TELE.update(); } 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 new file mode 100644 index 0000000..fafc88c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java @@ -0,0 +1,126 @@ +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; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.constants.ServoPositions; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.utilsv2.Robot; +import org.firstinspires.ftc.teamcode.utilsv2.Shooter; + +@Config +@TeleOp +public class NewShooterTest extends LinearOpMode { + + Robot robot; + + 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 turretPos = 0.51; + public static double hoodPos = 0.51; + public static double flywheel = 0; + + private enum ShootState { + IDLE, + WAIT_GATE, + WAIT_PUSH + } + + private ShootState shootState = ShootState.IDLE; + private long timestamp = 0; + + @Override + public void runOpMode() throws InterruptedException { + + Robot.resetInstance(); + + robot = Robot.getInstance(hardwareMap); + + Shooter shooter = new Shooter( + robot, + new MultipleTelemetry( + telemetry, + FtcDashboard.getInstance().getTelemetry() + ), + Constants.createFollower(hardwareMap), + true + ); + + shooter.setState(Shooter.ShooterState.MANUAL); + + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()) { + + robot.setHoodPos(hoodPos); + shooter.setTurretPosition(turretPos); + shooter.setFlywheelVelocity(flywheel); + robot.setSpinPos(ServoPositions.spindexer_A2); + + if (intake && !shoot) { + + shootState = ShootState.IDLE; + + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Closed); + + robot.setTransferPower(transferIntakePower); + robot.setIntakePower(intakePower); + robot.setTransferServoPos( + ServoPositions.transferServo_out); + } + + else if (shoot) { + robot.setIntakePower(intakePower); + + + switch (shootState) { + + case IDLE: + + robot.setTransferPower(transferShootPower); + + timestamp = System.currentTimeMillis(); + shootState = ShootState.WAIT_GATE; + + break; + + case WAIT_GATE: + + if (System.currentTimeMillis() - timestamp >= 300) { + + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Open); + + timestamp = System.currentTimeMillis(); + shootState = ShootState.WAIT_PUSH; + } + + break; + + case WAIT_PUSH: + + if (System.currentTimeMillis() - timestamp >= 100) { + + robot.setTransferServoPos( + ServoPositions.transferServo_in); + + shootState = ShootState.IDLE; + } + + break; + } + } + + shooter.update(); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java index ecba638..8623588 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java @@ -4,8 +4,6 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.hardware.DcMotor; -import org.firstinspires.ftc.teamcode.utils.Robot; - @Config public class Drivetrain { @@ -18,8 +16,8 @@ public class Drivetrain { private static final double STRAFE_MULTIPLIER = 1.2; - public static double FORWARD_ROTATION_CORRECTION = 0.03; - public static double STRAFE_ROTATION_CORRECTION = -0.03; + public static double FORWARD_ROTATION_CORRECTION = 0; + public static double STRAFE_ROTATION_CORRECTION = -0; private boolean tele = false; 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 new file mode 100644 index 0000000..a5cfada --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java @@ -0,0 +1,131 @@ +package org.firstinspires.ftc.teamcode.utilsv2; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import com.qualcomm.robotcore.hardware.PIDFCoefficients; + +import org.firstinspires.ftc.teamcode.utilsv2.Robot; + +import java.util.LinkedList; + +public class Flywheel { + Robot robot; + + public PIDFCoefficients shooterPIDF1, shooterPIDF2; + + private double velo = 0.0; + private double velo1 = 0.0; + private double velo2 = 0.0; + + private double averageVelocity = 0.0; + + double targetVelocity = 0.0; + + boolean steady = false; + + 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); + } + + public double getVelo() { + return velo; + } + + public double getVelo1() { + return velo1; + } + + public double getVelo2() { + return velo2; + } + + public double getAverageVelocity() { + return averageVelocity; + } + + public boolean getSteady() { + return steady; + } + + // 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; + + shooterPIDF2.p = p; + shooterPIDF2.i = i; + shooterPIDF2.d = d; + shooterPIDF2.f = 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); + + prevF = f; + } + } + + // Convert from RPM to Ticks per Second + private double RPM_to_TPS(double RPM) { + return (RPM * 28.0) / 60.0; + } + + // Convert from Ticks per Second to RPM + private double TPS_to_RPM(double TPS) { + return (TPS * 60.0) / 28.0; + } + + private void updateVelocityAverage(double newVelocity) { + + velocityHistory.add(newVelocity); + + int velocityHistorySize = 5; + if (velocityHistory.size() > velocityHistorySize) { + velocityHistory.removeFirst(); + } + + double sum = 0.0; + + for (double v : velocityHistory) { + sum += v; + } + + averageVelocity = sum / velocityHistory.size(); + } + + public void manageFlywheel(double commandedVelocity) { + + if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) { + targetVelocity = commandedVelocity; + } + + robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); + + robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); + + velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); + + velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); + + velo = (velo1 + velo2) / 2.0; + + updateVelocityAverage(velo); + + steady = (Math.abs(commandedVelocity - averageVelocity) < 50); + } +} \ 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 new file mode 100644 index 0000000..224cdca --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java @@ -0,0 +1,311 @@ +package org.firstinspires.ftc.teamcode.utilsv2; + +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.rev.RevColorSensorV3; +import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.TouchSensor; +import com.qualcomm.robotcore.hardware.VoltageSensor; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +public class Robot { + // Singleton instance + private static Robot instance; + + /** + * Returns the existing Robot instance or creates one if it doesn't exist. + */ + public static Robot getInstance(HardwareMap hardwareMap) { + if (instance == null) { + instance = new Robot(hardwareMap); + } + return instance; + } + + /** + * Optional: clears the singleton. + * Useful when switching OpModes. + */ + public static void resetInstance() { + instance = null; + } + + public static boolean usingLimelight = true; + public static boolean usingCamera = false; + public DcMotorEx frontLeft; + public DcMotorEx frontRight; + public DcMotorEx backLeft; + 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; + public Servo hood; + public Servo transferServo; + public Servo spindexBlocker; + public Servo rapidFireBlocker; + public Servo tilt1; + public Servo tilt2; + public Servo turr1; + public Servo turr2; + public Servo spin1; + public Servo spin2; + public TouchSensor insideBeam; + public TouchSensor outsideBeam; + + public RevColorSensorV3 revSensor; + + public VoltageSensor voltage; + + // Below is disregarded + public AnalogInput spin1Pos; + public AnalogInput spin2Pos; + public AnalogInput turr1Pos; + public AnalogInput transferServoPos; + public AprilTagProcessor aprilTagProcessor; + public WebcamName webcam; + public RevColorSensorV3 color1; + public RevColorSensorV3 color2; + public RevColorSensorV3 color3; + public Limelight3A limelight; + public Servo light; + + public Robot(HardwareMap hardwareMap) { + + //Define components w/ hardware map + frontLeft = hardwareMap.get(DcMotorEx.class, "fl"); + frontRight = hardwareMap.get(DcMotorEx.class, "fr"); + backLeft = hardwareMap.get(DcMotorEx.class, "bl"); + backRight = hardwareMap.get(DcMotorEx.class, "br"); + frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); + backLeft.setDirection(DcMotorSimple.Direction.REVERSE); + + frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + intake = hardwareMap.get(DcMotorEx.class, "intake"); + intake.setDirection(DcMotorSimple.Direction.REVERSE); + + shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1"); + + shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); + + shooter1.setDirection(DcMotorSimple.Direction.REVERSE); + shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F); + 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); + + hood = hardwareMap.get(Servo.class, "hood"); + + turr1 = hardwareMap.get(Servo.class, "turr1"); + + turr2 = hardwareMap.get(Servo.class, "turr2"); + + spin1 = hardwareMap.get(Servo.class, "spin1"); + + spin2 = hardwareMap.get(Servo.class, "spin2"); + + transfer = hardwareMap.get(DcMotorEx.class, "transfer"); + + + transferServo = hardwareMap.get(Servo.class, "transferServo"); + + transfer.setDirection(DcMotorSimple.Direction.REVERSE); + transfer.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + spindexBlocker = hardwareMap.get(Servo.class, "spinB"); + + rapidFireBlocker = hardwareMap.get(Servo.class, "rapidB"); + + tilt1 = hardwareMap.get(Servo.class, "tilt1"); + tilt2 = hardwareMap.get(Servo.class, "tilt2"); + + insideBeam = hardwareMap.get(TouchSensor.class, "beam1"); + outsideBeam = hardwareMap.get(TouchSensor.class, "beam2"); + + revSensor = hardwareMap.get(RevColorSensorV3.class, "rev"); + + // Below is disregarded + +// turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port +// +// spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos"); +// +// spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos"); +// +// transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos"); +// +// color1 = hardwareMap.get(RevColorSensorV3.class, "c1"); +// +// color2 = hardwareMap.get(RevColorSensorV3.class, "c2"); +// +// color3 = hardwareMap.get(RevColorSensorV3.class, "c3"); + + if (usingLimelight) { + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + } else if (usingCamera) { + webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); + aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults(); + } + +// light = hardwareMap.get(Servo.class, "light"); + + voltage = hardwareMap.voltageSensor.iterator().next(); + } + + // Voids below are used to minimize hardware calls to minimize loop times + + // Used to cut off digits that are negligible + private final int maxDigits = 5; + private final int roundingFactor = (int) Math.pow(10, maxDigits); + + private double prevFrontLeftPower = -10.501; + public void setFrontLeftPower(double pow){ + pow = (double) Math.round(pow * roundingFactor) / roundingFactor; + if (pow != prevFrontLeftPower){ + frontLeft.setPower(pow); + } + prevFrontLeftPower = pow; + } + + private double prevFrontRightPower = -10.501; + public void setFrontRightPower(double pow){ + pow = (double) Math.round(pow * roundingFactor) / roundingFactor; + if (pow != prevFrontRightPower){ + frontRight.setPower(pow); + } + prevFrontRightPower = pow; + } + + private double prevBackLeftPower = -10.501; + public void setBackLeftPower(double pow){ + pow = (double) Math.round(pow * roundingFactor) / roundingFactor; + if (pow != prevBackLeftPower){ + backLeft.setPower(pow); + } + prevBackLeftPower = pow; + } + + private double prevBackRightPower = -10.501; + public void setBackRightPower(double pow){ + pow = (double) Math.round(pow * roundingFactor) / roundingFactor; + if (pow != prevBackRightPower){ + backRight.setPower(pow); + } + prevBackRightPower = pow; + } + + private double prevIntakePower = -10.501; + public void setIntakePower(double pow){ + pow = (double) Math.round(pow * roundingFactor) / roundingFactor; + if (pow != prevIntakePower){ + intake.setPower(pow); + } + prevIntakePower = pow; + } + + private double prevTransferPower = -10.501; + public void setTransferPower(double pow){ + pow = (double) Math.round(pow * roundingFactor) / roundingFactor; + if (pow != prevTransferPower){ + transfer.setPower(pow); + } + prevTransferPower = pow; + } + + // shooter motors are done in separate class + + private double prevHoodPos = -10.501; + public void setHoodPos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevHoodPos){ + hood.setPosition(pos); + } + prevHoodPos = pos; + } + + private double prevTransferServoPos = -10.501; + public void setTransferServoPos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevTransferServoPos){ + transferServo.setPosition(pos); + } + prevTransferServoPos = pos; + } + + private double prevSpinPos = -10.501; + public void setSpinPos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevSpinPos){ + spin1.setPosition(pos); + spin2.setPosition(pos); + } + prevSpinPos = pos; + } + + private double prevTurretPos = -10.501; + public void setTurretPos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevTurretPos){ + turr1.setPosition(pos); + turr2.setPosition(pos); + } + prevTurretPos = pos; + } + + private double prevTilt1Pos = -10.501; + public void setTilt1Pos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevTilt1Pos){ + tilt1.setPosition(pos); + } + prevTilt1Pos = pos; + } + + private double prevTilt2Pos = -10.501; + public void setTilt2Pos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevTilt2Pos){ + tilt2.setPosition(pos); + } + prevTilt2Pos = pos; + } + + private double prevSpindexBlockerPos = -10.501; + public void setSpindexBlockerPos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevSpindexBlockerPos){ + spindexBlocker.setPosition(pos); + } + prevSpindexBlockerPos = pos; + } + + private double prevRapidFireBlockerPos = -10.501; + public void setRapidFireBlockerPos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevRapidFireBlockerPos){ + rapidFireBlocker.setPosition(pos); + } + prevRapidFireBlockerPos = pos; + } + +} 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 new file mode 100644 index 0000000..068aea2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -0,0 +1,160 @@ +package org.firstinspires.ftc.teamcode.utilsv2; + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.follower.Follower; + + +public class Shooter { + + Robot robot; + Flywheel fly; + Turret turr; + VelocityCommander commander; + + double goalX = 0.0; + double goalY = 0.0; + double obeliskX = 72; + double obeliskY = 144; + + private boolean red = true; + + + Follower follow; + + public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance) { + this.robot = rob; + this.fly = new Flywheel(rob); + this.turr = new Turret(rob); + this.follow = follower; + this.commander = new VelocityCommander(); + + setRedAlliance(redAlliance); + + if (redAlliance) { + goalX = 144; + } else { + goalX = 0; + } + goalY = 144; + } + + public void setRedAlliance(boolean input) { + this.red = input; + } + + private double flywheelVelocity = 0.0; + private double turretPosition = 0.5; + + public enum ShooterState { + READ_OBELISK, + TRACK_GOAL, + MANUAL_FLYWHEEL_TRACK_TURR, + MANUAL_TURRET_TRACK_FLY, + MANUAL, + NOTHING + } + + + private ShooterState state = ShooterState.NOTHING; + + public void setState(ShooterState shooterState) { + this.state = shooterState; + } + + public void setTurretPosition(double input) { + this.turretPosition = input; + } + + public void setFlywheelVelocity(double input) { + this.flywheelVelocity = input; + } + + public int getObeliskID() { + return turr.getObeliskID(); + } + + + public void update() { + + switch (state) { + case NOTHING: + break; + case MANUAL: + fly.manageFlywheel(flywheelVelocity); + turr.manual(turretPosition); + break; + case TRACK_GOAL: + turr.trackGoal( + (follow.getPose().getX() - goalX), + (follow.getPose().getY() - goalY), + follow.getHeading(), + follow.getAngularVelocity(), + follow.getVelocity().getXComponent(), + follow.getAcceleration().getXComponent(), + follow.getVelocity().getYComponent(), + follow.getAcceleration().getYComponent() + ); + + flywheelVelocity = commander.getVeloPredictive( + (follow.getPose().getX() - goalX), + (follow.getPose().getY() - goalY), + follow.getVelocity().getXComponent(), + follow.getAcceleration().getXComponent(), + follow.getVelocity().getYComponent(), + follow.getAcceleration().getYComponent() + ); + + fly.manageFlywheel(flywheelVelocity); + break; + case READ_OBELISK: + turr.trackObelisk( + (follow.getPose().getX() - goalX), + (follow.getPose().getY() - goalY), + follow.getHeading() + ); + + flywheelVelocity = commander.getVeloPredictive( + (follow.getPose().getX() - goalX), + (follow.getPose().getY() - goalY), + follow.getVelocity().getXComponent(), + follow.getAcceleration().getXComponent(), + follow.getVelocity().getYComponent(), + follow.getAcceleration().getYComponent() + ); + + fly.manageFlywheel(flywheelVelocity); + break; + + case MANUAL_TURRET_TRACK_FLY: + turr.manual(turretPosition); + flywheelVelocity = commander.getVeloPredictive( + (follow.getPose().getX() - goalX), + (follow.getPose().getY() - goalY), + follow.getVelocity().getXComponent(), + follow.getAcceleration().getXComponent(), + follow.getVelocity().getYComponent(), + follow.getAcceleration().getYComponent() + ); + + fly.manageFlywheel(flywheelVelocity); + break; + + case MANUAL_FLYWHEEL_TRACK_TURR: + turr.trackGoal( + (follow.getPose().getX() - goalX), + (follow.getPose().getY() - goalY), + follow.getHeading(), + follow.getAngularVelocity(), + follow.getVelocity().getXComponent(), + follow.getAcceleration().getXComponent(), + follow.getVelocity().getYComponent(), + follow.getAcceleration().getYComponent() + ); + fly.manageFlywheel(flywheelVelocity); + 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 new file mode 100644 index 0000000..0dc6cb4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java @@ -0,0 +1,183 @@ +package org.firstinspires.ftc.teamcode.utilsv2; + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.constants.ServoPositions; + +public class SpindexerTransferIntake { + + private final Robot robot; + + public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE) { + this.robot = rob; + } + + private final double sensorDistanceThreshold = 4.0; + private final long pulseTime = 50; // ms + + public enum SpindexerMode { + RAPID, + SORTED + } + + public enum RapidMode { + INTAKE, + TRANSFER_OFF, + BEFORE_PULSE_OUT, + PULSE_OUT, + PULSE_IN, + HOLD_BALLS, + OPEN_GATE, + SHOOT + } + + private SpindexerMode mode = SpindexerMode.RAPID; + private RapidMode rapidMode = RapidMode.INTAKE; + + /** + * Time when current state was entered. + */ + private long stateStartTime = System.currentTimeMillis(); + + public void setRapidMode(RapidMode newMode) { + if (rapidMode != newMode) { + rapidMode = newMode; + stateStartTime = System.currentTimeMillis(); + } + } + + public void setSpindexerMode(SpindexerMode spindexerMode) { + this.mode = spindexerMode; + } + + public RapidMode getRapidState(){ + return this.rapidMode; + } + + private long stateTime() { + return System.currentTimeMillis() - stateStartTime; + } + + public void update() { + + switch (mode) { + + case RAPID: + + robot.setSpindexBlockerPos( + ServoPositions.spindexBlocker_Open + ); + + switch (rapidMode) { + + case INTAKE: + + robot.setIntakePower(1); + robot.setTransferPower(1); + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Closed + ); + robot.setSpinPos( + ServoPositions.spindexer_A2 + ); + robot.setTransferServoPos( + ServoPositions.transferServo_out + ); + + if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) { + + setRapidMode(RapidMode.TRANSFER_OFF); + } + + + break; + + case TRANSFER_OFF: + + robot.setTransferPower(0.3); + + if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) { + setRapidMode(RapidMode.BEFORE_PULSE_OUT); + } + + break; + + case BEFORE_PULSE_OUT: + + robot.setIntakePower(1.0); + + if (stateTime() >= 300) { + setRapidMode(RapidMode.PULSE_OUT); + } + + break; + + case PULSE_OUT: + + robot.setIntakePower(-0.1); + + if (stateTime() >= pulseTime) { + setRapidMode(RapidMode.PULSE_IN); + } + + break; + + case PULSE_IN: + + robot.setIntakePower(1.0); + + if (stateTime() >= 200) { + setRapidMode(RapidMode.HOLD_BALLS); + } + + + break; + + case HOLD_BALLS: + + if (robot.insideBeam.isPressed() + && robot.outsideBeam.isPressed()) { + + robot.setIntakePower(0.1); + + } else { + + robot.setIntakePower(1); + } + + + + break; + + case OPEN_GATE: + + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Open + ); + + if (stateTime() >= 100) { + setRapidMode(RapidMode.SHOOT); + } + + break; + + case SHOOT: + + robot.setTransferServoPos( + ServoPositions.transferServo_in + ); + if (stateTime() >= 400) { + setRapidMode(RapidMode.INTAKE); + } + break; + } + break; + + case SORTED: + + // Future sorted-intake logic + break; + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java new file mode 100644 index 0000000..fd8edb8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java @@ -0,0 +1,111 @@ +package org.firstinspires.ftc.teamcode.utilsv2; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.robotcore.util.Range; + + +import java.util.List; + +@Config +public class Turret { + Robot robot; + + private final double servoTicksPer180 = 0.58; + private final double neutralPosition = 0.51; + private final double turretMin = 0.05; + private final double turretMax = 0.95; + private final double hVelK = 0; // TODO: Tune + private final double xVelK = 0; // TODO: Tune + private final double xAccK = 0; // TODO: Tune + private final double yVelK = 0; // TODO: Tune + private final double yAccK = 0; // TODO: Tune + + private int obeliskID = 0; + + + + public Turret(Robot rob) { + this.robot = rob; + } + + private double wrapAngle(double angle) { + while (angle > Math.PI) angle -= 2.0 * Math.PI; + while (angle < -Math.PI) angle += 2.0 * Math.PI; + return angle; + } + + public void trackObelisk(double dx, double dy, double h) { + + double heading = wrapAngle(h); + + double fieldRelativeHeading = Math.atan2(dy, dx); + + double desiredAngle = fieldRelativeHeading - heading; + double angleDelta = desiredAngle - Math.PI; + angleDelta = wrapAngle(angleDelta); + + double servoTicksFromNeutral = (angleDelta / (2.0 * Math.PI)) * (2.0 * servoTicksPer180); + + double servoAngle = neutralPosition + servoTicksFromNeutral; + + servoAngle = Range.clip(servoAngle, turretMin, turretMax); + + robot.setTurretPos(servoAngle); + + + detectObelisk(); + + } + + public int getObeliskID() { + return obeliskID; + } + + private int detectObelisk() { + robot.limelight.pipelineSwitch(1); + LLResult result = robot.limelight.getLatestResult(); + if (result != null && result.isValid()) { + List fiducials = result.getFiducialResults(); + double prevTx = -1000; + for (LLResultTypes.FiducialResult fiducial : fiducials) { + double currentTx = fiducial.getTargetXDegrees(); + if (currentTx > prevTx){ + obeliskID = fiducial.getFiducialId(); + } + } + } + return obeliskID; + } + + public void manual (double pos) { + robot.setTurretPos(pos); + + } + + + public void trackGoal(double dx, double dy, double h, double hVel, double xVel, double xAcc, double yVel, double yAcc) { + // dx, dy, dz is target - robot + // h is the raw heading where 0 degrees is positive x in the system of x, y + + double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot + double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK); // Negative bc dy = target - robot + double predictedH = h + (hVel * hVelK); // Positive bc h = robot heading + + predictedH = wrapAngle(predictedH); + + double fieldRelativeHeading = Math.atan2(predictedDy, predictedDx); + + double angleDelta = fieldRelativeHeading - predictedH; + angleDelta = wrapAngle(angleDelta); + + double servoTicksFromNeutral = (angleDelta / (2.0 * Math.PI)) * (2.0 * servoTicksPer180); + + double servoAngle = neutralPosition + servoTicksFromNeutral; + + servoAngle = Range.clip(servoAngle, turretMin, turretMax); + + robot.setTurretPos(servoAngle); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java new file mode 100644 index 0000000..297b723 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.utilsv2; + +public class VelocityCommander { + private final double goalH = 20.0; //TODO: Tune + private final double xVelK = 0; // TODO: Tune + private final double xAccK = 0; // TODO: Tune + private final double yVelK = 0; // TODO: Tune + private final double yAccK = 0; // TODO: Tune + + public VelocityCommander() { + + } + + private double distToRPM (double dist){ + return Math.sqrt(dist*dist + goalH*goalH); + //TODO: Add regression here using goalH + } + + public double getVeloStationary (double distance){ + return distToRPM(distance); + } + + public double getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc) { + + double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot + double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK); // Negative bc dy = target - robot + + double predictedDist = Math.sqrt(dx*dx + dy*dy); + + return distToRPM(predictedDist); + } + + +}