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..a9b171d 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,40 +99,40 @@ 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 intake1 = 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 intake2 = 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); @@ -165,7 +165,7 @@ public class Red extends LinearOpMode { Actions.runBlocking( new ParallelAction( - traj1.build() + shoot0.build() ) ); @@ -185,7 +185,7 @@ public class Red extends LinearOpMode { - shooter.moveTurret(0.31); + shooter.moveTurret(turretDefault); double time = getRuntime()-stamp; @@ -245,12 +245,12 @@ public class Red extends LinearOpMode { robot.intake.setPower(1); Actions.runBlocking( - traj2.build() + intake1.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() + intake2.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/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java index 6cc3514..1f3cedb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java @@ -15,12 +15,12 @@ public class Poses { public static double x1 = 50, y1 = 0, h1 = 0; - public static double x2 = 31, y2 = 32, h2 = Math.toRadians(135); + public static double x2 = 31, y2 = 32, h2 = Math.toRadians(140); - public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(135); + public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(140); - public static double x3 = 34, y3 = 58, h3 = Math.toRadians(135); + public static double x3 = 34, y3 = 58, h3 = Math.toRadians(140); public static Pose2d teleStart = new Pose2d(x1,-10,0); 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..3fc763b 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 @@ -19,5 +19,11 @@ public class ServoPositions { public static double hoodDefault = 0.35; + public static double turretDefault = 0.3; + + public static double turretBlueOffset = -0.01; + + public static double turretRedOffset = 0.01; + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Rejecter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Rejecter.java deleted file mode 100644 index 450d18c..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Rejecter.java +++ /dev/null @@ -1,27 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - - -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.hardware.Servo; - -import org.firstinspires.ftc.teamcode.utils.Robot; - -public class Rejecter implements Subsystem{ - - private final Servo servo; - - public double rpos = 0.5; - - public Rejecter(Robot robot){ - this.servo = robot.rejecter; - } - - public void rejecterPos(double pos){ - this.rpos = pos; - } - - @Override - public void update() { - this.servo.setPosition(rpos); - } -}