Added shooter class to manager flywheel turret and targetter

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java

@@ -20,6 +20,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferSe
 import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
 import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
 
+
 import androidx.annotation.NonNull;
 
 import com.acmerobotics.dashboard.FtcDashboard;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java

@@ -0,0 +1,132 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+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 com.qualcomm.robotcore.hardware.NormalizedRGBA;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@Config
+@TeleOp
+public class Hardware_Tester extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+
+    public static boolean subsystemMode = true;
+
+    // Bare Motor Powers
+    public static double flPow = 0;
+    public static double frPow = 0;
+    public static double blPow = 0;
+    public static double brPow = 0;
+    public static double intakePow = 0;
+    public static double transferPow = 0;
+    public static double shooter1Pow = 0;
+    public static double shooter2Pow = 0;
+
+    // Subsystem Motor Powers
+    public static double drivetrainPow = 0;
+    public static double shooterPow = 0;
+
+    // Bare Servo Positions
+    public static double spin1Pos = 0.501;
+    public static double spin2Pos = 0.501;
+    public static double turr1Pos = 0.501;
+    public static double turr2Pos = 0.501;
+    public static double transferServosPos = 0.501;
+    public static double hoodPos = 0.501;
+    public static double spindexBlockerPos = 0.501;
+    public static double rapidFireBlockerPos = 0.501;
+    public static double tilt1Pos = 0.501;
+    public static double tilt2Pos = 0.501;
+
+    // Subsystem Servo Positions
+    public static double spinPos = 0.501;
+    public static double turrPos = 0.501;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        robot = new Robot(hardwareMap);
+        TELE = new MultipleTelemetry();
+
+        if (isStopRequested()) return;
+        while (opModeIsActive()){
+            // Non-subsystem based components
+            robot.setIntakePower(intakePow);
+            robot.setTransferPower(transferPow);
+
+            if (transferServosPos != 0.501){
+                robot.setTransferServoPos(transferServosPos);
+            }
+            if (hoodPos != 0.501){
+                robot.setHoodPos(hoodPos);
+            }
+            if (rapidFireBlockerPos != 0.501){
+                robot.setRapidFireBlockerPos(rapidFireBlockerPos);
+            }
+            if (spindexBlockerPos != 0.501){
+                robot.setSpindexBlockerPos(spindexBlockerPos);
+            }
+            if (tilt1Pos != 0.501){
+                robot.setTilt1Pos(tilt1Pos);
+            }
+            if (tilt2Pos != 0.501){
+                robot.setTilt2Pos(tilt2Pos);
+            }
+
+            // Subsystem based components
+            if (subsystemMode){
+                robot.setFrontLeftPower(drivetrainPow);
+                robot.setFrontRightPower(drivetrainPow);
+                robot.setBackLeftPower(drivetrainPow);
+                robot.setBackRightPower(drivetrainPow);
+                robot.shooter1.setPower(shooterPow);
+                robot.shooter2.setPower(shooterPow);
+
+                if (spinPos != 0.501){
+                    robot.setSpinPos(spinPos);
+                }
+                if (turrPos != 0.501){
+                    robot.setTurretPos(turrPos);
+                }
+            } else {
+                robot.setFrontLeftPower(flPow);
+                robot.setFrontRightPower(frPow);
+                robot.setBackLeftPower(blPow);
+                robot.setBackRightPower(brPow);
+                robot.shooter1.setPower(shooter1Pow);
+                robot.shooter2.setPower(shooter2Pow);
+
+                if (spin1Pos != 0.501){
+                    robot.spin1.setPosition(spin1Pos);
+                }
+                if (spin2Pos != 0.501){
+                    robot.spin2.setPosition(spin2Pos);
+                }
+                if (turr1Pos != 0.501){
+                    robot.turr1.setPosition(turr1Pos);
+                }
+                if (turr2Pos != 0.501){
+                    robot.turr2.setPosition(turr2Pos);
+                }
+            }
+
+            // Sensor Data
+
+            TELE.addData("Beam Break 1?", robot.beam1.isPressed());
+            TELE.addData("Beam Break 2?", robot.beam2.isPressed());
+            TELE.addData("Beam Break 3?", robot.beam3.isPressed());
+
+            NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
+            TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.MM));
+            TELE.addData("REV Green", revColor.green / (revColor.red + revColor.blue + revColor.green));
+
+            TELE.addData("Voltage Sensor", robot.voltage.getVoltage());
+
+            TELE.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java

