ripark2/FTC-Code
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

a

Browse Source
This commit is contained in:
Anirudh Narayanan
2026-02-19 19:52:09 -06:00
parent 045ef98262
commit 3319e934c1
3 changed files with 308 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files

303
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java
View File

@@ -1,110 +1,285 @@
package org.firstinspires.ftc.teamcode.Auton; package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.teamcode.Auton;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware; import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
import org.firstinspires.ftc.teamcode.util.PersonalPID;
@Config @Config
@Autonomous(name = "Clean Red Depot") @Autonomous(name = "Blue Depot Cycle")
public class redDepot extends LinearOpMode { public class redDepot extends LinearOpMode {
Hware robot = new Hware(); Hware robot = new Hware();
public static double TICKS_PER_DEGREE = 9.738888 * 0.29969; public static double TICKS_PER_DEGREE = 9.738888 * 0.29969;
// --- Action Helpers --- //turret
public void setTurret(int degrees) { public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
robot.turret.setTargetPosition((int) (degrees * TICKS_PER_DEGREE)); public static double target = 0;
public static double ticksperdegree = 9.738888 * 0.29969;
public int minTicks = (int) (ticksperdegree * (-90));
public int maxTicks = (int) (ticksperdegree * (75));
PersonalPID pid;
Limelight3A limelight;
LLResult result;
boolean lock = true;
public void turretPower(double power) {
robot.turret.setPower(power);
}
public void turretPos(int position) {
turretPower(0);
robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE);
robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.turret.setPower(1.0); turretPower(1);
} }
public void startShooter(double velocity) { public void spintakeStart(){
robot.topFly.setVelocity(velocity); robot.intake.set(1);
robot.bottomFly.setVelocity(velocity);
robot.launchHood.setPosition(0.54);
} }
public void stopEverything() { public void spintakeEnd(){
// robot.intake.set(0); robot.intake.set(0);
// robot.topFly.setVelocity(0);
// robot.bottomFly.setVelocity(0);
// robot.activeTransfer.setPosition(0.8);
} }
@Override public void shoot(){
robot.topFly.setVelocity(1800);
robot.bottomFly.setVelocity(1800);
robot.launchHood.setPosition(0.55);
}
public void reset(){
robot.intake.set(0);
// robot.topFly.setVelocity(1200);
// robot.bottomFly.setVelocity(1200);
// robot.topFly.set(0);
// robot.bottomFly.set(0);
// robot.launchHood.setPosition(0.6);
robot.activeTransfer.setPosition(1);
}
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
//webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
robot.initialize(hardwareMap); robot.initialize(hardwareMap);
limelight = hardwareMap.get(Limelight3A.class, "limelight");
pid = new PersonalPID(p, i, d, f);
limelight.pipelineSwitch(1);
limelight.start();
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
pid = new PersonalPID(p, i, d, f);
Pose2d startPose = new Pose2d(54, -46, Math.toRadians(-52));
Pose2d startPose = new Pose2d(54, -52, Math.toRadians(205));
drive.setPoseEstimate(startPose); drive.setPoseEstimate(startPose);
// --- TRAJECTORY 1: Initial Score --- TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
TrajectorySequence traj1 = drive.trajectorySequenceBuilder(startPose) .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
.addDisplacementMarker(() -> { reset();
setTurret(-5); // turretPos(5);
startShooter(1660);
robot.intake.set(.8);
}) })
.strafeRight(35) .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
.addDisplacementMarker(() -> robot.activeTransfer.setPosition(1.0)) shoot();
})
.back(47)
.waitSeconds(0.5)
.UNSTABLE_addTemporalMarkerOffset(0.3, () -> {
robot.intake.set(1);
robot.activeTransfer.setPosition(0.85);
})
// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
// robot.launchHood.setPosition(0.5);
// })
.waitSeconds(1.5) .waitSeconds(1.5)
.addDisplacementMarker(this::stopEverything) .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
.build(); reset();
})
// --- TRAJECTORY 2: Intake 6-Ball --- //6ball
TrajectorySequence traj2 = drive.trajectorySequenceBuilder(traj1.end()) .lineToLinearHeading(new Pose2d(8,-28, Math.toRadians(-90)))
.lineToLinearHeading(new Pose2d(11, -28, Math.toRadians(265))) .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
.addDisplacementMarker(() -> robot.intake.set(1.0)) spintakeStart();
.forward(35) })
.addDisplacementMarker(() -> robot.intake.set(0)) .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
.build(); SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd();
shoot();
})
.lineToLinearHeading(new Pose2d(24,-14, Math.toRadians(-52)))
.UNSTABLE_addTemporalMarkerOffset(.5, () -> {
robot.intake.set(1);
robot.activeTransfer.setPosition(0.85);
})
// --- TRAJECTORY 3: Score 6-Ball --- // .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
TrajectorySequence traj3 = drive.trajectorySequenceBuilder(traj2.end()) // robot.launchHood.setPosition(0.5);
.lineToLinearHeading(new Pose2d(22, -20, Math.toRadians(205))) // })
.addDisplacementMarker(0.5, () -> startShooter(1660)) // Trigger half-way through
.addDisplacementMarker(() -> robot.activeTransfer.setPosition(1.0))
.waitSeconds(1.5) .waitSeconds(1.5)
.addDisplacementMarker(this::stopEverything)
.build();
// --- TRAJECTORY 4: Intake 9-Ball --- //9ball
TrajectorySequence traj4 = drive.trajectorySequenceBuilder(traj3.end()) .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
.lineToLinearHeading(new Pose2d(-14, -27, Math.toRadians(265))) reset();
.addDisplacementMarker(() -> robot.intake.set(1.0)) })
.forward(35) .lineToLinearHeading(new Pose2d(-19,-27, Math.toRadians(-90)))
.addDisplacementMarker(() -> robot.intake.set(0)) .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
.build(); spintakeStart();
})
// --- TRAJECTORY 5: Score 9-Ball & Park --- .forward(18, SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
TrajectorySequence traj5 = drive.trajectorySequenceBuilder(traj4.end()) SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
.lineToLinearHeading(new Pose2d(22, -20, Math.toRadians(205))) .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
.addDisplacementMarker(() -> { spintakeEnd();
startShooter(1660); })
robot.activeTransfer.setPosition(1.0); .lineToLinearHeading(new Pose2d(-12,-54, Math.toRadians(-90)))
.lineToLinearHeading(new Pose2d(26,-14, Math.toRadians(-52)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.UNSTABLE_addTemporalMarkerOffset(.5, () -> {
robot.intake.set(1);
robot.activeTransfer.setPosition(0.85);
}) })
.waitSeconds(1.5) .waitSeconds(1.5)
.addDisplacementMarker(this::stopEverything)
.lineToLinearHeading(new Pose2d(11, -28, Math.toRadians(-90))) //12ball
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.lineToLinearHeading(new Pose2d(-40,-22, Math.toRadians(-90)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart();
})
.forward(33, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd();
})
.lineToLinearHeading(new Pose2d(35,-14, Math.toRadians(-52)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
shoot();
})
.UNSTABLE_addTemporalMarkerOffset(.5, () -> {
robot.intake.set(0.8);
robot.activeTransfer.setPosition(0.85);
})
.waitSeconds(1.5)
// .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45)))
// .UNSTABLE_addTemporalMarkerOffset(0.5, () -> {
// turretPos(84);
// })
.waitSeconds(1)
.build(); .build();
// TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose)
// .strafeLeft(25)
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
// shoot();
// })
// .waitSeconds(1)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// reset();
// })
// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeStart();
// })
// .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeEnd();
// })
// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// shoot();
// })
// .waitSeconds(1)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// reset();
// })
// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .forward(10)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeStart();
// })
// .strafeLeft(10)
// .forward(3)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeEnd();
// })
// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// shoot();
// })
// .waitSeconds(1)
// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeStart();
// })
// .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeEnd();
// })
// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// shoot();
// })
// .waitSeconds(1)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// reset();
// })
// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .build();
waitForStart(); waitForStart();
if (opModeIsActive() && !isStopRequested()) {
// while (opModeIsActive()) {
drive.followTrajectorySequenceAsync(threeBall);
PoseStorage.currentPose = drive.getPoseEstimate();
if (isStopRequested()) return; while(opModeIsActive()){
robot.leftPTO.setPosition(0.2);
robot.rightPTO.setPosition(0.14);
drive.update();
result = limelight.getLatestResult();
double vision = result.getTx() + 3;
// Execute sequentially double target = ticksperdegree * vision;
drive.followTrajectorySequence(traj1); double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target);
drive.followTrajectorySequence(traj2); pid.setPIDF(p, i, d, f);
drive.followTrajectorySequence(traj3);
drive.followTrajectorySequence(traj4); if((robot.turret.getCurrentPosition() > maxTicks) && (targetTicks > 0)){
drive.followTrajectorySequence(traj5); targetTicks = targetTicks;
} else if((robot.turret.getCurrentPosition() < maxTicks) && (targetTicks < 0)){
targetTicks = targetTicks;
} else if((robot.turret.getCurrentPosition() > maxTicks) || (robot.turret.getCurrentPosition() < minTicks) ){
targetTicks = 0;
}
PoseStorage.currentPose = drive.getPoseEstimate();
robot.turret.setPower(- targetTicks);
}
// }
}
} }
} }

