diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autos/AutoBottomBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autos/AutoBottomBlue.java new file mode 100644 index 000000000000..52454cac2650 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autos/AutoBottomBlue.java @@ -0,0 +1,256 @@ +package org.firstinspires.ftc.teamcode.Autos; +import static android.os.SystemClock.sleep; + +import com.pedropathing.util.Timer; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.TelemetryManager; +import com.bylazar.telemetry.PanelsTelemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.follower.Follower; +import com.pedropathing.paths.PathChain; +import com.pedropathing.geometry.Pose; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.Subsystems.ShooterSubsystem; + +@Autonomous +@Configurable // Panels +public class AutoBottomBlue extends OpMode { + + private DcMotor intake; + private Servo gate, flick, hold; // servos + private TelemetryManager panelsTelemetry; // Panels Telemetry instance + public Follower follower; // Pedro Pathing follower instance + private int pathState; // Current autonomous path state (state machine) + private Paths paths; // Paths defined in the Paths class + private ShooterSubsystem shooter; + private double lastTime = 0.0; + private boolean shooterActive = false; + private int launchState3 = 0; + private int launchState2 = 0; + private int launchState1 = 0; + private Timer pathTimer; + private Timer launchTimer; + private boolean isDone; + + + + @Override + public void init() { + panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry(); + + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(new Pose(55.087885985748215, 9.254156769596198, Math.toRadians(90))); + + paths = new Paths(follower); // Build paths + pathTimer = new Timer(); + launchTimer = new Timer(); + intake = hardwareMap.get(DcMotor.class, "intake"); + gate = hardwareMap.get(Servo.class, "gate"); + hold = hardwareMap.get(Servo.class, "hold"); + flick = hardwareMap.get(Servo.class,"flick"); + gate.setPosition(0.3); +// Shooter subsystem + shooter = new ShooterSubsystem(hardwareMap); + panelsTelemetry.debug("Status", "Initialized"); + panelsTelemetry.update(telemetry); + } + @Override + public void start() { + lastTime = getRuntime(); + shooter.getLimelight().start(); + shooter.getLimelight().pipelineSwitch(1); // 1 is for blue tracking + } + @Override + public void loop() { + follower.update(); // Update Pedro Pathing + pathState = autonomousPathUpdate(); // Update autonomous state machine + + // launcher update + if (shooterActive) { + double currentTime = getRuntime(); + double dt = currentTime - lastTime; + lastTime = currentTime; + + shooter.update(follower.getPose(), follower.getVelocity(), dt, telemetry); + } + + // Log values to Panels and Driver Station + panelsTelemetry.debug("Path State", pathState); + panelsTelemetry.debug("X", follower.getPose().getX()); + panelsTelemetry.debug("Y", follower.getPose().getY()); + panelsTelemetry.debug("Heading", follower.getPose().getHeading()); + + telemetry.update(); + panelsTelemetry.update(telemetry); + } + + private void launch3balls() { // we call this function every time you want to launch 3 balls + switch (launchState3) { + case 0: + gate.setPosition(0.55); //0.55 open 0.3 closed + launchTimer.resetTimer(); + launchState3++; + hold.setPosition(0.5); + break; + + case 1: + if (launchTimer.getElapsedTimeSeconds() > 1) { + intake.setPower(1); + launchTimer.resetTimer(); + launchState3++; + } + break; + case 2: + intake.setPower(1); + if (launchTimer.getElapsedTimeSeconds() > 1) { + flick.setPosition(0.6); //0 is up + sleep(300); + flick.setPosition(1); + launchTimer.resetTimer(); + isDone = true; + } + break; + } + } + + public static class Paths { + public PathChain Starttoshoot1; + public PathChain Shoot1topickup1; + public PathChain Pickup1tointake1; + public PathChain Intake1toshoot2; + public PathChain Shoot2topark; + + public Paths(Follower follower) { + Starttoshoot1 = follower.pathBuilder().addPath( + new BezierLine( + new Pose(55.088, 9.254), + + new Pose(61.017, 23.914) + ) + ).setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(90)) + + .build(); + + Shoot1topickup1 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(61.017, 23.914), + new Pose(30.785, 8.586), + new Pose(15.562, 8.216) + ) + ).setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(-180)) + + .build(); + + Pickup1tointake1 = follower.pathBuilder().addPath( + new BezierLine( + new Pose(15.562, 8.216), + + new Pose(11.135878430586969, 8.164224594339366) + ) + ).setTangentHeadingInterpolation() + + .build(); + + Intake1toshoot2 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(11.135878430586969, 8.164224594339366), + new Pose(33.806, 21.343), + new Pose(61.071, 23.919) + ) + ).setLinearHeadingInterpolation(Math.toRadians(-180), Math.toRadians(90)) + + .build(); + + Shoot2topark = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(61.071, 23.919), + new Pose(48.837, 13.333), + new Pose(35.340, 11.164) + ) + ).setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(-180)) + + .build(); + } + } + + /* You could check for + - Follower State: "if(!follower.isBusy()) {}" + - Time: "if(pathTimer.getElapsedTimeSeconds() > 1) {}" + - Robot Position: "if(follower.getPose().getX() > 36) {}" + */ + public int autonomousPathUpdate() { + switch (pathState) { + case 0: + + shooterActive = !shooterActive; + + follower.followPath(paths.Starttoshoot1,true); + setPathState(1); + break; + + case 1: + + if (pathTimer.getElapsedTimeSeconds() > 4) { + launch3balls(); + } + + if (isDone) { + isDone = false; + launchState3 = 0; + launchState2 = 0; + launchState1 = 0; + gate.setPosition(0.3); + follower.followPath(paths.Shoot1topickup1, true); + setPathState(2); + + } + break; + + case 2: + + if (!follower.isBusy()) { + follower.followPath(paths.Pickup1tointake1, 0.7,true); + setPathState(3); + } + break; + + case 3: + + if (!follower.isBusy()) { + intake.setPower(0); + follower.followPath(paths.Intake1toshoot2, true); + setPathState(4); + } + break; + + case 4: + + if (pathTimer.getElapsedTimeSeconds() > 4) { + launch3balls(); + } + + if (isDone) { + isDone = false; + launchState3 = 0; + launchState2 = 0; + launchState1 = 0; + + follower.followPath(paths.Shoot2topark, true); + setPathState(-1); + } + break; + + } + return pathState; + } + public void setPathState(int pState) { + pathState = pState; + pathTimer.resetTimer(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autos/AutoBottomRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autos/AutoBottomRed.java new file mode 100644 index 000000000000..e2260d8cabe7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autos/AutoBottomRed.java @@ -0,0 +1,256 @@ +package org.firstinspires.ftc.teamcode.Autos; +import static android.os.SystemClock.sleep; + +import com.pedropathing.util.Timer; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.TelemetryManager; +import com.bylazar.telemetry.PanelsTelemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.follower.Follower; +import com.pedropathing.paths.PathChain; +import com.pedropathing.geometry.Pose; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.Subsystems.ShooterSubsystem; + +@Autonomous +@Configurable // Panels +public class AutoBottomRed extends OpMode { + + private DcMotor intake; + private Servo gate, flick, hold; // servos + private TelemetryManager panelsTelemetry; // Panels Telemetry instance + public Follower follower; // Pedro Pathing follower instance + private int pathState; // Current autonomous path state (state machine) + private Paths paths; // Paths defined in the Paths class + private ShooterSubsystem shooter; + private double lastTime = 0.0; + private boolean shooterActive = false; + private int launchState3 = 0; + private int launchState2 = 0; + private int launchState1 = 0; + private Timer pathTimer; + private Timer launchTimer; + private boolean isDone; + + + + @Override + public void init() { + panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry(); + + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(new Pose(88.91211401425178, 9.254156769596198, Math.toRadians(90))); + + paths = new Paths(follower); // Build paths + pathTimer = new Timer(); + launchTimer = new Timer(); + intake = hardwareMap.get(DcMotor.class, "intake"); + gate = hardwareMap.get(Servo.class, "gate"); + flick = hardwareMap.get(Servo.class,"flick"); + hold = hardwareMap.get(Servo.class, "hold"); + gate.setPosition(0.3); +// Shooter subsystem + shooter = new ShooterSubsystem(hardwareMap); + panelsTelemetry.debug("Status", "Initialized"); + panelsTelemetry.update(telemetry); + } + @Override + public void start() { + lastTime = getRuntime(); + shooter.getLimelight().start(); + shooter.getLimelight().pipelineSwitch(0); // 0 is for red tracking + } + @Override + public void loop() { + follower.update(); // Update Pedro Pathing + pathState = autonomousPathUpdate(); // Update autonomous state machine + + // launcher update + if (shooterActive) { + double currentTime = getRuntime(); + double dt = currentTime - lastTime; + lastTime = currentTime; + + shooter.update(follower.getPose(), follower.getVelocity(), dt, telemetry); + } + + // Log values to Panels and Driver Station + panelsTelemetry.debug("Path State", pathState); + panelsTelemetry.debug("X", follower.getPose().getX()); + panelsTelemetry.debug("Y", follower.getPose().getY()); + panelsTelemetry.debug("Heading", follower.getPose().getHeading()); + + telemetry.update(); + panelsTelemetry.update(telemetry); + } + + private void launch3balls() { // we call this function every time you want to launch 3 balls + switch (launchState3) { + case 0: + gate.setPosition(0.55); //0.55 open 0.3 closed + launchTimer.resetTimer(); + launchState3++; + hold.setPosition(0.5); + break; + + case 1: + if (launchTimer.getElapsedTimeSeconds() > 1) { + intake.setPower(1); + launchTimer.resetTimer(); + launchState3++; + } + break; + case 2: + intake.setPower(1); + if (launchTimer.getElapsedTimeSeconds() > 1) { + flick.setPosition(0.6); //0 is up + sleep(300); + flick.setPosition(1); + launchTimer.resetTimer(); + isDone = true; + } + break; + } + } + + public static class Paths { + public PathChain Starttoshoot1; + public PathChain Shoot1topickup1; + public PathChain Pickup1tointake1; + public PathChain Intake1toshoot2; + public PathChain Shoot2topark; + + public Paths(Follower follower) { + Starttoshoot1 = follower.pathBuilder().addPath( + new BezierLine( + new Pose(88.912, 9.254), + + new Pose(82.983, 23.914) + ) + ).setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(90)) + + .build(); + + Shoot1topickup1 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(82.983, 23.914), + new Pose(113.215, 8.586), + new Pose(128.438, 8.216) + ) + ).setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(360)) + + .build(); + + Pickup1tointake1 = follower.pathBuilder().addPath( + new BezierLine( + new Pose(128.438, 8.216), + + new Pose(132.86412156941302, 7.971453510002014) + ) + ).setTangentHeadingInterpolation() + + .build(); + + Intake1toshoot2 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(7.971453510002014, 7.971453510002014), + new Pose(110.194, 21.343), + new Pose(82.929, 23.919) + ) + ).setLinearHeadingInterpolation(Math.toRadians(360), Math.toRadians(90)) + + .build(); + + Shoot2topark = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(82.929, 23.919), + new Pose(95.163, 13.333), + new Pose(108.660, 11.164) + ) + ).setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(360)) + + .build(); + } + } + + /* You could check for + - Follower State: "if(!follower.isBusy()) {}" + - Time: "if(pathTimer.getElapsedTimeSeconds() > 1) {}" + - Robot Position: "if(follower.getPose().getX() > 36) {}" + */ + public int autonomousPathUpdate() { + switch (pathState) { + case 0: + + shooterActive = !shooterActive; + + follower.followPath(paths.Starttoshoot1,true); + setPathState(1); + break; + + case 1: + + if (pathTimer.getElapsedTimeSeconds() > 4) { + launch3balls(); + } + + if (isDone) { + isDone = false; + launchState3 = 0; + launchState2 = 0; + launchState1 = 0; + gate.setPosition(0.3); + follower.followPath(paths.Shoot1topickup1, true); + setPathState(2); + + } + break; + + case 2: + + if (!follower.isBusy()) { + follower.followPath(paths.Pickup1tointake1, 0.7,true); + setPathState(3); + } + break; + + case 3: + + if (!follower.isBusy()) { + intake.setPower(0); + follower.followPath(paths.Intake1toshoot2, true); + setPathState(4); + } + break; + + case 4: + + if (pathTimer.getElapsedTimeSeconds() > 4) { + launch3balls(); + } + + if (isDone) { + isDone = false; + launchState3 = 0; + launchState2 = 0; + launchState1 = 0; + + follower.followPath(paths.Shoot2topark, true); + setPathState(-1); + } + break; + + } + return pathState; + } + public void setPathState(int pState) { + pathState = pState; + pathTimer.resetTimer(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autos/AutoTopBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autos/AutoTopBlue.java new file mode 100644 index 000000000000..8b4736d35fb0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autos/AutoTopBlue.java @@ -0,0 +1,327 @@ +package org.firstinspires.ftc.teamcode.Autos; +import static android.os.SystemClock.sleep; + +import com.pedropathing.util.Timer; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.TelemetryManager; +import com.bylazar.telemetry.PanelsTelemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.follower.Follower; +import com.pedropathing.paths.PathChain; +import com.pedropathing.geometry.Pose; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.Subsystems.ShooterSubsystem; + +@Autonomous +@Configurable // Panels +public class AutoTopBlue extends OpMode { + + private DcMotor intake; + private Servo gate, hold, flick; // servos + private TelemetryManager panelsTelemetry; // Panels Telemetry instance + public Follower follower; // Pedro Pathing follower instance + private int pathState; // Current autonomous path state (state machine) + private Paths paths; // Paths defined in the Paths class + private ShooterSubsystem shooter; + private Timer pathTimer; + private Timer launchTimer; + private double lastTime = 0.0; + private boolean shooterActive = false; + private boolean isDone = false; + private boolean isReset = false; + private int launchState3 = 0; + + @Override + public void init() { + panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry(); + + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(new Pose(13.975903614457835, 112.86746987951807, Math.toRadians(90))); + + paths = new Paths(follower); // Build paths + intake = hardwareMap.get(DcMotor.class, "intake"); + gate = hardwareMap.get(Servo.class, "gate"); + hold = hardwareMap.get(Servo.class, "hold"); + flick = hardwareMap.get(Servo.class, "flick"); + pathTimer = new Timer(); + launchTimer = new Timer(); +// Shooter subsystem + shooter = new ShooterSubsystem(hardwareMap); + panelsTelemetry.debug("Status", "Initialized"); + panelsTelemetry.update(telemetry); + } + @Override + public void start() { + lastTime = getRuntime(); + shooter.getLimelight().start(); + shooter.getLimelight().pipelineSwitch(1); // 1 is for blue tracking + } + @Override + public void loop() { + follower.update(); // Update Pedro Pathing + pathState = autonomousPathUpdate(); // Update autonomous state machine + + // launcher update + if (shooterActive) { + double currentTime = getRuntime(); + double dt = currentTime - lastTime; + lastTime = currentTime; + + shooter.update(follower.getPose(), follower.getVelocity(), dt, telemetry); + } + + // Log values to Panels and Driver Station + panelsTelemetry.debug("Path State", pathState); + panelsTelemetry.debug("X", follower.getPose().getX()); + panelsTelemetry.debug("Y", follower.getPose().getY()); + panelsTelemetry.debug("Heading", follower.getPose().getHeading()); + + telemetry.update(); + panelsTelemetry.update(telemetry); + } + private void launch3balls() { // we call this function every time you want to launch 3 balls + switch (launchState3) { + case 0: + gate.setPosition(0.55); //0.55 open 0.3 closed + launchTimer.resetTimer(); + launchState3++; + hold.setPosition(0.5); + break; + + case 1: + if (launchTimer.getElapsedTimeSeconds() > 1) { + intake.setPower(1); + launchTimer.resetTimer(); + launchState3++; + } + break; + case 2: + intake.setPower(1); + if (launchTimer.getElapsedTimeSeconds() > 1) { + flick.setPosition(0.6); //0 is up + sleep(300); + flick.setPosition(1); + launchTimer.resetTimer(); + isDone = true; + } + break; + } + } + + public static class Paths { + public PathChain Starttoshoot1; + public PathChain ShootPretopickup1; + public PathChain Pickup1toshoot2; + public PathChain Shoot3togate; + public PathChain Gatetopickup2; + public PathChain Pickup2tointake; + public PathChain intaketoshoot3; + public PathChain Shoot3topark; + + public Paths(Follower follower) { + Starttoshoot1 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(13.783, 115.759), + new Pose(32.241, 100.735), + new Pose(54.072, 92.096) + ) + ).setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(180)) + + .build(); + + ShootPretopickup1 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(54.072, 92.096), + new Pose(67.114, 55.488), + new Pose(16.361, 59.398) + ) + ).setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)) + + .build(); + + Pickup1toshoot2 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(16.361, 59.398), + new Pose(26.898, 51.102), + new Pose(45.319, 79.645), + new Pose(54.108, 92.060) + ) + ).setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)) + + .build(); + + Shoot3togate = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(54.108, 92.060), + new Pose(59.602, 82.681), + new Pose(17.518, 68.988) + ) + ).setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(90)) + + .build(); + + Gatetopickup2 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(17.518, 68.988), + new Pose(46.512, 69.283), + new Pose(49.024, 85.867) + ) + ).setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(180)) + + .build(); + + Pickup2tointake = follower.pathBuilder().addPath( + new BezierLine( + new Pose(49.024, 85.867), + + new Pose(18.102, 84.735) + ) + ).setTangentHeadingInterpolation() + + .build(); + + intaketoshoot3 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(18.102, 84.735), + new Pose(42.476, 79.651), + new Pose(54.072, 92.084) + ) + ).setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)) + + .build(); + + Shoot3topark = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(54.072, 92.084), + new Pose(60.946, 99.373), + new Pose(56.735, 113.482) + ) + ).setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)) + + .build(); + } + } +/* You could check for + - Follower State: "if(!follower.isBusy()) {}" + - Time: "if(pathTimer.getElapsedTimeSeconds() > 1) {}" + - Robot Position: "if(follower.getPose().getX() > 36) {}" + */ + + public int autonomousPathUpdate() { + switch (pathState) { + case 0: + + shooterActive = true; + + follower.followPath(paths.Starttoshoot1, true); + setPathState(1); + pathTimer.resetTimer(); + break; + + case 1: + + if (pathTimer.getElapsedTimeSeconds() > 3) { + launch3balls(); + } + + if (isDone) { + isDone = false; + launchState3 = 0; + gate.setPosition(0.3); + hold.setPosition(0.5); + follower.followPath(paths.ShootPretopickup1, true); + setPathState(2); + } + break; + + case 2: + if (isReset == false && !follower.isBusy()) { + pathTimer.resetTimer(); + isReset = true; + } + if (!follower.isBusy()) { + if(pathTimer.getElapsedTimeSeconds() > 0.8) { + hold.setPosition(0.3); + follower.followPath(paths.Pickup1toshoot2, true); + intake.setPower(0); + setPathState(5); + pathTimer.resetTimer(); + } + } + break; + + case 5: + if (!follower.isBusy() && pathTimer.getElapsedTimeSeconds() > 1) { + launch3balls(); + } + + if (isDone) { + isDone = false; + isReset = false; + launchState3 = 0; + gate.setPosition(0.3); + hold.setPosition(0.5); + follower.followPath(paths.Shoot3togate, true); + setPathState(6); + pathTimer.resetTimer(); + } + break; + + case 6: + follower.followPath(paths.Gatetopickup2, true); + setPathState(7); + break; + + case 7: + if (!follower.isBusy()) { + follower.followPath(paths.Pickup2tointake); + setPathState(8); + } + break; + + case 8: + if (isReset == false && !follower.isBusy()){ + pathTimer.resetTimer(); + isReset = true; + } + if (!follower.isBusy()) { + if(pathTimer.getElapsedTimeSeconds() > 0.5) { + hold.setPosition(0.3); + follower.followPath(paths.intaketoshoot3, true); + intake.setPower(0); + pathTimer.resetTimer(); + setPathState(9); + } + } + break; + + case 9: + if (!follower.isBusy()) { + launch3balls(); + } + + + if (isDone) { + follower.followPath(paths.Shoot3topark); + isDone = false; + isReset = false; + launchState3 = 0; + gate.setPosition(0.3); + hold.setPosition(0.5); + setPathState(-1); + } + break; + } + return pathState; + } + public void setPathState(int pState) { + pathState = pState; + pathTimer.resetTimer(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autos/AutoTopRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autos/AutoTopRed.java new file mode 100644 index 000000000000..0791dfd58f64 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autos/AutoTopRed.java @@ -0,0 +1,330 @@ +package org.firstinspires.ftc.teamcode.Autos; +import static android.os.SystemClock.sleep; + +import com.pedropathing.util.Timer; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.TelemetryManager; +import com.bylazar.telemetry.PanelsTelemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.follower.Follower; +import com.pedropathing.paths.PathChain; +import com.pedropathing.geometry.Pose; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.Subsystems.ShooterSubsystem; + +@Autonomous +@Configurable // Panels +public class AutoTopRed extends OpMode { + + private DcMotor intake; + private Servo gate, hold, flick; // servos + private TelemetryManager panelsTelemetry; // Panels Telemetry instance + public Follower follower; // Pedro Pathing follower instance + private int pathState; // Current autonomous path state (state machine) + private Paths paths; // Paths defined in the Paths class + private ShooterSubsystem shooter; + private Timer pathTimer; + private Timer launchTimer; + private Timer shootTimer; + private double lastTime = 0.0; + private boolean shooterActive = false; + private int launchState3 = 0; + private boolean isDone = false; + private boolean isReset = false; + + @Override + public void init() { + panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry(); + + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(new Pose(130.21686746987953, 115.7590361445783, Math.toRadians(90))); + + paths = new Paths(follower); // Build paths + intake = hardwareMap.get(DcMotor.class, "intake"); + gate = hardwareMap.get(Servo.class, "gate"); + hold = hardwareMap.get(Servo.class, "hold"); + flick = hardwareMap.get(Servo.class,"flick"); + pathTimer = new Timer(); + launchTimer = new Timer(); +// Shooter subsystem + shooter = new ShooterSubsystem(hardwareMap); + panelsTelemetry.debug("Status", "Initialized"); + panelsTelemetry.update(telemetry); + + hold.setPosition(0.3); + flick.setPosition(1); + } + @Override + public void start() { + lastTime = getRuntime(); + shooter.getLimelight().start(); + shooter.getLimelight().pipelineSwitch(0); // 0 is for red tracking + } + @Override + public void loop() { + follower.update(); // Update Pedro Pathing + pathState = autonomousPathUpdate(); // Update autonomous state machine + + // launcher update + if (shooterActive) { + double currentTime = getRuntime(); + double dt = currentTime - lastTime; + lastTime = currentTime; + + shooter.update(follower.getPose(), follower.getVelocity(), dt, telemetry); + } + + // Log values to Panels and Driver Station + panelsTelemetry.debug("Path State", pathState); + panelsTelemetry.debug("X", follower.getPose().getX()); + panelsTelemetry.debug("Y", follower.getPose().getY()); + panelsTelemetry.debug("Heading", follower.getPose().getHeading()); + + telemetry.update(); + panelsTelemetry.update(telemetry); + } + + private void launch3balls() { // we call this function every time you want to launch 3 balls + switch (launchState3) { + case 0: + gate.setPosition(0.55); //0.55 open 0.3 closed + launchTimer.resetTimer(); + launchState3++; + hold.setPosition(0.5); + break; + + case 1: + if (launchTimer.getElapsedTimeSeconds() > 1) { + intake.setPower(1); + launchTimer.resetTimer(); + launchState3++; + } + break; + case 2: + intake.setPower(1); + if (launchTimer.getElapsedTimeSeconds() > 1) { + flick.setPosition(0.6); //0 is up + sleep(300); + flick.setPosition(1); + launchTimer.resetTimer(); + isDone = true; + } + break; + } + } + public static class Paths { + public PathChain Starttoshoot1; + public PathChain ShootPretopickup1; + public PathChain Pickup1toshoot2; + public PathChain Shoot3togate; + public PathChain Gatetopickup2; + public PathChain Pickup2tointake; + public PathChain intaketoshoot3; + public PathChain Shoot3topark; + + public Paths(Follower follower) { + Starttoshoot1 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(130.217, 115.759), + new Pose(111.759, 100.735), + new Pose(89.928, 92.096) + ) + ).setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(0)) + + .build(); + + ShootPretopickup1 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(89.928, 92.096), + new Pose(76.886, 55.488), + new Pose(127.639, 59.398) + ) + ).setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0)) + + .build(); + + Pickup1toshoot2 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(127.639, 59.398), + new Pose(117.102, 51.102), + new Pose(98.681, 79.645), + new Pose(89.892, 92.060) + ) + ).setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0)) + + .build(); + + Shoot3togate = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(89.892, 92.060), + new Pose(84.398, 82.681), + new Pose(126.482, 68.988) + ) + ).setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(90)) + + .build(); + + Gatetopickup2 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(126.482, 68.988), + new Pose(97.488, 69.283), + new Pose(94.976, 85.867) + ) + ).setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(0)) + + .build(); + + Pickup2tointake = follower.pathBuilder().addPath( + new BezierLine( + new Pose(94.976, 85.867), + + new Pose(125.898, 84.735) + ) + ).setTangentHeadingInterpolation() + .build(); + + intaketoshoot3 = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(125.898, 84.735), + new Pose(101.524, 79.651), + new Pose(89.928, 92.084) + ) + ).setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0)) + + .build(); + + Shoot3topark = follower.pathBuilder().addPath( + new BezierCurve( + new Pose(89.928, 92.084), + new Pose(83.054, 99.373), + new Pose(87.265, 113.482) + ) + ).setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0)) + + .build(); + } + } +/* You could check for + - Follower State: "if(!follower.isBusy()) {}" + - Time: "if(pathTimer.getElapsedTimeSeconds() > 1) {}" + - Robot Position: "if(follower.getPose().getX() > 36) {}" + */ + + public int autonomousPathUpdate() { + switch (pathState) { + case 0: + + shooterActive = true; + + follower.followPath(paths.Starttoshoot1, true); + setPathState(1); + pathTimer.resetTimer(); + break; + + case 1: + + if (pathTimer.getElapsedTimeSeconds() > 3) { + launch3balls(); + } + + if (isDone) { + isDone = false; + launchState3 = 0; + gate.setPosition(0.3); + hold.setPosition(0.5); + follower.followPath(paths.ShootPretopickup1, true); + setPathState(2); + } + break; + + case 2: + if (isReset == false && !follower.isBusy()) { + pathTimer.resetTimer(); + isReset = true; + } + if (!follower.isBusy()) { + if(pathTimer.getElapsedTimeSeconds() > 0.8) { + hold.setPosition(0.3); + follower.followPath(paths.Pickup1toshoot2, true); + intake.setPower(0); + setPathState(5); + pathTimer.resetTimer(); + } + } + break; + + case 5: + if (!follower.isBusy() && pathTimer.getElapsedTimeSeconds() > 1) { + launch3balls(); + } + + if (isDone) { + isDone = false; + isReset = false; + launchState3 = 0; + gate.setPosition(0.3); + hold.setPosition(0.5); + follower.followPath(paths.Shoot3togate, true); + setPathState(6); + pathTimer.resetTimer(); + } + break; + + case 6: + follower.followPath(paths.Gatetopickup2, true); + setPathState(7); + break; + + case 7: + if (!follower.isBusy()) { + follower.followPath(paths.Pickup2tointake); + setPathState(8); + } + break; + + case 8: + if (isReset == false && !follower.isBusy()){ + pathTimer.resetTimer(); + isReset = true; + } + if (!follower.isBusy()) { + if(pathTimer.getElapsedTimeSeconds() > 0.5) { + hold.setPosition(0.3); + follower.followPath(paths.intaketoshoot3, true); + intake.setPower(0); + pathTimer.resetTimer(); + setPathState(9); + } + } + break; + + case 9: + if (!follower.isBusy()) { + launch3balls(); + } + + + if (isDone) { + follower.followPath(paths.Shoot3topark); + isDone = false; + isReset = false; + launchState3 = 0; + gate.setPosition(0.3); + hold.setPosition(0.5); + setPathState(-1); + } + break; + } + return pathState; + } + public void setPathState(int pState) { + pathState = pState; + pathTimer.resetTimer(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Subsystems/ShooterSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Subsystems/ShooterSubsystem.java new file mode 100644 index 000000000000..cd616d9dd86c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Subsystems/ShooterSubsystem.java @@ -0,0 +1,173 @@ +package org.firstinspires.ftc.teamcode.Subsystems; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; + +import com.bylazar.configurables.annotations.Configurable; +import com.pedropathing.geometry.Pose; +import com.pedropathing.math.Vector; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +@Configurable +public class ShooterSubsystem { + + private final DcMotorEx flywheel1; + private final DcMotorEx flywheel2; + private final DcMotorEx turret; + private final Servo hood; + private final Limelight3A limelight; + + // Turret PIDF + public static double turretKp = 1.5; + public static double turretKi = 0.0; + public static double turretKd = 0.03; + public static double turretKf = 0.03; + + // Distance to RPM (from ta) + public static double C = -2000; + public static double D = 6000; + + // Distance to the hood + public static double A = 19.67; + public static double B = 6; + + // Motion compensation (RPM only) + public static double RPM_PER_MPS = 0; + + // RPM limits + public static double MIN_RPM = 3000; + public static double MAX_RPM = 4700; + private double turretIntegral = 0; + private double lastError = 0; + + private double filteredTa = 0; + private static final double TA_ALPHA = 0.15; + + private static final double TICKS_TO_RAD = 2.0 * Math.PI / 8192.0; + + public ShooterSubsystem(HardwareMap hw) { + + flywheel1 = hw.get(DcMotorEx.class, "flywheel1"); + flywheel2 = hw.get(DcMotorEx.class, "flywheel2"); + turret = hw.get(DcMotorEx.class, "turret"); + hood = hw.get(Servo.class, "hood"); + limelight = hw.get(Limelight3A.class, "limelight"); + + flywheel1.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); + flywheel2.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); + + hood.setPosition(0); + } + + public void update(Pose pose, Vector velocity, double dt, Telemetry telemetry) { + + double tps1 = flywheel1.getVelocity(); + double tps2 = flywheel2.getVelocity(); + double rpm1 = (tps1 / 28) * 60; + double rpm2 = (tps2 / 28) * 60; + + double avgRPM = (rpm1 + rpm2) / 2; + + + telemetry.addData("Flywheel RPM", avgRPM); + telemetry.addData("Hood Position", hood.getPosition()); + telemetry.addData("Limelight tx", limelight.getLatestResult().getTx()); + telemetry.addData("Limelight ta", limelight.getLatestResult().getTa()); + telemetry.update(); + + LLResult result = limelight.getLatestResult(); + if (result != null && result.isValid()) { + filteredTa = TA_ALPHA * result.getTa() + (1 - TA_ALPHA) * filteredTa; + } // else keep previous filteredTa + + // --- TURRET TRACKING --- + if (result != null && result.isValid()) { + + double txRad = Math.toRadians(result.getTx()); + + // Normalize to -π..π for shortest rotation + txRad = ((txRad + Math.PI) % (2 * Math.PI)) - Math.PI; + + // PIDF calculations + turretIntegral += txRad * dt; + double derivative = (txRad - lastError) / dt; + + double output = + turretKp * txRad + + turretKi * turretIntegral + + turretKd * derivative + + turretKf * Math.signum(txRad); + + turret.setPower(output); + lastError = txRad; + + } else { + // Target lost → stop turret + turret.setPower(0); + } + + // robot motion + double turretAngle = + turret.getCurrentPosition() * TICKS_TO_RAD; + + double vForward = + velocity.getXComponent() * Math.cos(turretAngle) + + velocity.getYComponent() * Math.sin(turretAngle); + + // flywheel RPM + double targetRPM = + C * Math.sqrt(filteredTa) + D; + + // Backward compensation only + targetRPM += (-vForward) * RPM_PER_MPS; + targetRPM = clamp(targetRPM, MIN_RPM, MAX_RPM); + + double TPS = targetRPM * 28 / 60; + + + flywheel1.setVelocity(TPS); + flywheel2.setVelocity(TPS); + + // hood angle + double hoodAngle = A * Math.sqrt(filteredTa) + B; // hood angle in degrees + + hoodAngle = clamp(hoodAngle, 23, 58); + +// Convert to servo position + hood.setPosition(angleToServo(hoodAngle)); + } + + private double angleToServo(double angleDeg){ + // Map hood angle (degrees) to servo position (0-1) + // 1 = 58° (fully down), 0 = 23° (fully up) + double servoPos = (58.0 - angleDeg) / (58.0 - 23.0); + return clamp(servoPos, 0, 1); + } + + + + private double clamp(double v, double min, double max) { + return Math.max(min, Math.min(max, v)); + } + + public Limelight3A getLimelight() { + return limelight; + } + + public DcMotorEx getFlywheel1() { + return flywheel1; + } + + public DcMotorEx getFlywheel2() { + return flywheel2; + } + public Servo getHood() { + return hood; + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOps/BottomBlueTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOps/BottomBlueTeleOp.java new file mode 100644 index 000000000000..a09f303f9e57 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOps/BottomBlueTeleOp.java @@ -0,0 +1,206 @@ +package org.firstinspires.ftc.teamcode.TeleOps; + +import static android.os.SystemClock.sleep; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; + +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; +import com.pedropathing.geometry.*; +import com.pedropathing.math.*; +import com.pedropathing.paths.*; +import com.pedropathing.paths.HeadingInterpolator; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.pedropathing.util.Timer; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.Subsystems.ShooterSubsystem; + +import java.util.function.Supplier; + +@Configurable +@TeleOp +public class BottomBlueTeleOp extends OpMode { + + int intakeFlag = 0; + int gateFlag = 1; + + private DcMotor intake; + private Servo /*liftleft, liftright,*/ gate, flick, hold, light; // servos + private Follower follower; + public static Pose startingPose; + private boolean automatedDrive; + private boolean slowMode = false; + private double slowModeMultiplier = 0.5; + private Supplier pathChain; + + private TelemetryManager telemetryM; + private ShooterSubsystem shooter; + private double lastTime = 0.0; + private Timer flickTimer; + + + @Override + public void init() { + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(startingPose == null ? new Pose(72,72,90) : startingPose); + follower.update(); + telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); + intake = hardwareMap.get(DcMotor.class, "intake"); + // liftleft = hardwareMap.get(Servo.class, "liftleft"); + // liftright = hardwareMap.get(Servo.class, "liftright"); + gate = hardwareMap.get(Servo.class, "gate"); + flick = hardwareMap.get(Servo.class,"flick"); + hold = hardwareMap.get(Servo.class, "hold"); + light = hardwareMap.get(Servo.class, "light"); + + // Shooter subsystem + shooter = new ShooterSubsystem(hardwareMap); + + flickTimer = new Timer(); + + // Example path (optional) + pathChain = () -> follower.pathBuilder() + .addPath(new Path(new BezierCurve(follower::getPose, new Pose(45, 98)))) + .setHeadingInterpolation(HeadingInterpolator.linearFromPoint(follower::getHeading, Math.toRadians(45), 0.8)) + .build(); + telemetry.addLine("Initialized"); + + gate.setPosition(0.3); + flick.setPosition(1); + hold.setPosition(0.3); + } + + @Override + public void start() { + follower.startTeleopDrive(); + lastTime = getRuntime(); + shooter.getLimelight().start(); + shooter.getLimelight().pipelineSwitch(1); // 1 is for blue tracking + + } + + @Override + public void loop() { + + follower.update(); + telemetryM.update(); + + // TeleOp driving + if (!automatedDrive) { + //Make the last parameter false for field-centric + //In case the drivers want to use a "slowMode" you can scale the vectors + //This is the normal version to use in the TeleOp + if (!slowMode) follower.setTeleOpDrive( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x, + true // Robot Centric + ); + //This is how it looks with slowMode on + else follower.setTeleOpDrive( + -gamepad1.left_stick_y * slowModeMultiplier, + -gamepad1.left_stick_x * slowModeMultiplier, + -gamepad1.right_stick_x * slowModeMultiplier, + true // Robot Centric + ); + } + + + // if (detectedColorBottom == ColorSensorBottom.DetectedColor.GREEN) + + // launcher update + double currentTime = getRuntime(); + double dt = currentTime - lastTime; + lastTime = currentTime; + + shooter.update(follower.getPose(), follower.getVelocity(), dt, telemetry); + + double tps1 = shooter.getFlywheel1().getVelocity(); + double tps2 = shooter.getFlywheel2().getVelocity(); + double rpm1 = (tps1 / 28) * 60; + double rpm2 = (tps2 / 28) * 60; + + double avgRPM = (rpm1 + rpm2) / 2; + + + telemetry.addData("Flywheel RPM", avgRPM); + telemetry.addData("Hood Position", shooter.getHood().getPosition()); + telemetry.addData("Limelight tA", shooter.getLimelight().getLatestResult().getTa()); + telemetry.update(); + + + if (gamepad1.dpadLeftWasPressed()) { + follower.followPath(pathChain.get()); + automatedDrive = true; + } + if (automatedDrive && (gamepad1.dpadRightWasPressed() || !follower.isBusy())) { + follower.startTeleopDrive(); + automatedDrive = false; + } + + if (gamepad1.xWasPressed()){ + flickTimer.resetTimer(); + flick.setPosition(0.6); //0 is up + sleep(300); + flick.setPosition(1); + } + + if (gamepad1.aWasPressed()) { // intake in + if (intakeFlag == 0) { + intake.setPower(1); + hold.setPosition(0.5); + intakeFlag = 1; + } + else if (intakeFlag == 1){ + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } + else if (intakeFlag == -1){ + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } + } + + if (gamepad1.bWasPressed()) { // intake out + if (intakeFlag == 0) { + intake.setPower(-1); + hold.setPosition(0.5); + intakeFlag = -1; + } + else if (intakeFlag == -1){ + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } else if (intakeFlag == 1) { + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } + } + + if (gamepad1.yWasPressed()) { // gate open + if (gateFlag == 0) { + gate.setPosition(0.3); + light.setPosition(0); + gateFlag = 1; + } + else if (gateFlag == 1) { + gate.setPosition(0.5); + light.setPosition(0.64); + gateFlag = 0; + } + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOps/BottomRedTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOps/BottomRedTeleOp.java new file mode 100644 index 000000000000..90714ed5d481 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOps/BottomRedTeleOp.java @@ -0,0 +1,204 @@ +package org.firstinspires.ftc.teamcode.TeleOps; + +import static android.os.SystemClock.sleep; + +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; +import com.pedropathing.geometry.*; +import com.pedropathing.math.*; +import com.pedropathing.paths.*; +import com.pedropathing.paths.HeadingInterpolator; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.pedropathing.util.Timer; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.Subsystems.ShooterSubsystem; + +import java.util.function.Supplier; + +@Configurable +@TeleOp +public class BottomRedTeleOp extends OpMode { + + int intakeFlag = 0; + int gateFlag = 1; + + private DcMotor intake; + private Servo /*liftleft, liftright,*/ gate, flick, hold, light; // servos + private Follower follower; + public static Pose startingPose; + private boolean automatedDrive; + private boolean slowMode = false; + private double slowModeMultiplier = 0.5; + private Supplier pathChain; + + private TelemetryManager telemetryM; + private ShooterSubsystem shooter; + private double lastTime = 0.0; + private Timer flickTimer; + + @Override + public void init() { + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(startingPose == null ? new Pose(72,72,90) : startingPose); + follower.update(); + telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); + intake = hardwareMap.get(DcMotor.class, "intake"); + // liftleft = hardwareMap.get(Servo.class, "liftleft"); + // liftright = hardwareMap.get(Servo.class, "liftright"); + gate = hardwareMap.get(Servo.class, "gate"); + flick = hardwareMap.get(Servo.class,"flick"); + hold = hardwareMap.get(Servo.class, "hold"); + light = hardwareMap.get(Servo.class, "light"); + + // Shooter subsystem + shooter = new ShooterSubsystem(hardwareMap); + flickTimer = new Timer(); + // Example path (optional) + pathChain = () -> follower.pathBuilder() + .addPath(new Path(new BezierCurve(follower::getPose, new Pose(45, 98)))) + .setHeadingInterpolation(HeadingInterpolator.linearFromPoint(follower::getHeading, Math.toRadians(45), 0.8)) + .build(); + telemetry.addLine("Initialized"); + + gate.setPosition(0.3); + flick.setPosition(1); + hold.setPosition(0.3); + } + + @Override + public void start() { + follower.startTeleopDrive(); + lastTime = getRuntime(); + shooter.getLimelight().start(); + shooter.getLimelight().pipelineSwitch(0); // 0 is for red tracking + + } + + @Override + public void loop() { + + follower.update(); + telemetryM.update(); + + // TeleOp driving + if (!automatedDrive) { + //Make the last parameter false for field-centric + //In case the drivers want to use a "slowMode" you can scale the vectors + //This is the normal version to use in the TeleOp + if (!slowMode) follower.setTeleOpDrive( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x, + true // Robot Centric + ); + //This is how it looks with slowMode on + else follower.setTeleOpDrive( + -gamepad1.left_stick_y * slowModeMultiplier, + -gamepad1.left_stick_x * slowModeMultiplier, + -gamepad1.right_stick_x * slowModeMultiplier, + true // Robot Centric + ); + } + + + + // if (detectedColorBottom == ColorSensorBottom.DetectedColor.GREEN) + + // launcher update + double currentTime = getRuntime(); + double dt = currentTime - lastTime; + lastTime = currentTime; + + shooter.update(follower.getPose(), follower.getVelocity(), dt, telemetry); + + double tps1 = shooter.getFlywheel1().getVelocity(); + double tps2 = shooter.getFlywheel2().getVelocity(); + double rpm1 = (tps1 / 28) * 60; + double rpm2 = (tps2 / 28) * 60; + + double avgRPM = (rpm1 + rpm2) / 2; + + + telemetry.addData("Flywheel RPM", avgRPM); + telemetry.addData("Hood Position", shooter.getHood().getPosition()); + telemetry.addData("Limelight tA", shooter.getLimelight().getLatestResult().getTa()); + telemetry.update(); + + + if (gamepad1.dpadLeftWasPressed()) { + follower.followPath(pathChain.get()); + automatedDrive = true; + } + if (automatedDrive && (gamepad1.dpadRightWasPressed() || !follower.isBusy())) { + follower.startTeleopDrive(); + automatedDrive = false; + } + + + if (gamepad1.xWasPressed()){ + flickTimer.resetTimer(); + flick.setPosition(0.6); //0 is up + sleep(300); + flick.setPosition(1); + } + + if (gamepad1.aWasPressed()) { // intake in + if (intakeFlag == 0) { + intake.setPower(1); + hold.setPosition(0.5); + intakeFlag = 1; + } + else if (intakeFlag == 1){ + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } + else if (intakeFlag == -1){ + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } + } + + if (gamepad1.bWasPressed()) { // intake out + if (intakeFlag == 0) { + intake.setPower(-1); + hold.setPosition(0.5); + intakeFlag = -1; + } + else if (intakeFlag == -1){ + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } else if (intakeFlag == 1) { + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } + } + + if (gamepad1.yWasPressed()) { // gate open + if (gateFlag == 0) { + gate.setPosition(0.3); + light.setPosition(0); + gateFlag = 1; + } + else if (gateFlag == 1) { + gate.setPosition(0.5); + light.setPosition(0.64); + gateFlag = 0; + } + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOps/TopBlueTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOps/TopBlueTeleOp.java new file mode 100644 index 000000000000..8ca0f93e3137 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOps/TopBlueTeleOp.java @@ -0,0 +1,204 @@ +package org.firstinspires.ftc.teamcode.TeleOps; + +import static android.os.SystemClock.sleep; + +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; +import com.pedropathing.geometry.*; +import com.pedropathing.math.*; +import com.pedropathing.paths.*; +import com.pedropathing.paths.HeadingInterpolator; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.pedropathing.util.Timer; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.Subsystems.ShooterSubsystem; + +import java.util.function.Supplier; + +@Configurable +@TeleOp +public class TopBlueTeleOp extends OpMode { + + int intakeFlag = 0; + int gateFlag = 1; + + private DcMotor intake; + private Servo /*liftleft, liftright,*/ gate, flick, hold, light; // servos + private Follower follower; + public static Pose startingPose; + private boolean automatedDrive; + private boolean slowMode = false; + private double slowModeMultiplier = 0.5; + private Supplier pathChain; + + private TelemetryManager telemetryM; + private ShooterSubsystem shooter; + private double lastTime = 0.0; + + private Timer flickTimer; + + @Override + public void init() { + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(startingPose == null ? new Pose(72,72,90) : startingPose); + follower.update(); + telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); + intake = hardwareMap.get(DcMotor.class, "intake"); + // liftleft = hardwareMap.get(Servo.class, "liftleft"); + // liftright = hardwareMap.get(Servo.class, "liftright"); + gate = hardwareMap.get(Servo.class, "gate"); + flick = hardwareMap.get(Servo.class,"flick"); + hold = hardwareMap.get(Servo.class, "hold"); + light = hardwareMap.get(Servo.class, "light"); + + // Shooter subsystem + shooter = new ShooterSubsystem(hardwareMap); + + flickTimer = new Timer(); + // Example path (optional) + pathChain = () -> follower.pathBuilder() + .addPath(new Path(new BezierCurve(follower::getPose, new Pose(45, 98)))) + .setHeadingInterpolation(HeadingInterpolator.linearFromPoint(follower::getHeading, Math.toRadians(45), 0.8)) + .build(); + telemetry.addLine("Initialized"); + + gate.setPosition(0.3); + flick.setPosition(1); + hold.setPosition(0.3); + } + + @Override + public void start() { + follower.startTeleopDrive(); + lastTime = getRuntime(); + shooter.getLimelight().start(); + shooter.getLimelight().pipelineSwitch(1); // 1 is for blue tracking + + } + + @Override + public void loop() { + + follower.update(); + telemetryM.update(); + + // TeleOp driving + if (!automatedDrive) { + //Make the last parameter false for field-centric + //In case the drivers want to use a "slowMode" you can scale the vectors + //This is the normal version to use in the TeleOp + if (!slowMode) follower.setTeleOpDrive( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x, + true // Robot Centric + ); + //This is how it looks with slowMode on + else follower.setTeleOpDrive( + -gamepad1.left_stick_y * slowModeMultiplier, + -gamepad1.left_stick_x * slowModeMultiplier, + -gamepad1.right_stick_x * slowModeMultiplier, + true // Robot Centric + ); + } + + + // if (detectedColorBottom == ColorSensorBottom.DetectedColor.GREEN) + + // launcher update + double currentTime = getRuntime(); + double dt = currentTime - lastTime; + lastTime = currentTime; + + shooter.update(follower.getPose(), follower.getVelocity(), dt, telemetry); + + double tps1 = shooter.getFlywheel1().getVelocity(); + double tps2 = shooter.getFlywheel2().getVelocity(); + double rpm1 = (tps1 / 28) * 60; + double rpm2 = (tps2 / 28) * 60; + + double avgRPM = (rpm1 + rpm2) / 2; + + + telemetry.addData("Flywheel RPM", avgRPM); + telemetry.addData("Hood Position", shooter.getHood().getPosition()); + telemetry.addData("Limelight tA", shooter.getLimelight().getLatestResult().getTa()); + telemetry.update(); + + + if (gamepad1.dpadLeftWasPressed()) { + follower.followPath(pathChain.get()); + automatedDrive = true; + } + if (automatedDrive && (gamepad1.dpadRightWasPressed() || !follower.isBusy())) { + follower.startTeleopDrive(); + automatedDrive = false; + } + + if (gamepad1.xWasPressed()){ + flickTimer.resetTimer(); + flick.setPosition(0.6); //0 is up + sleep(300); + flick.setPosition(1); + } + + if (gamepad1.aWasPressed()) { // intake in + if (intakeFlag == 0) { + intake.setPower(1); + hold.setPosition(0.5); + intakeFlag = 1; + } + else if (intakeFlag == 1){ + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } + else if (intakeFlag == -1){ + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } + } + + if (gamepad1.bWasPressed()) { // intake out + if (intakeFlag == 0) { + intake.setPower(-1); + hold.setPosition(0.5); + intakeFlag = -1; + } + else if (intakeFlag == -1){ + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } else if (intakeFlag == 1) { + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } + } + + if (gamepad1.yWasPressed()) { // gate open + if (gateFlag == 0) { + gate.setPosition(0.3); + light.setPosition(0); + gateFlag = 1; + } + else if (gateFlag == 1) { + gate.setPosition(0.55); + light.setPosition(0.64); + gateFlag = 0; + } + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOps/TopRedTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOps/TopRedTeleOp.java new file mode 100644 index 000000000000..2a8302bb4654 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOps/TopRedTeleOp.java @@ -0,0 +1,191 @@ +package org.firstinspires.ftc.teamcode.TeleOps; + +import static android.os.SystemClock.sleep; + +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; +import com.pedropathing.geometry.*; +import com.pedropathing.math.*; +import com.pedropathing.paths.*; +import com.pedropathing.paths.HeadingInterpolator; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.pedropathing.util.Timer; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; + + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.Subsystems.ShooterSubsystem; + +import java.util.function.Supplier; + +@Configurable +@TeleOp +public class TopRedTeleOp extends OpMode { + + int intakeFlag = 0; + int gateFlag = 1; + + private DcMotor intake; + private Servo /*liftleft, liftright,*/ gate, flick, hold, light; // servos + private Follower follower; + public static Pose startingPose; + private boolean automatedDrive; + private boolean slowMode = false; + private double slowModeMultiplier = 0.5; + private Supplier pathChain; + + private TelemetryManager telemetryM; + private ShooterSubsystem shooter; + private double lastTime = 0.0; + private Timer flickTimer; + + + @Override + public void init() { + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(startingPose == null ? new Pose(72,72,90) : startingPose); + follower.update(); + telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); + intake = hardwareMap.get(DcMotor.class, "intake"); + // liftleft = hardwareMap.get(Servo.class, "liftleft"); + // liftright = hardwareMap.get(Servo.class, "liftright"); + gate = hardwareMap.get(Servo.class, "gate"); + flick = hardwareMap.get(Servo.class,"flick"); + hold = hardwareMap.get(Servo.class, "hold"); + light = hardwareMap.get(Servo.class, "light"); + + // Shooter subsystem + shooter = new ShooterSubsystem(hardwareMap); + flickTimer = new Timer(); + + // Example path (optional) + pathChain = () -> follower.pathBuilder() + .addPath(new Path(new BezierCurve(follower::getPose, new Pose(45, 98)))) + .setHeadingInterpolation(HeadingInterpolator.linearFromPoint(follower::getHeading, Math.toRadians(45), 0.8)) + .build(); + telemetry.addLine("Initialized"); + + gate.setPosition(0.3); + flick.setPosition(1); + hold.setPosition(0.3); + } + + @Override + public void start() { + follower.startTeleopDrive(); + lastTime = getRuntime(); + shooter.getLimelight().start(); + shooter.getLimelight().pipelineSwitch(0); // 0 is for red tracking + + } + + @Override + public void loop() { + + follower.update(); + telemetryM.update(); + + // TeleOp driving + if (!automatedDrive) { + //Make the last parameter false for field-centric + //In case the drivers want to use a "slowMode" you can scale the vectors + //This is the normal version to use in the TeleOp + if (!slowMode) follower.setTeleOpDrive( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x, + true // Robot Centric + ); + //This is how it looks with slowMode on + else follower.setTeleOpDrive( + -gamepad1.left_stick_y * slowModeMultiplier, + -gamepad1.left_stick_x * slowModeMultiplier, + -gamepad1.right_stick_x * slowModeMultiplier, + true // Robot Centric + ); + } + + + // if (detectedColorBottom == ColorSensorBottom.DetectedColor.GREEN) + + // launcher update + double currentTime = getRuntime(); + double dt = currentTime - lastTime; + lastTime = currentTime; + + shooter.update(follower.getPose(), follower.getVelocity(), dt, telemetry); + + if (gamepad1.dpadLeftWasPressed()) { + follower.followPath(pathChain.get()); + automatedDrive = true; + } + if (automatedDrive && (gamepad1.dpadRightWasPressed() || !follower.isBusy())) { + follower.startTeleopDrive(); + automatedDrive = false; + } + + if (gamepad1.xWasPressed()){ + flickTimer.resetTimer(); + flick.setPosition(0.6); //0 is up + sleep(300); + flick.setPosition(1); + } + + if (gamepad1.aWasPressed()) { // intake in + if (intakeFlag == 0) { + intake.setPower(1); + hold.setPosition(0.5); + intakeFlag = 1; + } + else if (intakeFlag == 1){ + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } + else if (intakeFlag == -1){ + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } + } + + if (gamepad1.bWasPressed()) { // intake out + if (intakeFlag == 0) { + intake.setPower(-1); + hold.setPosition(0.5); + intakeFlag = -1; + } + else if (intakeFlag == -1){ + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } else if (intakeFlag == 1) { + intake.setPower(0); + hold.setPosition(0.3); + intakeFlag = 0; + } + } + + if (gamepad1.yWasPressed()) { // gate open + if (gateFlag == 0) { + gate.setPosition(0.3); + light.setPosition(0); + gateFlag = 1; + } + else if (gateFlag == 1) { + gate.setPosition(0.55); + light.setPosition(0.64); + gateFlag = 0; + } + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index fa30420ac7cd..18dc24e14806 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -1,19 +1,63 @@ package org.firstinspires.ftc.teamcode.pedroPathing; +import com.pedropathing.control.FilteredPIDFCoefficients; +import com.pedropathing.control.PIDFCoefficients; +import com.pedropathing.control.PredictiveBrakingCoefficients; import com.pedropathing.follower.Follower; import com.pedropathing.follower.FollowerConstants; import com.pedropathing.ftc.FollowerBuilder; +import com.pedropathing.ftc.drivetrains.MecanumConstants; +import com.pedropathing.ftc.localization.constants.PinpointConstants; import com.pedropathing.paths.PathConstraints; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + public class Constants { - public static FollowerConstants followerConstants = new FollowerConstants(); + public static FollowerConstants followerConstants = new FollowerConstants() + .mass(17.95) // mass in kg + // .forwardZeroPowerAcceleration(-42.944916498283) // have to be negative + //.lateralZeroPowerAcceleration(-53.72242120209232) + // .translationalPIDFCoefficients(new PIDFCoefficients(0.15, 0, 0.015, 0.04)) // these are the PIDF + .headingPIDFCoefficients(new PIDFCoefficients(1.9, 0, 0.1, 0.01)) + .predictiveBrakingCoefficients(new PredictiveBrakingCoefficients(0.14, 0.1432543903599445, 0.0013771510969864621)) + // .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.05,0.0,0.00001,0.6,0.05)) + .centripetalScaling(0.0); + ; + + public static MecanumConstants driveConstants = new MecanumConstants() + .maxPower(1) // max motor power + .rightFrontMotorName("rf") // Motor hardware maps + .rightRearMotorName("rr") + .leftRearMotorName("lr") + .leftFrontMotorName("lf") + .leftFrontMotorDirection(DcMotorSimple.Direction.REVERSE) // Motor directions + .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE) + .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD) + .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD) + .xVelocity(66.64853169598919) + .yVelocity(46.88268946850394) + .useBrakeModeInTeleOp(true); +; + + public static PinpointConstants localizerConstants = new PinpointConstants() + .forwardPodY(-3) // Y offset + .strafePodX(-7) // X offset + .distanceUnit(DistanceUnit.INCH) + .hardwareMapName("pinpoint") // hardware map for pinpoint computer + .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) + .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD) // Encoder direction + .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED); - public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); + public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 0.3); public static Follower createFollower(HardwareMap hardwareMap) { return new FollowerBuilder(followerConstants, hardwareMap) .pathConstraints(pathConstraints) + .mecanumDrivetrain(driveConstants) + .pinpointLocalizer(localizerConstants) .build(); } }