nighttime sleepy

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

@@ -227,6 +227,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                 }
                 break;
             case OPENGATE:
+                robot.setIntakePower(0);
                 if (currentTime - timeStamp > openGateTime){
                     follower.followPath(openGate_shoot1, true);
                     pathState = PathState.DRIVE_SHOOT1;
@@ -275,7 +276,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                 break;
             case DRIVE_PICKUP3:
                 if (!follower.isBusy()){
-                    shooter.setFlywheelVelocity(2200);
+                    shooter.setFlywheelVelocity(2250);
                     robot.setHoodPos(0.75);
                     follower.followPath(drivePickup3_pickup3, intakePower, false);
                     pathState = PathState.PICKUP3;

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

@@ -8,6 +8,7 @@ import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
 import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
@@ -31,7 +32,7 @@ public class TeleopV4 extends LinearOpMode {
     ParkTilter parkTilter;
     MeasuringLoopTimes loopTimes;
 
-    public static Pose relocalizePose = new Pose(56, 11, 0);
+    public static Pose relocalizePose = new Pose(54.7, 11.4, 0);
     public static Pose teleStart = new Pose(0,0,0);
 
     private boolean firstTickFull = true;
@@ -122,9 +123,9 @@ public class TeleopV4 extends LinearOpMode {
 
             if (gamepad1.crossWasPressed()){
                 if (Color.redAlliance){
-                    relocalizePose = new Pose(57.5, 5, 0);
+                    relocalizePose = new Pose(54.4, 12, 0);
                 } else {
-                    relocalizePose = new Pose(-57.5, 5, Math.toRadians(180));
+                    relocalizePose = new Pose(-54.4, 12, Math.toRadians(180));
                 }
                 follower.setPose(relocalizePose);
                 sleep(500);
@@ -216,7 +217,7 @@ public class TeleopV4 extends LinearOpMode {
                 }
 
                 if (intakeFull && firstTickFull){
-                    gamepad1.rumble(100);
+                    gamepad1.rumble(250);
                     firstTickFull = false;
                 }
 
@@ -254,9 +255,9 @@ public class TeleopV4 extends LinearOpMode {
             }
 
             if (gamepad2.dpadUpWasPressed()){
-                hoodOffset+=0.02;
-            } else if (gamepad2.dpadDownWasPressed()){
                 hoodOffset-=0.02;
+            } else if (gamepad2.dpadDownWasPressed()){
+                hoodOffset+=0.02;
             }
 
             if (gamepad2.dpadRightWasPressed()){
@@ -275,15 +276,25 @@ public class TeleopV4 extends LinearOpMode {
 //            TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin());
 //            TELE.addData("Loop Time Min", loopTimes.getMinLoopTimeOneMin());
 //
-            TELE.addData("Distance From Goal", commander.getDistance());
+//            TELE.addData("Distance From Goal", commander.getDistance());
 //            TELE.addData("Hood Position", commander.getHoodPredicted());
 //            TELE.addData("Transfer Power", robot.transfer.getPower());
-            TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
-            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
-            TELE.addData("Velocity 1", flywheel.getVelo1());
-            TELE.addData("Velocity 2", flywheel.getVelo2());
+//            TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
+//            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
+//            TELE.addData("Velocity 1", flywheel.getVelo1());
+//            TELE.addData("Velocity 2", flywheel.getVelo2());
             TELE.addData("Flywheel Offset", flywheelOffset);
             TELE.addData("Hood Offset", hoodOffset);
+
+//            TELE.addData("FR Drive", robot.frontRight.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("FL Drive", robot.frontRight.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("BL Drive", robot.backLeft.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("BR Drive", robot.backRight.getCurrent(CurrentUnit.AMPS));
+
+//            TELE.addData("Flywheel 1", robot.shooter1.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("Flywheel 2", robot.shooter2.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("Transfer", robot.transfer.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("Intake", robot.intake.getCurrent(CurrentUnit.AMPS));
 //
 //            TELE.addData("Current Position", currentPose);
 //

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

@@ -36,7 +36,7 @@ public class NewShooterTest extends LinearOpMode {
     private boolean shooting = false;
     public static int flywheelVelo = 0;
     public static double hoodPos = 0.5;
-    public static double transferPower = -1;
+    public static double transferPower = -0.8;
 
     @Override
     public void runOpMode() throws InterruptedException {

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

@@ -15,6 +15,7 @@ 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.robotcore.external.navigation.CurrentUnit;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
 
 public class Robot {
@@ -80,6 +81,11 @@ public class Robot {
     public Limelight3A limelight;
     public Servo light;
 
+    // Current Limits
+    public static double intakeCurrentLimit = 6.0;
+    public static double transferCurrentLimit = 5.0;
+    public static double drivetrainCurrentLimit = 7.0;
+
     public Robot(HardwareMap hardwareMap) {
 
         //Define components w/ hardware map
@@ -120,7 +126,6 @@ public class Robot {
 
         transfer = hardwareMap.get(DcMotorEx.class, "transfer");
 
-
         transferServo = hardwareMap.get(Servo.class, "transferServo");
 
         transfer.setDirection(DcMotorSimple.Direction.REVERSE);
@@ -138,6 +143,13 @@ public class Robot {
 
         revSensor = hardwareMap.get(RevColorSensorV3.class, "rev");
 
+        frontLeft.setCurrentAlert(drivetrainCurrentLimit, CurrentUnit.AMPS);
+        frontRight.setCurrentAlert(drivetrainCurrentLimit, CurrentUnit.AMPS);
+        backLeft.setCurrentAlert(drivetrainCurrentLimit, CurrentUnit.AMPS);
+        backRight.setCurrentAlert(drivetrainCurrentLimit, CurrentUnit.AMPS);
+        intake.setCurrentAlert(intakeCurrentLimit, CurrentUnit.AMPS);
+        transfer.setCurrentAlert(transferCurrentLimit, CurrentUnit.AMPS);
+
         // Below is disregarded
 
 //        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
@@ -174,6 +186,12 @@ public class Robot {
 
     private double prevFrontLeftPower = -10.501;
     public void setFrontLeftPower(double pow){
+        if (frontLeft.isOverCurrent()){
+            double current = frontLeft.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * drivetrainCurrentLimit / current;
+            }
+        }
         pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevFrontLeftPower){
             frontLeft.setPower(pow);
@@ -183,6 +201,12 @@ public class Robot {
 
     private double prevFrontRightPower = -10.501;
     public void setFrontRightPower(double pow){
+        if (frontRight.isOverCurrent()){
+            double current = frontRight.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * drivetrainCurrentLimit / current;
+            }
+        }
         pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevFrontRightPower){
             frontRight.setPower(pow);
@@ -192,6 +216,12 @@ public class Robot {
 
     private double prevBackLeftPower = -10.501;
     public void setBackLeftPower(double pow){
+        if (backLeft.isOverCurrent()){
+            double current = backLeft.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * drivetrainCurrentLimit / current;
+            }
+        }
         pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevBackLeftPower){
             backLeft.setPower(pow);
@@ -201,6 +231,12 @@ public class Robot {
 
     private double prevBackRightPower = -10.501;
     public void setBackRightPower(double pow){
+        if (backRight.isOverCurrent()){
+            double current = backRight.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * drivetrainCurrentLimit / current;
+            }
+        }
         pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevBackRightPower){
             backRight.setPower(pow);
@@ -210,6 +246,12 @@ public class Robot {
 
     private double prevIntakePower = -10.501;
     public void setIntakePower(double pow){
+        if (intake.isOverCurrent()){
+            double current = intake.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * intakeCurrentLimit / current;
+            }
+        }
         pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevIntakePower){
             intake.setPower(pow);
@@ -220,6 +262,12 @@ public class Robot {
     private double prevTransferPower = -10.501;
     public void setTransferPower(double pow){
         pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
+//        if (transfer.isOverCurrent()){
+//            double current = transfer.getCurrent(CurrentUnit.AMPS);
+//            if (current != 0) {
+//                pow = pow * transferCurrentLimit / current;
+//            }
+//        }
         if (pow != prevTransferPower){
             transfer.setPower(pow);
         }

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

@@ -138,8 +138,8 @@ public class Shooter {
                 );
 
                 flywheelVelocity = commander.getPredictedRPM();
-
-                robot.setHoodPos(commander.getHoodPredicted() + TeleopV4.hoodOffset);
+                double hood1 = Math.max(0.35, Math.min(0.88, commander.getHoodPredicted() + TeleopV4.hoodOffset));
+                robot.setHoodPos(hood1);
                 fly.manageFlywheel(flywheelVelocity + TeleopV4.flywheelOffset);
                 fly.setF(voltage);
                 break;
@@ -181,7 +181,8 @@ public class Shooter {
                 );
 
                 flywheelVelocity = commander.getPredictedRPM();
-                robot.setHoodPos(commander.getHoodPredicted() + TeleopV4.hoodOffset);
+                double hood2 = Math.max(0.35, Math.min(0.88, commander.getHoodPredicted() + TeleopV4.hoodOffset));
+                robot.setHoodPos(hood2);
                 fly.manageFlywheel(flywheelVelocity + TeleopV4.flywheelOffset);
                 break;
 

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 = 5.35;
+    public static final double sensorDistanceThreshold = 6.0;//4.85//5.35
     final long pulseTime = 70; // ms
 
     private DesiredPattern desiredPattern = DesiredPattern.GPP;
@@ -205,7 +205,7 @@ public class SpindexerTransferIntake {
     private RapidMode rapidMode = RapidMode.INTAKE;
     private SortedIntakeStates sortedIntakeStates = SortedIntakeStates.IDLE;
     private BallStates[] ballColors = {BallStates.UNKNOWN, BallStates.UNKNOWN, BallStates.UNKNOWN};
-    final double greenThresh = 0.39;
+    final double greenThresh = 0.41;
     final double spinMovementTime = 250;
 
     /**
@@ -318,24 +318,23 @@ public class SpindexerTransferIntake {
 
                     case INTAKE:
 
-                        robot.setIntakePower(1);
+                        robot.setIntakePower(0.8);
                         robot.setRapidFireBlockerPos(
                                 ServoPositions.rapidFireBlocker_Closed
                         );
                         robot.setSpinPos(
                                 ServoPositions.spindexer_A2
                         );
-                        robot.setTransferPower(-1);
+                        robot.setTransferPower(-0.8);
                         robot.setTransferServoPos(
                                 ServoPositions.transferServo_out
                         );
 
-                        if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
-
+                        if (robot.insideBeam.isPressed()) {
                             holdBallsTicker++;
                         }
 
-                        if (holdBallsTicker > 20){
+                        if (holdBallsTicker > 15){
                             setRapidMode(RapidMode.TRANSFER_OFF);
                             holdBallsTicker = 0;
                         }

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

@@ -17,8 +17,8 @@ public class Turret {
 
     private final double servoTicksPer180 = 0.58;
     public static double neutralPosition = 0.51;
-    private final double turretMin = 0.08;
-    private final double turretMax = 0.91;
+    private final double turretMin = 0.13;
+    private final double turretMax = 0.87;
     public static boolean limelightUsed = true;
     public static double B_PID_P = 0.0001, B_PID_I = 0.0, B_PID_D = 0.000005;
     LLResult result;

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

@@ -79,9 +79,9 @@ public class VelocityCommander {
 
     private void distToTransferPow(double dist, double voltage){
         if (dist < 140){
-            transferPow = -1;
+            transferPow = -0.8;
         } else {
-            transferPow = -0.6;
+            transferPow = -0.7;
         }
     }
     public double getTransferPow(){return transferPow;}