29
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Blue.java
View File

@@ -18,7 +18,7 @@ import org.firstinspires.ftc.teamcode.util.PersonalPID;
@Config @Config
@TeleOp(name = "TeleOp BLUE - Final Optimized") @TeleOp(name = "TeleOp BLUE - Final Optimized")
public class DriverControlsV1 extends LinearOpMode { public class DriverControlsV1Blue extends LinearOpMode {
// --- DASHBOARD TUNING --- // --- DASHBOARD TUNING ---
public static double flywheelTuner = 1.0; public static double flywheelTuner = 1.0;
@@ -34,7 +34,7 @@ public class DriverControlsV1 extends LinearOpMode {
private double lastTuner = -1.0; private double lastTuner = -1.0;
private double lastPTOPosition = -1.0; private double lastPTOPosition = -1.0;
enum ShootState { IDLE, START_SHOOT, RESET } enum ShootState { IDLE, START_SHOOT, RESET, START_SHOOT_FAR }
//turret //turret
public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007; public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
@@ -44,6 +44,7 @@ public class DriverControlsV1 extends LinearOpMode {
public int minTicks = (int) (ticksperdegree * (-90)); public int minTicks = (int) (ticksperdegree * (-90));
public int maxTicks = (int) (ticksperdegree * (75)); public int maxTicks = (int) (ticksperdegree * (75));
public int offsetCam = 3;
PersonalPID pid; PersonalPID pid;
Limelight3A limelight; Limelight3A limelight;
LLResult result; LLResult result;
@@ -81,7 +82,7 @@ public class DriverControlsV1 extends LinearOpMode {
while (opModeIsActive()) { while (opModeIsActive()) {
result = limelight.getLatestResult(); result = limelight.getLatestResult();
double vision = result.getTx()-3; double vision = result.getTx()-offsetCam;
// 1. INPUTS & TURRET // 1. INPUTS & TURRET
// if (gamepad2.yWasPressed()) { // if (gamepad2.yWasPressed()) {
@@ -178,6 +179,12 @@ public class DriverControlsV1 extends LinearOpMode {
shootTimer.reset(); shootTimer.reset();
} }
if (gamepad2.dpadLeftWasPressed()) {
shooting = true;
shootStatus = ShootState.START_SHOOT_FAR;
shootTimer.reset();
}
if (gamepad2.yWasPressed()) { if (gamepad2.yWasPressed()) {
flywheelTuner+=.01; flywheelTuner+=.01;
} }
@@ -186,6 +193,12 @@ public class DriverControlsV1 extends LinearOpMode {
flywheelTuner-=.01; flywheelTuner-=.01;
} }
if (gamepad2.xWasPressed()) {
offsetCam = 0;
} else if (gamepad2.bWasPressed()) {
offsetCam = 3;
}
switch (shootStatus) { switch (shootStatus) {
case START_SHOOT: case START_SHOOT:
robot.activeTransfer.setPosition(0.85); robot.activeTransfer.setPosition(0.85);
@@ -195,6 +208,16 @@ public class DriverControlsV1 extends LinearOpMode {
shootStatus = ShootState.RESET; shootStatus = ShootState.RESET;
} }
break; break;
case START_SHOOT_FAR:
robot.activeTransfer.setPosition(0.85);
robot.intake.set(.6);
targetTicks = 0;
if (shootTimer.milliseconds() > 2000) {
shootStatus = ShootState.RESET;
}
break;
case RESET: case RESET:
if (shootTimer.milliseconds() > 2500) { if (shootTimer.milliseconds() > 2500) {
shootStatus = ShootState.IDLE; shootStatus = ShootState.IDLE;

54
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java
View File

@@ -17,7 +17,7 @@ import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
import org.firstinspires.ftc.teamcode.util.PersonalPID; import org.firstinspires.ftc.teamcode.util.PersonalPID;
@Config @Config
@TeleOp(name = "TeleOp RED - Final Optimized") @TeleOp(name = "TeleOp BLUE - Final Optimized")
public class DriverControlsV1Red extends LinearOpMode { public class DriverControlsV1Red extends LinearOpMode {
// --- DASHBOARD TUNING --- // --- DASHBOARD TUNING ---
@@ -27,14 +27,14 @@ public class DriverControlsV1Red extends LinearOpMode {
public static double HOOD_MIN = 0.3; public static double HOOD_MIN = 0.3;
public static double HOOD_MAX = 1.0; public static double HOOD_MAX = 1.0;
public static double HOOD_CORRECTION_SENSITIVITY = 0.04; public static double HOOD_CORRECTION_SENSITIVITY = 0.01;
// ------------------------- // -------------------------
// State tracking to prevent redundant hardware writes (The Fix) // State tracking to prevent redundant hardware writes (The Fix)
private double lastTuner = -1.0; private double lastTuner = -1.0;
private double lastPTOPosition = -1.0; private double lastPTOPosition = -1.0;
enum ShootState { IDLE, START_SHOOT, RESET } enum ShootState { IDLE, START_SHOOT, RESET, START_SHOOT_FAR }
//turret //turret
public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007; public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
@@ -44,6 +44,7 @@ public class DriverControlsV1Red extends LinearOpMode {
public int minTicks = (int) (ticksperdegree * (-90)); public int minTicks = (int) (ticksperdegree * (-90));
public int maxTicks = (int) (ticksperdegree * (75)); public int maxTicks = (int) (ticksperdegree * (75));
public int offsetCam = 3;
PersonalPID pid; PersonalPID pid;
Limelight3A limelight; Limelight3A limelight;
LLResult result; LLResult result;
@@ -58,7 +59,7 @@ public class DriverControlsV1Red extends LinearOpMode {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
if (PoseStorage.currentPose != null) drive.setPoseEstimate(PoseStorage.currentPose); if (PoseStorage.currentPose != null) drive.setPoseEstimate(PoseStorage.currentPose);
ShooterSubsystemRed shooterMap = new ShooterSubsystemRed(); ShooterSubsystem shooterMap = new ShooterSubsystem();
ShootState shootStatus = ShootState.IDLE; ShootState shootStatus = ShootState.IDLE;
ElapsedTime shootTimer = new ElapsedTime(); ElapsedTime shootTimer = new ElapsedTime();
@@ -81,7 +82,7 @@ public class DriverControlsV1Red extends LinearOpMode {
while (opModeIsActive()) { while (opModeIsActive()) {
result = limelight.getLatestResult(); result = limelight.getLatestResult();
double vision = result.getTx(); double vision = result.getTx()+offsetCam;
// 1. INPUTS & TURRET // 1. INPUTS & TURRET
// if (gamepad2.yWasPressed()) { // if (gamepad2.yWasPressed()) {
@@ -104,7 +105,7 @@ public class DriverControlsV1Red extends LinearOpMode {
// 2. PTO OPTIMIZATION (Only write if value is new) // 2. PTO OPTIMIZATION (Only write if value is new)
double targetPTO = 0.2; double targetPTO = 0.15;
if (targetPTO != lastPTOPosition) { if (targetPTO != lastPTOPosition) {
robot.leftPTO.setPosition(targetPTO); robot.leftPTO.setPosition(targetPTO);
robot.rightPTO.setPosition(targetPTO); robot.rightPTO.setPosition(targetPTO);
@@ -156,7 +157,7 @@ public class DriverControlsV1Red extends LinearOpMode {
double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0; double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0;
double velocityError = targetVelocity - currentVel; double velocityError = targetVelocity - currentVel;
double hoodAdjustment = (velocityError / 80.0) * HOOD_CORRECTION_SENSITIVITY; double hoodAdjustment = (velocityError / 50.0) * HOOD_CORRECTION_SENSITIVITY;
double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX); double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX);
if (gamepad2.dpad_up) adjust = true; if (gamepad2.dpad_up) adjust = true;
@@ -167,8 +168,8 @@ public class DriverControlsV1Red extends LinearOpMode {
robot.topFly.setVelocity(targetVelocity); robot.topFly.setVelocity(targetVelocity);
robot.launchHood.setPosition(correctedHood); robot.launchHood.setPosition(correctedHood);
} else { } else {
robot.bottomFly.setVelocity(0); robot.bottomFly.setVelocity(1700);
robot.topFly.setVelocity(0); robot.topFly.setVelocity(1700);
} }
// 7. SHOOTING STATE MACHINE // 7. SHOOTING STATE MACHINE
@@ -178,15 +179,45 @@ public class DriverControlsV1Red extends LinearOpMode {
shootTimer.reset(); shootTimer.reset();
} }
if (gamepad2.dpadLeftWasPressed()) {
shooting = true;
shootStatus = ShootState.START_SHOOT_FAR;
shootTimer.reset();
}
if (gamepad2.yWasPressed()) {
flywheelTuner+=.01;
}
if (gamepad2.aWasPressed()) {
flywheelTuner-=.01;
}
if (gamepad2.xWasPressed()) {
offsetCam = 0;
} else if (gamepad2.bWasPressed()) {
offsetCam = 3;
}
switch (shootStatus) { switch (shootStatus) {
case START_SHOOT: case START_SHOOT:
robot.activeTransfer.setPosition(0.81); robot.activeTransfer.setPosition(0.85);
robot.intake.set(1.0); robot.intake.set(1);
targetTicks = 0; targetTicks = 0;
if (shootTimer.milliseconds() > 2000) { if (shootTimer.milliseconds() > 2000) {
shootStatus = ShootState.RESET; shootStatus = ShootState.RESET;
} }
break; break;
case START_SHOOT_FAR:
robot.activeTransfer.setPosition(0.85);
robot.intake.set(.6);
targetTicks = 0;
if (shootTimer.milliseconds() > 2000) {
shootStatus = ShootState.RESET;
}
break;
case RESET: case RESET:
if (shootTimer.milliseconds() > 2500) { if (shootTimer.milliseconds() > 2500) {
shootStatus = ShootState.IDLE; shootStatus = ShootState.IDLE;
@@ -219,6 +250,7 @@ public class DriverControlsV1Red extends LinearOpMode {
telemetry.addData("Status", "Running"); telemetry.addData("Status", "Running");
telemetry.addData("Vel Error", (int)velocityError); telemetry.addData("Vel Error", (int)velocityError);
telemetry.addData("Hood", "%.2f", correctedHood); telemetry.addData("Hood", "%.2f", correctedHood);
telemetry.addData("TunerMult", flywheelTuner);
telemetry.update(); telemetry.update();
} }
} }