cowtown end

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

@@ -47,7 +47,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
     public static double rapidShootTime = 0.4;
     public static double openGate1Time = 1.8;
     public static double openGate2Time = 1;
-    public static double openGateWaitTimeMax = 3.5;
+    public static double openGateWaitTimeMax = 3;
     public static int maxLoopCycles = 4;
 
     // Initialize path state machine
@@ -76,7 +76,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
     public static double shoot2X = 16, shoot2Y = 4, shoot2H = -30;
     public static double intakeGateControlX = 60, intakeGateControlY = -12;
     public static double toGateX = 60, toGateY = -10;
-    public static double intakeGateX = 62, intakeGateY = -12.5, intakeGateH = 25;
+    public static double intakeGateX = 62, intakeGateY = -12, intakeGateH = 25;
     public static double shootGateControlX = 40, shootGateControlY = -5;
     public static double shootGateX = 16, shootGateY = 4, shootGateH = -30;
     public static double shootLeaveControlX = 56, shootLeaveControlY = -10;
@@ -294,7 +294,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                 if (currentTime - timeStamp > rapidShootTime ||
                         (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
                                 spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
-                    follower.followPath(shoot2_intakeGate, false);
+                    follower.followPath(shoot2_intakeGate, true);
                     pathState = PathState.INTAKE_GATE;
                     timeStamp = currentTime;
                     toGateBool = true;
@@ -302,10 +302,10 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                 break;
             case INTAKE_GATE:
                 if ((currentTime - timeStamp > openGateWaitTimeMax)){
-                    if (getRuntime() - runtime > 27){
+                    if (cycle == 0){
-                        follower.followPath(intakeGate_awayFromGate, true);
+                        openGateWaitTimeMax = openGateWaitTimeMax + 0.75;
-                        pathState = PathState.WAIT_SHOOT_LEAVE;
+                    }
-                    } else if (getRuntime() - runtime > 22 || cycle >= maxLoopCycles - 1){
+                    if (cycle >= maxLoopCycles - 1){
                         follower.followPath(intakeGate_shootLeave, true);
                         pathState = PathState.DRIVE_SHOOT_LEAVE;
                         shooter.setFlywheelVelocity(2300);
@@ -336,7 +336,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                         follower.followPath(shootGate_leave, true);
                         pathState = PathState.WAIT_SHOOT_LEAVE;
                     } else {
-                        follower.followPath(shootGate_intakeGate, false);
+                        follower.followPath(shootGate_intakeGate, true);
                         pathState = PathState.INTAKE_GATE;
                         toGateBool = true;
                     }

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

@@ -132,7 +132,9 @@ public class TeleopV4 extends LinearOpMode {
                 gamepad1.rumble(100);
             }
 
+            if (!isStopRequested()){
                 follower.update();
+            }
             Pose currentPose = follower.getPose();
             teleStart = currentPose;
 
@@ -209,16 +211,11 @@ public class TeleopV4 extends LinearOpMode {
                     shooting = false;
                 }
 
-                if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed() && !shooting){
+                if (SpindexerTransferIntake.intakeFull && firstTickFull){
-                    intakeFull = true;
-                } else {
-                    intakeFull = false;
-                    firstTickFull = true;
-                }
-
-                if (intakeFull && firstTickFull){
                     gamepad1.rumble(250);
                     firstTickFull = false;
+                } else if (!SpindexerTransferIntake.intakeFull){
+                    firstTickFull = true;
                 }
 
                 if (gamepad1.right_trigger > 0.5 &&

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

@@ -53,7 +53,7 @@ public class SpindexerTransferIntake {
     }
 
     int[] shootOrder = {0, 1, 2};
-    public static final double sensorDistanceThreshold = 6.0;//4.85//5.35
+    public static final double sensorDistanceThreshold = 5.0;//6.0;//4.85//5.35
     final long pulseTime = 70; // ms
 
     private DesiredPattern desiredPattern = DesiredPattern.GPP;
@@ -289,6 +289,7 @@ public class SpindexerTransferIntake {
     private int greenTicks = 0;
     private int ballTicks = 0;
     private int holdBallsTicker = 0;
+    public static boolean intakeFull = false;
     public void update() {
 
 //        TELE.addData("Sorted State", sortedIntakeStates);
@@ -318,7 +319,7 @@ public class SpindexerTransferIntake {
 
                     case INTAKE:
 
-                        robot.setIntakePower(0.8);
+                        robot.setIntakePower(1);
                         robot.setRapidFireBlockerPos(
                                 ServoPositions.rapidFireBlocker_Closed
                         );
@@ -330,11 +331,11 @@ public class SpindexerTransferIntake {
                                 ServoPositions.transferServo_out
                         );
 
-                        if (robot.insideBeam.isPressed()) {
+                        if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
                             holdBallsTicker++;
                         }
 
-                        if (holdBallsTicker > 15){
+                        if (holdBallsTicker > 5){
                             setRapidMode(RapidMode.TRANSFER_OFF);
                             holdBallsTicker = 0;
                         }
@@ -346,6 +347,7 @@ public class SpindexerTransferIntake {
                         if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
                             setRapidMode(RapidMode.BEFORE_PULSE_OUT);
                         }
+                        robot.setTransferServoPos(ServoPositions.transferServo_in);
                         robot.setTransferPower(-0.3);
 
                         break;
@@ -354,7 +356,7 @@ public class SpindexerTransferIntake {
 
                         robot.setIntakePower(1.0);
 
-                        if (stateTime() >= 700) {
+                        if (stateTime() >= 100) {
                             setRapidMode(RapidMode.PULSE_OUT);
                         }
 
@@ -362,7 +364,7 @@ public class SpindexerTransferIntake {
 
                     case PULSE_OUT:
 
-                        robot.setIntakePower(-0.1);
+//                        robot.setIntakePower(0);
 
                         if (stateTime() >= pulseTime) {
                             setRapidMode(RapidMode.PULSE_IN);
@@ -374,7 +376,7 @@ public class SpindexerTransferIntake {
 
                         robot.setIntakePower(1.0);
 
-                        if (stateTime() >= 400) {
+                        if (stateTime() >= 100) {
                             setRapidMode(RapidMode.HOLD_BALLS);
                         }
 
@@ -383,7 +385,9 @@ public class SpindexerTransferIntake {
                     case HOLD_BALLS:
 
                         if (robot.insideBeam.isPressed()
-                                && robot.outsideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
+                                && robot.outsideBeam.isPressed()) {
+
+                            intakeFull = true;
 
                             robot.setTransferPower(0);
                             robot.setIntakePower(0.1);
@@ -391,11 +395,12 @@ public class SpindexerTransferIntake {
 
                         } else {
                             setRapidMode(RapidMode.INTAKE);
+                            intakeFull = false;
                         }
                         break;
 
                     case OPEN_GATE:
-
+                        intakeFull = false;
                         if (stateTime() >= 100) {
                             setRapidMode(RapidMode.SHOOT);
                         }

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

@@ -79,7 +79,7 @@ public class VelocityCommander {
 
     private void distToTransferPow(double dist, double voltage){
         if (dist < 140){
-            transferPow = -0.8;
+            transferPow = -0.85;
         } else {
             transferPow = -0.7;
         }