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 2e604b3..29dadb2 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 @@ -31,6 +31,8 @@ public class TeleopV4 extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { + Robot.resetInstance(); + robot = Robot.getInstance(hardwareMap); TELE = new MultipleTelemetry( @@ -66,12 +68,14 @@ public class TeleopV4 extends LinearOpMode { gamepad1.left_stick_x ); + follower.update(); + shooter.update(); spindexerTransferIntake.update(); SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState(); - if (gamepad1.xWasPressed() && + if (gamepad1.leftBumperWasPressed() && (state == SpindexerTransferIntake.RapidMode.INTAKE || state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF || state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT || @@ -82,7 +86,7 @@ public class TeleopV4 extends LinearOpMode { spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); } - if (gamepad1.aWasPressed() && + if (gamepad1.right_trigger > 0.5 && (state == SpindexerTransferIntake.RapidMode.INTAKE || state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) { @@ -90,8 +94,7 @@ public class TeleopV4 extends LinearOpMode { SpindexerTransferIntake.RapidMode.HOLD_BALLS ); } - - if (gamepad1.yWasPressed() + if (gamepad1.rightBumperWasPressed() && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) { spindexerTransferIntake.setRapidMode( @@ -99,7 +102,6 @@ public class TeleopV4 extends LinearOpMode { ); } - TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java index f6ee1ad..170c7f2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java @@ -130,7 +130,7 @@ public class Hardware_Tester extends LinearOpMode { // TELE.addData("Beam Break 3?", robot.beam3.isPressed()); NormalizedRGBA revColor = robot.revSensor.getNormalizedColors(); - TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.MM)); + TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.CM)); TELE.addData("REV Green", revColor.green / (revColor.red + revColor.blue + revColor.green)); TELE.addData("Voltage Sensor", robot.voltage.getVoltage()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java index 3e4c735..5f893bd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -86,8 +86,8 @@ public class Shooter { break; case TRACK_GOAL: turr.trackGoal( - (follow.getPose().getX() - goalX), - (follow.getPose().getY() - goalY), + (goalX - follow.getPose().getX()), + (goalY - follow.getPose().getY()), follow.getHeading(), follow.getAngularVelocity(), follow.getVelocity().getXComponent(), @@ -97,8 +97,8 @@ public class Shooter { ); flywheelVelocity = commander.getVeloPredictive( - (follow.getPose().getX() - goalX), - (follow.getPose().getY() - goalY), + (goalX - follow.getPose().getX()), + (goalY - follow.getPose().getY()), follow.getVelocity().getXComponent(), follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), @@ -109,14 +109,14 @@ public class Shooter { break; case READ_OBELISK: turr.trackObelisk( - (follow.getPose().getX() - goalX), - (follow.getPose().getY() - goalY), + (goalX - follow.getPose().getX()), + (goalY - follow.getPose().getY()), follow.getHeading() ); flywheelVelocity = commander.getVeloPredictive( - (follow.getPose().getX() - goalX), - (follow.getPose().getY() - goalY), + (goalX - follow.getPose().getX()), + (goalY - follow.getPose().getY()), follow.getVelocity().getXComponent(), follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), @@ -129,8 +129,8 @@ public class Shooter { case MANUAL_TURRET_TRACK_FLY: turr.manual(turretPosition); flywheelVelocity = commander.getVeloPredictive( - (follow.getPose().getX() - goalX), - (follow.getPose().getY() - goalY), + (goalX - follow.getPose().getX()), + (goalY - follow.getPose().getY()), follow.getVelocity().getXComponent(), follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), @@ -142,8 +142,8 @@ public class Shooter { case MANUAL_FLYWHEEL_TRACK_TURR: turr.trackGoal( - (follow.getPose().getX() - goalX), - (follow.getPose().getY() - goalY), + (goalX - follow.getPose().getX()), + (goalY - follow.getPose().getY()), follow.getHeading(), follow.getAngularVelocity(), follow.getVelocity().getXComponent(), 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 0dc6cb4..5896484 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 @@ -13,8 +13,8 @@ public class SpindexerTransferIntake { this.robot = rob; } - private final double sensorDistanceThreshold = 4.0; - private final long pulseTime = 50; // ms + private final double sensorDistanceThreshold = 6.0; + private final long pulseTime = 100; // ms public enum SpindexerMode { RAPID, @@ -74,7 +74,7 @@ public class SpindexerTransferIntake { case INTAKE: robot.setIntakePower(1); - robot.setTransferPower(1); + robot.setTransferPower(-0.7); robot.setRapidFireBlockerPos( ServoPositions.rapidFireBlocker_Closed ); @@ -95,7 +95,7 @@ public class SpindexerTransferIntake { case TRANSFER_OFF: - robot.setTransferPower(0.3); + robot.setTransferPower(-0.7); if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) { setRapidMode(RapidMode.BEFORE_PULSE_OUT); @@ -145,9 +145,6 @@ public class SpindexerTransferIntake { robot.setIntakePower(1); } - - - break; case OPEN_GATE: 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 297b723..1c22106 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 @@ -12,7 +12,7 @@ public class VelocityCommander { } private double distToRPM (double dist){ - return Math.sqrt(dist*dist + goalH*goalH); + return Math.sqrt(dist*dist + goalH*goalH) * 20; //TODO: Add regression here using goalH }