diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java index 519366d..df8936b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java @@ -170,16 +170,16 @@ public class Blue extends LinearOpMode { while(opModeInInit()) { if (gamepad2.dpadUpWasPressed()){ - hoodDefault -= 0.02; + hoodDefault -= 0.01; } if (gamepad2.dpadDownWasPressed()){ - hoodDefault += 0.02; + hoodDefault += 0.01; } robot.hood.setPosition(hoodDefault); - shooter.setTurretPosition(0.3); + shooter.setTurretPosition(turret_blue); aprilTag.initTelemetry(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java index f20214c..f2a0c96 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java @@ -99,45 +99,45 @@ public class Red extends LinearOpMode { - TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0)) + TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(x1, y1), h1 ); - TrajectoryActionBuilder traj2 = drive.actionBuilder(new Pose2d(x1, y1, h1)) + TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1)) .turnTo(Math.toRadians(135)) .strafeToLinearHeading(new Vector2d(x2, y2), h2 ); - TrajectoryActionBuilder traj3 = drive.actionBuilder(new Pose2d(x2, y2, h2)) + TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(x2, y2, h2)) .strafeToLinearHeading(new Vector2d(x1, y1), h1 ); - TrajectoryActionBuilder traj4 = drive.actionBuilder(new Pose2d(x1, y1, h1)) + TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(x1, y1, h1)) .strafeToLinearHeading(new Vector2d(x2_b, y2_b), h2_b ) .strafeToLinearHeading(new Vector2d(x3, y3), h3 ); - TrajectoryActionBuilder traj5 = drive.actionBuilder(new Pose2d(x3, y3, h3)) + TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(x3, y3, h3)) .strafeToLinearHeading(new Vector2d(x1, y1), h1 ); - TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1)) + TrajectoryActionBuilder park = drive.actionBuilder(new Pose2d(x1, y1, h1)) .strafeToLinearHeading(new Vector2d(x1, y1+30), h1 ); while(opModeInInit()) { if (gamepad2.dpadUpWasPressed()){ - hoodDefault -= 0.02; + hoodDefault -= 0.01; } if (gamepad2.dpadDownWasPressed()){ - hoodDefault += 0.02; + hoodDefault += 0.01; } robot.hood.setPosition(hoodDefault); - shooter.setTurretPosition(0.33); + shooter.setTurretPosition(turret_red); aprilTag.initTelemetry(); @@ -165,7 +165,7 @@ public class Red extends LinearOpMode { Actions.runBlocking( new ParallelAction( - traj1.build() + shoot0.build() ) ); @@ -245,12 +245,12 @@ public class Red extends LinearOpMode { robot.intake.setPower(1); Actions.runBlocking( - traj2.build() + pickup1.build() ); Actions.runBlocking( - traj3.build() + shoot1.build() ); shooter.setManualPower(1); @@ -329,12 +329,12 @@ public class Red extends LinearOpMode { robot.intake.setPower(1); Actions.runBlocking( - traj4.build() + pickup2.build() ); Actions.runBlocking( - traj5.build() + shoot2.build() ); shooter.setManualPower(1); @@ -411,7 +411,7 @@ public class Red extends LinearOpMode { spindexer.outtake3(); Actions.runBlocking( - traj6.build() + park.build() ); drive.updatePoseEstimate(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index beb7514..ff30df7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -16,8 +16,12 @@ public class ServoPositions { public static double transferServo_out = 0.13; public static double transferServo_in = 0.4; + public static double transferServoPos = 0.5; public static double hoodDefault = 0.35; + public static double turret_red = 0.33; + public static double turret_blue = 0.3; + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java index 304c371..ba04fd9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.subsystems; +import static java.lang.Runtime.getRuntime; + import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.controller.PIDController; @@ -45,6 +47,7 @@ public class Shooter implements Subsystem { private double p = 0.0003, i = 0, d = 0.00001; private PIDController controller; + private double pow = 0.0; @@ -100,8 +103,9 @@ public class Shooter implements Subsystem { telemetry.addData("hoodPos", gethoodPosition()); telemetry.addData("turretPos", getTurretPosition()); telemetry.addData("PID Coefficients", "P: %.6f, I: %.6f, D: %.6f", p, i, d); - telemetry.addData("Current Fly 1", fly1.getCurrent(CurrentUnit.AMPS)); - telemetry.addData("Current Fly 2", fly2.getCurrent(CurrentUnit.AMPS)); + telemetry.addData("Power Fly 1", fly1.getPower()); + telemetry.addData("Pow Fly 2", fly2.getPower()); + telemetry.addData("Pow", pow); } @@ -118,7 +122,7 @@ public class Shooter implements Subsystem { public void setTurretPosition(double pos) {turretPos = pos;} public double getVelocity() { - return ((double) ((fly1.getVelocity(AngleUnit.DEGREES) + fly2.getVelocity(AngleUnit.DEGREES)) /2)); + return ((double) ((fly1.getVelocity(AngleUnit.DEGREES)))); } public void setVelocity(double vel){velocity = vel;} @@ -292,13 +296,23 @@ public class Shooter implements Subsystem { } else if (Objects.equals(shooterMode, "VEL")){ - fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + fly1.setPower(pow); + fly2.setPower(pow); - fly1.setVelocity(velocity); - fly2.setPower(fly1.getPower()); + if (velocity == 0.0){ + fly1.setPower(0); + fly2.setPower(0); + } else if (-velocity < -Math.abs(fly1.getVelocity())){ + while (-velocity < -Math.abs(fly1.getVelocity())){ + pow += 0.001; + fly1.setPower(pow); + fly2.setPower(pow); + } + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java index 89e3d07..08bb18a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java @@ -1,7 +1,8 @@ package org.firstinspires.ftc.teamcode.teleop; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodDefault; +import static org.firstinspires.ftc.teamcode.constants.Poses.*; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -215,7 +216,6 @@ public class TeleopV1 extends LinearOpMode { - waitForStart(); if (isStopRequested()) return; @@ -233,6 +233,7 @@ public class TeleopV1 extends LinearOpMode { TELE.addData("off", offset); + robot.transferServo.setPosition(transferServoPos); robot.hood.setPosition(pos);