turret

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java

@@ -120,6 +120,7 @@ public class TeleopV3 extends LinearOpMode {
     private double transferStamp = 0.0;
     private int tickerA = 1;
     private boolean transferIn = false;
+    boolean turretInterpolate = false;
 
     public static double velPrediction(double distance) {
         if (distance < 30) {
@@ -394,7 +395,7 @@ public class TeleopV3 extends LinearOpMode {
             double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
 
             targetingSettings = targeting.calculateSettings
-                    (robotX, robotY, robotHeading, 0.0);
+                    (robotX,robotY,robotHeading,0.0, turretInterpolate);
 
             turret.trackGoal(deltaPose);
 
@@ -636,6 +637,7 @@ public class TeleopV3 extends LinearOpMode {
                 }
             }
 
+
 //
 //            if (shootAll) {
 //
@@ -807,6 +809,7 @@ public class TeleopV3 extends LinearOpMode {
             for (LynxModule hub : allHubs) {
                 hub.clearBulkCache();
             }
+
 //
             TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
             TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
@@ -833,9 +836,10 @@ public class TeleopV3 extends LinearOpMode {
             TELE.addData("shootall commanded", shootAll);
             // Targeting Debug
             TELE.addData("robotX", robotX);
-            TELE.addData("robotY", robotY);
+            TELE.addData( "robotY", robotY);
             TELE.addData("robotInchesX", targeting.robotInchesX);
-            TELE.addData("robotInchesY", targeting.robotInchesY);
+            TELE.addData( "robotInchesY", targeting.robotInchesY);
+            TELE.addData("Targeting Interpolate", turretInterpolate);
             TELE.addData("Targeting GridX", targeting.robotGridX);
             TELE.addData("Targeting GridY", targeting.robotGridY);
             TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);

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

@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.tests;
 
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -55,7 +56,7 @@ public class ShooterTest extends LinearOpMode {
                 rightShooter.setPower(parameter);
                 leftShooter.setPower(parameter);
             } else if (mode == 1) {
-                flywheel.setPIDF(P,I,D,F);
+                flywheel.setPIDF(P, I, D, F);
                 flywheel.manageFlywheel((int) Velocity);
             }
 
@@ -63,7 +64,6 @@ public class ShooterTest extends LinearOpMode {
                 robot.hood.setPosition(hoodPos);
             }
 
-
             robot.transfer.setPower(transferPower);
             if (shoot) {
                 robot.transferServo.setPosition(transferServo_in);

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

@@ -16,6 +16,8 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 @Autonomous
 @Config
 public class TurretTest extends LinearOpMode {
+
+    public static boolean zeroTurr = false;
     @Override
     public void runOpMode() throws InterruptedException {
 
@@ -41,6 +43,12 @@ public class TurretTest extends LinearOpMode {
             webcam.update();
             webcam.displayAllTelemetry();
 
+            TELE.addData("tpos", turret.getTurrPos());
+
+            if(zeroTurr){
+                turret.zeroTurretEncoder();
+            }
+
 
 
             TELE.update();

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

@@ -8,6 +8,8 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+
 @TeleOp
 @Config
 public class PositionalServoProgrammer extends LinearOpMode {
@@ -25,11 +27,19 @@ public class PositionalServoProgrammer extends LinearOpMode {
     public static double hoodPos = 0.501;
     public static int mode = 0; //0 for positional, 1 for power
 
+    Turret turret;
+
     @Override
     public void runOpMode() throws InterruptedException {
         robot = new Robot(hardwareMap);
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
         servo = new Servos(hardwareMap);
+
+        AprilTagWebcam cam = new AprilTagWebcam();
+
+        cam.init(robot, TELE);
+
+        turret = new Turret(robot, TELE,cam );
         waitForStart();
         if (isStopRequested()) return;
         while (opModeIsActive()){
@@ -66,12 +76,13 @@ public class PositionalServoProgrammer extends LinearOpMode {
             //TODO: @KeshavAnandCode do the above please
 
             TELE.addData("spindexer pos", servo.getSpinPos());
-            TELE.addData("turret pos", servo.getTurrPos());
+            TELE.addData("turret pos", robot.turr1.getPosition());
             TELE.addData("spindexer voltage 1", robot.spin1Pos.getVoltage());
             TELE.addData("spindexer voltage 2", robot.spin2Pos.getVoltage());
             TELE.addData("hood pos", robot.hood.getPosition());
             TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage());
             TELE.addData("spindexer pow", robot.spin1.getPower());
+            TELE.addData("tpos ", turret.getTurrPos() );
             TELE.update();
         }
     }

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

@@ -79,10 +79,10 @@ public class Robot {
         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(1400);
+        shooter1.setVelocity(0);
         shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
-        shooter2.setVelocity(1400);
+        shooter2.setVelocity(0);
 
         hood = hardwareMap.get(Servo.class, "hood");
 

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

@@ -52,7 +52,9 @@ public class Spindexer {
     }
 
     enum IntakeState {
-        UNKNOWN,
+        UNKNOWN_START,
+        UNKNOWN_MOVE,
+        UNKNOWN_DETECT,
         INTAKE,
         FINDNEXT,
         MOVING,
@@ -62,8 +64,8 @@ public class Spindexer {
         SHOOTWAIT,
     };
 
-    public IntakeState currentIntakeState = IntakeState.UNKNOWN;
+    public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
-
+    public int unknownColorDetect = 0;
     enum BallColor {
         UNKNOWN,
         GREEN,
@@ -131,13 +133,13 @@ public class Spindexer {
         for (int i = 0; i < 3; i++) {
             resetBallPosition(i);
         }
-        currentIntakeState = IntakeState.UNKNOWN;
+        currentIntakeState = IntakeState.UNKNOWN_START;
     }
 
     // Detects if a ball is found and what color.
     // Returns true is there was a new ball found in Position 1
     // FIXIT: Reduce number of times that we read the color sensors for loop times.
-    public boolean detectBalls() {
+    public boolean detectBalls(boolean detectRearColor, boolean detectFrontColor) {
 
         boolean newPos1Detection = false;
         int spindexerBallPos = 0;
@@ -153,6 +155,7 @@ public class Spindexer {
             // Mark Ball Found
             newPos1Detection = true;
 
+            if (detectRearColor) {
                 // Detect which color
                 double green = robot.color1.getNormalizedColors().green;
                 double red = robot.color1.getNormalizedColors().red;
@@ -167,24 +170,26 @@ public class Spindexer {
                     ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN;  // purple
                 }
            }
+        }
         // Position 2
         // Find which ball position this is in the spindexer
         spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()];
         if (distanceFrontDriver < 60) {
             // reset FoundEmpty because looking for 3 in a row before reset
             ballPositions[spindexerBallPos].foundEmpty = 0;
-            // FIXIT: Comment out for now due to loop time concerns
+            if (detectFrontColor) {
-//            double green = robot.color2.getNormalizedColors().green;
+                double green = robot.color2.getNormalizedColors().green;
-//            double red = robot.color2.getNormalizedColors().red;
+                double red = robot.color2.getNormalizedColors().red;
-//            double blue = robot.color2.getNormalizedColors().blue;
+                double blue = robot.color2.getNormalizedColors().blue;
-//
-//            double gP = green / (green + red + blue);
 
-//           if (gP >= 0.4) {
+                double gP = green / (green + red + blue);
-//               b2 = 2;  // purple
+
-//           } else {
+                if (gP >= 0.4) {
-//               b2 = 1;  // green
+                    ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE;  // purple
-//           }
+                } else {
+                    ballPositions[spindexerBallPos].ballColor = BallColor.GREEN;  // purple
+                }
+            }
         } else {
             if (!ballPositions[spindexerBallPos].isEmpty) {
                 if (ballPositions[spindexerBallPos].foundEmpty > 3) {
@@ -201,18 +206,19 @@ public class Spindexer {
 
             // reset FoundEmpty because looking for 3 in a row before reset
             ballPositions[spindexerBallPos].foundEmpty = 0;
-            // FIXIT: Comment out for now due to loop time concerns
+            if (detectFrontColor) {
-//            double green = robot.color3.getNormalizedColors().green;
+                double green = robot.color3.getNormalizedColors().green;
-//            double red = robot.color3.getNormalizedColors().red;
+                double red = robot.color3.getNormalizedColors().red;
-//            double blue = robot.color3.getNormalizedColors().blue;
+                double blue = robot.color3.getNormalizedColors().blue;
 
-//            double gP = green / (green + red + blue);
+                double gP = green / (green + red + blue);
 
-//           if (gP >= 0.4) {
+                if (gP >= 0.4) {
-//               b3 = 2;  // purple
+                    ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE;  // purple
-//           } else {
+                } else {
-//               b3 = 1;  // green
+                    ballPositions[spindexerBallPos].ballColor = BallColor.GREEN;  // purple
-//           }
+                }
+            }
         } else {
             if (!ballPositions[spindexerBallPos].isEmpty) {
                 if (ballPositions[spindexerBallPos].foundEmpty > 3) {
@@ -255,15 +261,35 @@ public class Spindexer {
     public boolean processIntake() {
 
         switch (currentIntakeState) {
-            case UNKNOWN:
+            case UNKNOWN_START:
                 // For now just set position ONE if UNKNOWN
                 commandedIntakePosition = 0;
                 servos.setSpinPos(intakePositions[0]);
-                currentIntakeState = Spindexer.IntakeState.MOVING;
+                currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE;
+                break;
+            case UNKNOWN_MOVE:
+                // Stopping when we get to the new position
+                if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
+                    currentIntakeState = Spindexer.IntakeState.UNKNOWN_DETECT;
+                    stopSpindexer();
+                    detectBalls(true, true);
+                    unknownColorDetect = 0;
+                } else {
+                    // Keep moving the spindexer
+                    moveSpindexerToPos(intakePositions[commandedIntakePosition]);
+                }
+                break;
+            case UNKNOWN_DETECT:
+                if (unknownColorDetect >5) {
+                    currentIntakeState = Spindexer.IntakeState.FINDNEXT;
+                } else {
+                    detectBalls(true, true);
+                    unknownColorDetect++;
+                }
                 break;
             case INTAKE:
                 // Ready for intake and Detecting a New Ball
-                if (detectBalls()) {
+                if (detectBalls(true, false)) {
                     ballPositions[commandedIntakePosition].isEmpty = false;
                     currentIntakeState = Spindexer.IntakeState.FINDNEXT;
                 } else {
@@ -311,7 +337,7 @@ public class Spindexer {
                 if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
                     currentIntakeState = Spindexer.IntakeState.INTAKE;
                     stopSpindexer();
-                    detectBalls();
+                    detectBalls(false, false);
                 } else {
                     // Keep moving the spindexer
                     moveSpindexerToPos(intakePositions[commandedIntakePosition]);
@@ -320,7 +346,7 @@ public class Spindexer {
 
             case FULL:
                 // Double Check Colors
-                detectBalls();
+                detectBalls(false, false); // Minimize hardware calls
                 if (ballPositions[0].isEmpty || ballPositions[1].isEmpty || ballPositions[2].isEmpty) {
                     // Error handling found an empty spot, get it ready for a ball
                     currentIntakeState = Spindexer.IntakeState.FINDNEXT;
@@ -378,7 +404,7 @@ public class Spindexer {
                 if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
                     currentIntakeState = Spindexer.IntakeState.INTAKE;
                     stopSpindexer();
-                    detectBalls();
+                    detectBalls(true, false);
                 } else {
                     // Keep moving the spindexer
                     moveSpindexerToPos(intakePositions[commandedIntakePosition]);

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

@@ -84,28 +84,58 @@ public class Targeting {
     {
     }
 
-    public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity) {
+    public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) {
         Settings recommendedSettings = new Settings(0.0, 0.0);
 
         double cos45 = Math.cos(Math.toRadians(-45));
         double sin45 = Math.sin(Math.toRadians(-45));
-        double rotatedY = (robotX -40.0) * sin45 + (robotY +7.0) * cos45;
+        double rotatedY = (robotX - 40.0) * sin45 + (robotY + 7.0) * cos45;
-        double rotatedX = (robotX -40.0) * cos45 - (robotY +7.0) * sin45;
+        double rotatedX = (robotX - 40.0) * cos45 - (robotY + 7.0) * sin45;
 
         // Convert robot coordinates to inches
         robotInchesX = rotatedX * unitConversionFactor;
         robotInchesY = rotatedY * unitConversionFactor;
 
         // Find approximate location in the grid
-        robotGridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) +1);
+        int gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
-        robotGridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
+        int gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
 
-        // Use Grid Location to perform lookup
+        //clamp
-        // Keep it simple for now but may want to interpolate results
+        robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
+        robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
+
+        // basic search
+        if(!interpolate) {
             if ((robotGridY < 6) && (robotGridX <6)) {
                 recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM;
                 recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle;
             }
             return recommendedSettings;
+        } else {
+
+            // bilinear interpolation
+            int x0 = robotGridX;
+            int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1);
+            int y0 = gridY;
+            int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1);
+
+            double x = (robotInchesX - (x0 * tileSize)) / tileSize;
+            double y = (robotInchesY - (y0 * tileSize)) / tileSize;
+
+            double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM;
+            double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM;
+            double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM;
+            double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM;
+
+            double angle00 = KNOWNTARGETING[y0][x0].hoodAngle;
+            double angle10 = KNOWNTARGETING[y0][x1].hoodAngle;
+            double angle01 = KNOWNTARGETING[y1][x0].hoodAngle;
+            double angle11 = KNOWNTARGETING[y1][x1].hoodAngle;
+
+            recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11;
+            recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11;
+
+            return recommendedSettings;
+        }
     }
 }

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

@@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
+import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 
@@ -12,13 +13,15 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 public class Turret {
 
     public static double turretTolerance = 0.02;
-    public static double turrPosScalar = 1.009;
+    public static double turrPosScalar = 0.00011264432;
     public static double turret180Range = 0.4;
     public static double turrDefault = 0.4;
     public static double cameraBearingEqual = 1;
     public static double errorLearningRate = 0.15;
     public static double turrMin = 0.2;
     public static double turrMax = 0.8;
+    public static double mult = 0.0;
+    private boolean lockOffset = false;
     public static double deltaAngleThreshold = 0.02;
     public static double angleMultiplier = 0.0;
     Robot robot;
@@ -29,7 +32,7 @@ public class Turret {
     private double offset = 0.0;
     private double bearing = 0.0;
 
-
+    public static double clampTolerance = 0.03;
 
     public Turret(Robot rob, MultipleTelemetry tele, AprilTagWebcam cam) {
         this.TELE = tele;
@@ -38,13 +41,18 @@ public class Turret {
     }
 
     public double getTurrPos() {
-        return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3);
+        return turrPosScalar * (robot.intake.getCurrentPosition()) + turrDefault;
 
     }
 
-    public void manualSetTurret(double pos){
+    public void zeroTurretEncoder() {
+        robot.intake.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+    }
+
+    public void manualSetTurret(double pos) {
         robot.turr1.setPosition(pos);
-        robot.turr2.setPosition(1-pos);
+        robot.turr2.setPosition(1 - pos);
     }
 
     public boolean turretEqual(double pos) {
@@ -89,7 +97,13 @@ public class Turret {
         return obeliskID;
     }
 
+    public void zeroOffset() {
+        offset = 0.0;
+    }
 
+    public void lockOffset(boolean lock) {
+        lockOffset = lock;
+    }
 
     /*
         Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
@@ -120,10 +134,7 @@ public class Turret {
 //
         double tagBearingDeg = getBearing();  // + = target is to the left
 
-        if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual) {
+
-            // Slowly learn turret offset (persistent calibration)
-            offset -= tagBearingDeg * errorLearningRate;
-        }
 
         turretAngleDeg += offset;
 
@@ -131,8 +142,22 @@ public class Turret {
 
         double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
 
+        double currentEncoderPos = this.getTurrPos();
+
+        if (!turretEqual(turretPos)) {
+            double diff = turretPos - currentEncoderPos;
+            turretPos = turretPos + diff * mult;
+        }
+
+        if (currentEncoderPos < (turrMin + clampTolerance) || currentEncoderPos > (turrMax - clampTolerance)) {
             // Clamp to servo range
             turretPos = Math.max(turrMin, Math.min(turretPos, turrMax));
+        } else {
+            if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual && !lockOffset) {
+                // Slowly learn turret offset (persistent calibration)
+                offset -= tagBearingDeg * errorLearningRate;
+            }
+        }
 
         robot.turr1.setPosition(turretPos);
         robot.turr2.setPosition(1.0 - turretPos);