Merge remote-tracking branch 'origin/master'

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java

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

@@ -6,8 +6,6 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.controller.PIDFController;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.CRServo;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
 
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
@@ -46,7 +44,7 @@ public class PIDServoTest extends LinearOpMode {
             controller.setPIDF(p, i, d, f);
 
             if (mode == 0) {
-                pos = scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3);
+                pos = robot.turr1Pos.getCurrentPosition();
 
                 double pid = controller.calculate(pos, target);
 
@@ -62,7 +60,7 @@ public class PIDServoTest extends LinearOpMode {
             }
 
             telemetry.addData("pos", pos);
-            telemetry.addData("Turret Voltage", robot.turr1Pos.getVoltage());
+            telemetry.addData("Turret Voltage", robot.turr1Pos.getCurrentPosition());
             telemetry.addData("Spindex Voltage", robot.spin1Pos.getVoltage());
             telemetry.addData("target", target);
             telemetry.addData("Mode", mode);
@@ -71,4 +69,5 @@ public class PIDServoTest extends LinearOpMode {
         }
 
     }
+
 }

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

@@ -9,7 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 
-import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.FlywheelV2;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
 @Config
@@ -26,7 +26,7 @@ public class ShooterTest extends LinearOpMode {
     public static double turretPos = 0.501;
     public static boolean shoot = false;
     Robot robot;
-    Flywheel flywheel;
+    FlywheelV2 flywheel;
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -34,7 +34,7 @@ public class ShooterTest extends LinearOpMode {
         robot = new Robot(hardwareMap);
         DcMotorEx leftShooter = robot.shooter1;
         DcMotorEx rightShooter = robot.shooter2;
-        flywheel = new Flywheel();
+        flywheel = new FlywheelV2();
 
         MultipleTelemetry TELE = new MultipleTelemetry(
                 telemetry, FtcDashboard.getInstance().getTelemetry()
@@ -50,7 +50,7 @@ public class ShooterTest extends LinearOpMode {
                 rightShooter.setPower(parameter);
                 leftShooter.setPower(parameter);
             } else if (mode == 1) {
-                double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition());
+                double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
                 rightShooter.setPower(powPID);
                 leftShooter.setPower(powPID);
                 TELE.addData("PIDPower", powPID);
@@ -71,7 +71,9 @@ public class ShooterTest extends LinearOpMode {
             } else {
                 robot.transferServo.setPosition(transferServo_out);
             }
-            TELE.addData("Velocity", flywheel.getVelo());
+            TELE.addData("Velocity", flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()));
+            TELE.addData("Velocity 1", flywheel.getVelo1());
+            TELE.addData("Velocity 2", flywheel.getVelo2());
             TELE.addData("Power", robot.shooter1.getPower());
             TELE.addData("Steady?", flywheel.getSteady());
             TELE.addData("Position", robot.shooter1.getCurrentPosition());

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

@@ -73,12 +73,18 @@ public class FlywheelV2 {
         targetVelocity = commandedVelocity;
         velo = getVelo(shooter1CurPos, shooter2CurPos);
         // Flywheel PID code here
-        if (targetVelocity - velo > 500) {
+        if (targetVelocity - velo > 4500) {
             powPID = 1.0;
-        } else if (velo - targetVelocity > 500) {
+        } else if (velo - targetVelocity > 4500) {
             powPID = 0.0;
         } else {
-            double feed = Math.log((668.39 / (targetVelocity + 591.96)) - 0.116) / -4.18;
+
+            double a = 2539.07863;
+            double c = 1967.6498;
+            double d = -0.289647;
+            double h = -1.1569;
+
+            double feed = Math.log((a / (targetVelocity + c)) + d) / h;
 
             // --- PROPORTIONAL CORRECTION ---
             double error = targetVelocity - velo;
@@ -88,7 +94,7 @@ public class FlywheelV2 {
             correction = Math.max(-maxStep, Math.min(maxStep, correction));
 
             // --- FINAL MOTOR POWER ---
-            powPID = feed + correction;
+            powPID = feed;
 
             // clamp to allowed range
             powPID = Math.max(0, Math.min(1, powPID));

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

@@ -61,13 +61,25 @@ public class PositionalServoProgrammer extends LinearOpMode {
             if (hoodPos != 0.501){
                 robot.hood.setPosition(hoodPos);
             }
-            TELE.addData("spindexer", servo.getSpinPos());
+            // To check configuration of spindexer:
-            TELE.addData("turret", servo.getTurrPos());
+            // Set "mode" to 1 and spindexPow to 0.1
-            TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage());
+            // If the spindexer is turning clockwise, the servos are reversed. Swap the configuration of the two servos, DO NOT TOUCH THE ACTUAL CODE
-            TELE.addData("hood voltage", robot.hoodPos.getVoltage());
+            // Do the previous test again to confirm
+            // Set "mode" to 0 but keep spindexPos at 0.501
+            // Manually turn the spindexer until "spindexer pos" is set close to 0
+            // Check each spindexer voltage:
+                // If "spindexer voltage 1" is closer to 0 than "spindexer voltage 2," then you are done!
+                // If "spindexer voltage 2" is closer to 0 than "spindexer voltage 1," swap the two spindexer analog inputs in the configuration, DO NOT TOUCH THE ACTUAL CODE
+            //TODO: @KeshavAnandCode do the above please
+
+            TELE.addData("spindexer pos", servo.getSpinPos());
+            TELE.addData("turret pos", servo.getTurrPos());
+            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("turret voltage", robot.turr1Pos.getVoltage());
+            TELE.addData("turret voltage", robot.turr1Pos.getCurrentPosition());
-            TELE.addData("Spin Equal", servo.spinEqual(spindexPos));
+            TELE.addData("spindexer pow", robot.spin1.getPower());
             TELE.update();
         }
     }

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

@@ -7,7 +7,6 @@ import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.DigitalChannel;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
 
@@ -28,28 +27,16 @@ public class Robot {
     public DcMotorEx shooter2;
     public Servo hood;
     public Servo transferServo;
-    public Servo rejecter;
     public CRServo turr1;
     public CRServo turr2;
     public CRServo spin1;
     public CRServo spin2;
-    public DigitalChannel pin0;
-    public DigitalChannel pin1;
-    public DigitalChannel pin2;
-    public DigitalChannel pin3;
-    public DigitalChannel pin4;
-    public DigitalChannel pin5;
-    public AnalogInput analogInput;
-    public AnalogInput analogInput2;
     public AnalogInput spin1Pos;
     public AnalogInput spin2Pos;
-    public AnalogInput hoodPos;
+    public DcMotorEx turr1Pos;
-    public AnalogInput turr1Pos;
-    public AnalogInput turr2Pos;
     public AnalogInput transferServoPos;
     public AprilTagProcessor aprilTagProcessor;
     public WebcamName webcam;
-    public DcMotorEx shooterEncoder;
     public RevColorSensorV3 color1;
     public RevColorSensorV3 color2;
     public RevColorSensorV3 color3;
@@ -57,10 +44,12 @@ public class Robot {
 
     public static boolean usingLimelight = true;
 
+    public static boolean usingCamera = true;
+
     public Robot(HardwareMap hardwareMap) {
 
         //Define components w/ hardware map
-
+        //TODO: fix the configuration of these - I trust you to figure it out yourself @KeshavAnandCode
         frontLeft = hardwareMap.get(DcMotorEx.class, "fl");
         frontRight = hardwareMap.get(DcMotorEx.class, "fr");
         backLeft = hardwareMap.get(DcMotorEx.class, "bl");
@@ -74,30 +63,24 @@ public class Robot {
         backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
 
         intake = hardwareMap.get(DcMotorEx.class, "intake");
-        rejecter = hardwareMap.get(Servo.class, "rejecter");
 
         shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
 
         shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
-
+        //TODO: figure out which shooter motor is reversed using ShooterTest and change it in code @KeshavAnandCode
         shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
         shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
-        shooterEncoder = shooter1;
-
         hood = hardwareMap.get(Servo.class, "hood");
 
-        hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");
-
         turr1 = hardwareMap.get(CRServo.class, "t1");
 
-        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos");
-
         turr2 = hardwareMap.get(CRServo.class, "t2");
 
-        turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos");
+        turr1Pos = intake; // Encoder of turret plugged in intake port
 
+        //TODO: check spindexer configuration (both servo and analog input) - check comments in PositionalServoProgrammer
         spin1 = hardwareMap.get(CRServo.class, "spin1");
 
         spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
@@ -109,22 +92,6 @@ public class Robot {
         spin1.setDirection(DcMotorSimple.Direction.REVERSE);
         spin2.setDirection(DcMotorSimple.Direction.REVERSE);
 
-        pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
-
-        pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
-
-        pin2 = hardwareMap.get(DigitalChannel.class, "pin2");
-
-        pin3 = hardwareMap.get(DigitalChannel.class, "pin3");
-
-        pin4 = hardwareMap.get(DigitalChannel.class, "pin4");
-
-        pin5 = hardwareMap.get(DigitalChannel.class, "pin5");
-
-        analogInput = hardwareMap.get(AnalogInput.class, "analog");
-
-        analogInput2 = hardwareMap.get(AnalogInput.class, "analog2");
-
         transfer = hardwareMap.get(DcMotorEx.class, "transfer");
 
         transferServo = hardwareMap.get(Servo.class, "transferServo");
@@ -132,10 +99,7 @@ public class Robot {
         transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
 
         transfer.setDirection(DcMotorSimple.Direction.REVERSE);
-
+        transfer.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
-
-        webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
 
         color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
 
@@ -145,6 +109,9 @@ public class Robot {
 
         if (usingLimelight){
             limelight = hardwareMap.get(Limelight3A.class, "limelight");
+        } else if (usingCamera){
+            webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
+            aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
         }
     }
 }

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

@@ -14,7 +14,7 @@ public class Servos {
 
     //PID constants
     // TODO: get PIDF constants
-    public static double spinP = 3.4, spinI = 0, spinD = 0.075, spinF = 0.02;
+    public static double spinP = 3.3, spinI = 0, spinD = 0.1, spinF = 0.02;
     public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF = 0.0;
 
     public static double spin_scalar = 1.0086;
@@ -45,7 +45,7 @@ public class Servos {
     }
 
     public double getTurrPos() {
-        return turret_scalar * ((robot.turr1Pos.getVoltage() - turret_restPos) / 3.3);
+        return robot.turr1Pos.getCurrentPosition();
     }
 
     public double setTurrPos(double pos) {