diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index 41a9a5a..bb2fe84 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/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("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("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("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); // diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java index 7954ca5..1cfe2c5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java +++ b/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 { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java index 68eff26..86a6dc2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java +++ b/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); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java index b868ea9..42a3bba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java +++ b/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; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java index 4d7e748..7ef9287 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java +++ b/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;}