nighttime sleepy

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

@@ -217,7 +217,7 @@ public class TeleopV4 extends LinearOpMode {
                 }
 
                 if (intakeFull && firstTickFull){
-                    gamepad1.rumble(100);
+                    gamepad1.rumble(250);
                     firstTickFull = false;
                 }
 
@@ -276,25 +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("Theoretical Velocity RPM", commander.getPredictedRPM());
-            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
+//            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
-            TELE.addData("Velocity 1", flywheel.getVelo1());
+//            TELE.addData("Velocity 1", flywheel.getVelo1());
-            TELE.addData("Velocity 2", flywheel.getVelo2());
+//            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("FR Drive", robot.frontRight.getCurrent(CurrentUnit.AMPS));
-            TELE.addData("FL 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("BL Drive", robot.backLeft.getCurrent(CurrentUnit.AMPS));
-            TELE.addData("BR Drive", robot.backRight.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("BR Drive", robot.backRight.getCurrent(CurrentUnit.AMPS));
 
-            TELE.addData("Flywheel 1", robot.shooter1.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("Flywheel 1", robot.shooter1.getCurrent(CurrentUnit.AMPS));
-            TELE.addData("Flywheel 2", robot.shooter2.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("Flywheel 2", robot.shooter2.getCurrent(CurrentUnit.AMPS));
-            TELE.addData("Transfer", robot.transfer.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("Transfer", robot.transfer.getCurrent(CurrentUnit.AMPS));
-            TELE.addData("Intake", robot.intake.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/SpindexerTransferIntake.java

@@ -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/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;}