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 f030251..bb07e40 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 @@ -53,7 +53,7 @@ public class TeleopV4 extends LinearOpMode { parkTilter = new ParkTilter(robot); - shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel); + shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander); shooter.setState(Shooter.ShooterState.TRACK_GOAL); spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander); spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); @@ -64,9 +64,7 @@ public class TeleopV4 extends LinearOpMode { if (isStopRequested()) return; while (opModeIsActive()) { - //Drivetrain - drivetrain.drive( -gamepad1.right_stick_y, gamepad1.right_stick_x, @@ -113,6 +111,11 @@ public class TeleopV4 extends LinearOpMode { parkTilter.unpark(); } + TELE.addData("Distance From Goal", commander.getDistance()); + TELE.addData("Hood Position", commander.getHoodPredicted()); + TELE.addData("Transfer Power", robot.transfer.getPower()); + TELE.addData("Velocity RPM", commander.getPredictedRPM()); + TELE.update(); } 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 a9e6519..ec91447 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 @@ -14,6 +14,7 @@ import org.firstinspires.ftc.teamcode.utilsv2.Flywheel; import org.firstinspires.ftc.teamcode.utilsv2.Robot; import org.firstinspires.ftc.teamcode.utilsv2.Shooter; import org.firstinspires.ftc.teamcode.utilsv2.Turret; +import org.firstinspires.ftc.teamcode.utilsv2.VelocityCommander; @Config @TeleOp @@ -25,6 +26,7 @@ public class NewShooterTest extends LinearOpMode { Shooter shooter; MultipleTelemetry TELE; Follower follower; + VelocityCommander commander; public static boolean intake = true; @@ -61,6 +63,7 @@ public class NewShooterTest extends LinearOpMode { flywheel = new Flywheel(robot); turret = new Turret(robot); + commander = new VelocityCommander(); shooter = new Shooter( robot, @@ -68,7 +71,8 @@ public class NewShooterTest extends LinearOpMode { follower, true, turret, - flywheel + flywheel, + commander ); shooter.setState(Shooter.ShooterState.MANUAL); 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 6849d11..9f76ae4 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 @@ -21,12 +21,12 @@ public class Shooter { Follower follow; - public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance, Turret turret, Flywheel flywheel) { + public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance, Turret turret, Flywheel flywheel, VelocityCommander com) { this.robot = rob; this.fly = flywheel; this.turr = turret; this.follow = follower; - this.commander = new VelocityCommander(); + this.commander = com; setRedAlliance(redAlliance); 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 ed1e318..8f5109d 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 @@ -66,10 +66,6 @@ public class SpindexerTransferIntake { public void update() { - if (mode == SpindexerMode.RAPID && rapidMode == RapidMode.INTAKE){ - robot.setTransferPower(-0.7); - } - switch (mode) { case RAPID: @@ -89,6 +85,7 @@ public class SpindexerTransferIntake { robot.setSpinPos( ServoPositions.spindexer_A2 ); + robot.setTransferPower(-0.7); robot.setTransferServoPos( ServoPositions.transferServo_out ); @@ -106,6 +103,7 @@ public class SpindexerTransferIntake { if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) { setRapidMode(RapidMode.BEFORE_PULSE_OUT); } + robot.setTransferPower(-0.3); break; @@ -163,6 +161,8 @@ public class SpindexerTransferIntake { setRapidMode(RapidMode.SHOOT); } + robot.setTransferPower(commander.getTransferPow()); + break; case SHOOT: @@ -173,6 +173,9 @@ public class SpindexerTransferIntake { if (stateTime() >= 400) { setRapidMode(RapidMode.INTAKE); } + + robot.setTransferPower(commander.getTransferPow()); + break; } break; 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 c2484c8..5d99897 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 @@ -8,6 +8,7 @@ public class VelocityCommander { private final double yAccK = 0; // TODO: Tune private double hoodPos = 0.88; private double transferPow = -1; + private double veloRPM = 2000; public VelocityCommander() {} @@ -46,7 +47,7 @@ public class VelocityCommander { } private final double hoodA = -4.3276177*Math.pow(10, -13); - private final double hoodB = 2.68062979*Math.pow(10, -11); + private final double hoodB = 2.68062979*Math.pow(10, -10); private final double hoodC = -7.12859632*Math.pow(10, -8); private final double hoodD = 0.0000106010785; private final double hoodE = -0.000960693973; @@ -73,6 +74,9 @@ public class VelocityCommander { hoodPos = Math.max(0.35, Math.min(0.88, hoodPos)); } } + public double getHoodPredicted(){ + return hoodPos; + } private void distToTransferPow(double dist){ if (dist < 118){ @@ -83,13 +87,8 @@ public class VelocityCommander { transferPow = -0.5; } } - public double getTransferPow(){return transferPow;} - public double getHoodPredicted(){ - return hoodPos; - } - // 27 public double getVeloStationary (double distance){ return distToRPM(distance); @@ -105,9 +104,12 @@ public class VelocityCommander { predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight*goalHeight); distToHood(predictedDist); - - return distToRPM(predictedDist); + distToTransferPow(predictedDist); + veloRPM = distToRPM(predictedDist); + return veloRPM; } + public double getPredictedRPM(){return veloRPM;} + public double getDistance(){return predictedDist;} }