@@ -10,6 +10,7 @@ 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;
@@ -38,12 +39,22 @@ public class Robot {
     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 beam1;
+    public TouchSensor beam2;
+    public TouchSensor beam3;
+    public RevColorSensorV3 revSensor;
+
+    public VoltageSensor voltage;
+
+    // Below is disregarded
     public AnalogInput spin1Pos;
     public AnalogInput spin2Pos;
     public AnalogInput turr1Pos;
@@ -55,7 +66,6 @@ public class Robot {
     public RevColorSensorV3 color3;
     public Limelight3A limelight;
     public Servo light;
-    public VoltageSensor voltage;
 
     public Robot(HardwareMap hardwareMap) {
 
@@ -89,29 +99,44 @@ public class Robot {
 
         hood = hardwareMap.get(Servo.class, "hood");
 
-        turr1 = hardwareMap.get(Servo.class, "t1");
+        turr1 = hardwareMap.get(Servo.class, "turr1");
 
-        turr2 = hardwareMap.get(Servo.class, "t2");
+        turr2 = hardwareMap.get(Servo.class, "turr2");
-
-        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
 
         spin1 = hardwareMap.get(Servo.class, "spin2");
 
-        spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
-
         spin2 = hardwareMap.get(Servo.class, "spin1");
 
-        spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
-
         transfer = hardwareMap.get(DcMotorEx.class, "transfer");
 
         transferServo = hardwareMap.get(Servo.class, "transferServo");
 
-        transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
-
         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");
+
+        beam1 = hardwareMap.get(TouchSensor.class, "beam1");
+        beam2 = hardwareMap.get(TouchSensor.class, "beam2");
+        beam3 = hardwareMap.get(TouchSensor.class, "beam3");
+
+        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");
@@ -128,4 +153,140 @@ public class Robot {
         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;
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java

@@ -76,10 +76,10 @@ public class Drivetrain {
         double frontRightPower = (y - x - rx) / denominator;
         double backRightPower = (y + x - rx) / denominator;
 
-        robot.frontLeft.setPower(frontLeftPower);
+        robot.setFrontLeftPower(frontLeftPower);
-        robot.backLeft.setPower(backLeftPower);
+        robot.setBackLeftPower(backLeftPower);
-        robot.frontRight.setPower(frontRightPower);
+        robot.setFrontRightPower(frontRightPower);
-        robot.backRight.setPower(backRightPower);
+        robot.setBackRightPower(backRightPower);
 
         if (tele) {
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java

@@ -26,9 +26,9 @@ public class Flywheel {
 
     private final LinkedList<Double> velocityHistory = new LinkedList<>();
 
-    public Flywheel(HardwareMap hardwareMap) {
+    public Flywheel(Robot rob) {
 
-        robot = new Robot(hardwareMap);
+        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);
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java

@@ -1,5 +1,162 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
 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;
+            goalY = 144;
+        } else {
+            goalX = 0;
+            goalY = 144;
+        }
+    }
+
+    public void setRedAlliance(boolean input) {
+        this.red = input;
+    }
+
+    private double flywheelVelocity = 0.0;
+    private double turretPosition = 0.5;
+
+    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;
+
+        }
+
+    }
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java

@@ -18,7 +18,6 @@ public class Turret {
     private final double turretMin = 0.04; //TODO: Tune
     private final double turretMax = 0.94; //TODO: Tune
     private final double hVelK = 0; // TODO: Tune
-    private final double hAccK = 0; // TODO: Tune
     private final double xVelK = 0; // TODO: Tune
     private final double xAccK = 0; // TODO: Tune
     private final double yVelK = 0; // TODO: Tune
@@ -81,14 +80,19 @@ public class Turret {
         return obeliskID;
     }
 
+    public void manual (double pos) {
+        robot.turr1.setPosition(pos);
+        robot.turr2.setPosition(pos);
+    }
 
-    public void trackGoal(double dx, double dy, double h, double hVel, double hAcc, double xVel, double xAcc, double yVel, double yAcc) {
+
+    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) + (0.5 * hAcc * hAccK); // Positive bc h = robot heading
+        double predictedH = h + (hVel * hVelK); // Positive bc h = robot heading
 
         predictedH = wrapAngle(predictedH);
 

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);
+    }
+
+
+}