KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Deleted files + Drivetrain.java

Browse Source
This commit is contained in:
KeshavAnandCode
2025-11-24 17:19:18 -06:00
parent 09d82c1e02
commit 894a8d26fb
15 changed files with 18 additions and 3037 deletions
Download Patch File Download Diff File Expand all files Collapse all files

478
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java
View File

@@ -1,478 +0,0 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Poses.h1;
import static org.firstinspires.ftc.teamcode.constants.Poses.h2;
import static org.firstinspires.ftc.teamcode.constants.Poses.h2_b;
import static org.firstinspires.ftc.teamcode.constants.Poses.h3;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.Poses.x1;
import static org.firstinspires.ftc.teamcode.constants.Poses.x2;
import static org.firstinspires.ftc.teamcode.constants.Poses.x2_b;
import static org.firstinspires.ftc.teamcode.constants.Poses.x3;
import static org.firstinspires.ftc.teamcode.constants.Poses.y1;
import static org.firstinspires.ftc.teamcode.constants.Poses.y2;
import static org.firstinspires.ftc.teamcode.constants.Poses.y2_b;
import static org.firstinspires.ftc.teamcode.constants.Poses.y3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ParallelAction;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
import org.firstinspires.ftc.teamcode.subsystems.Intake;
import org.firstinspires.ftc.teamcode.subsystems.Shooter;
import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
import org.firstinspires.ftc.teamcode.subsystems.Transfer;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
@Autonomous (preselectTeleOp = "TeleopV1")
public class Blue extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
AprilTag aprilTag;
Shooter shooter;
public static double angle = 0.1;
Spindexer spindexer;
Transfer transfer;
public class shooterOn implements Action {
int ticker = 1;
double stamp = 0.0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker ==1){
stamp = getRuntime();
}
ticker ++;
if (getRuntime() - stamp < 0.2){
return true;
} else if (getRuntime() - stamp < 0.4) {
shooter.setManualPower(1);
shooter.update();
return true;
} else {
return false;
}
}
}
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
drive = new MecanumDrive(hardwareMap, new Pose2d(
0,0,0
));
aprilTag = new AprilTag(robot, TELE);
shooter = new Shooter(robot, TELE);
spindexer = new Spindexer(robot, TELE);
spindexer.outtake3();
shooter.setShooterMode("MANUAL");
shooter.sethoodPosition(0.53);
transfer = new Transfer(robot);
transfer.setTransferPosition(transferServo_out);
Intake intake = new Intake(robot);
robot.hood.setPosition(hoodDefault);
TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
TrajectoryActionBuilder traj2 = 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))
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
TrajectoryActionBuilder traj4 = 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))
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x1, y1-30), h1 );
while(opModeInInit()) {
if (gamepad2.dpadUpWasPressed()){
hoodDefault -= 0.02;
}
if (gamepad2.dpadDownWasPressed()){
hoodDefault += 0.02;
}
robot.hood.setPosition(hoodDefault);
shooter.setTurretPosition(0.3);
aprilTag.initTelemetry();
aprilTag.update();
shooter.update();
TELE.addData("hood", hoodDefault);
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
if (opModeIsActive()){
robot.hood.setPosition(hoodDefault);
transfer.setTransferPower(1);
transfer.update();
Actions.runBlocking(
new ParallelAction(
traj1.build(),
new shooterOn()
)
);
transfer.setTransferPower(1);
transfer.update();
shooter.setManualPower(1);
double stamp = getRuntime();
stamp = getRuntime();
while (getRuntime()-stamp<4.5) {
shooter.moveTurret(0.3);
double time = getRuntime()-stamp;
if (time < 0.3) {
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 1) {
spindexer.outtake3();
} else if (time < 1.4) {
transfer.transferIn();
} else if (time < 2.6) {
transfer.transferOut();
spindexer.outtake2();
} else if (time < 3.0) {
transfer.transferIn();
} else if (time < 4.0) {
transfer.transferOut();
spindexer.outtake1();
} else if (time < 4.4) {
transfer.transferIn();
} else {
transfer.transferOut();
spindexer.outtake3();
shooter.setManualPower(1);
}
transfer.update();
shooter.update();
spindexer.update();
}
spindexer.outtake3();
robot.intake.setPower(1);
Actions.runBlocking(
traj2.build()
);
Actions.runBlocking(
traj3.build()
);
shooter.setManualPower(1);
robot.intake.setPower(0);
spindexer.outtake3();
stamp = getRuntime();
shooter.setManualPower(1);
while (getRuntime()-stamp<4.5) {
shooter.moveTurret(0.3);
double time = getRuntime()-stamp;
if (time < 0.3) {
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 1) {
spindexer.outtake3();
} else if (time < 1.4) {
transfer.transferIn();
} else if (time < 2.6) {
transfer.transferOut();
spindexer.outtake2();
} else if (time < 3.0) {
transfer.transferIn();
} else if (time < 4.0) {
transfer.transferOut();
spindexer.outtake1();
} else if (time < 4.4) {
transfer.transferIn();
} else {
transfer.transferOut();
spindexer.outtake3();
shooter.setManualPower(1);
}
transfer.update();
shooter.update();
spindexer.update();
}
spindexer.outtake3();
robot.intake.setPower(1);
Actions.runBlocking(
traj4.build()
);
Actions.runBlocking(
traj5.build()
);
shooter.setManualPower(1);
robot.intake.setPower(0);
stamp = getRuntime();
shooter.setManualPower(1);
while (getRuntime()-stamp<4.5) {
shooter.moveTurret(0.3);
double time = getRuntime()-stamp;
if (time < 0.3) {
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 1) {
spindexer.outtake3();
} else if (time < 1.4) {
transfer.transferIn();
} else if (time < 2.6) {
transfer.transferOut();
spindexer.outtake2();
} else if (time < 3.0) {
transfer.transferIn();
} else if (time < 4.0) {
transfer.transferOut();
spindexer.outtake1();
} else if (time < 4.4) {
transfer.transferIn();
} else {
transfer.transferOut();
spindexer.outtake3();
shooter.setManualPower(1);
}
transfer.update();
shooter.update();
spindexer.update();
}
spindexer.outtake3();
Actions.runBlocking(
traj6.build()
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addLine("finished");
TELE.update();
sleep (2000);
}
}
}

431
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java
View File

@@ -1,431 +0,0 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ParallelAction;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
import org.firstinspires.ftc.teamcode.subsystems.Intake;
import org.firstinspires.ftc.teamcode.subsystems.Shooter;
import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
import org.firstinspires.ftc.teamcode.subsystems.Transfer;
import org.firstinspires.ftc.teamcode.utils.Robot;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
@Config
@Autonomous
public class Red extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
AprilTag aprilTag;
Shooter shooter;
public static double angle = 0.1;
Spindexer spindexer;
Transfer transfer;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
drive = new MecanumDrive(hardwareMap, new Pose2d(
0,0,0
));
aprilTag = new AprilTag(robot, TELE);
shooter = new Shooter(robot, TELE);
spindexer = new Spindexer(robot, TELE);
spindexer.outtake3();
shooter.setShooterMode("MANUAL");
shooter.sethoodPosition(hoodDefault);
transfer = new Transfer(robot);
transfer.setTransferPosition(transferServo_out);
Intake intake = new Intake(robot);
robot.hood.setPosition(hoodDefault);
TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
TrajectoryActionBuilder traj2 = 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))
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
TrajectoryActionBuilder traj4 = 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))
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x1, y1+30), h1 );
while(opModeInInit()) {
if (gamepad2.dpadUpWasPressed()){
hoodDefault -= 0.02;
}
if (gamepad2.dpadDownWasPressed()){
hoodDefault += 0.02;
}
robot.hood.setPosition(hoodDefault);
shooter.setTurretPosition(0.33);
aprilTag.initTelemetry();
aprilTag.update();
shooter.update();
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
if (opModeIsActive()){
robot.hood.setPosition(hoodDefault);
transfer.setTransferPower(1);
transfer.update();
Actions.runBlocking(
new ParallelAction(
traj1.build()
)
);
transfer.setTransferPower(1);
transfer.update();
shooter.setManualPower(1);
double stamp = getRuntime();
stamp = getRuntime();
while (getRuntime()-stamp<4.5) {
shooter.moveTurret(0.31);
double time = getRuntime()-stamp;
if (time < 0.3) {
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 1) {
spindexer.outtake3();
} else if (time < 1.4) {
transfer.transferIn();
} else if (time < 2.6) {
transfer.transferOut();
spindexer.outtake2();
} else if (time < 3.0) {
transfer.transferIn();
} else if (time < 4.0) {
transfer.transferOut();
spindexer.outtake1();
} else if (time < 4.4) {
transfer.transferIn();
} else {
transfer.transferOut();
spindexer.outtake3();
shooter.setManualPower(0);
}
transfer.update();
shooter.update();
spindexer.update();
}
spindexer.outtake3();
robot.intake.setPower(1);
Actions.runBlocking(
traj2.build()
);
Actions.runBlocking(
traj3.build()
);
shooter.setManualPower(1);
robot.intake.setPower(0);
spindexer.outtake3();
stamp = getRuntime();
shooter.setManualPower(1);
while (getRuntime()-stamp<4.5) {
shooter.moveTurret(0.31);
double time = getRuntime()-stamp;
if (time < 0.3) {
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 1) {
spindexer.outtake3();
} else if (time < 1.4) {
transfer.transferIn();
} else if (time < 2.6) {
transfer.transferOut();
spindexer.outtake2();
} else if (time < 3.0) {
transfer.transferIn();
} else if (time < 4.0) {
transfer.transferOut();
spindexer.outtake1();
} else if (time < 4.4) {
transfer.transferIn();
} else {
transfer.transferOut();
spindexer.outtake3();
shooter.setManualPower(0);
}
transfer.update();
shooter.update();
spindexer.update();
}
spindexer.outtake3();
robot.intake.setPower(1);
Actions.runBlocking(
traj4.build()
);
Actions.runBlocking(
traj5.build()
);
shooter.setManualPower(1);
robot.intake.setPower(0);
stamp = getRuntime();
shooter.setManualPower(1);
while (getRuntime()-stamp<4.5) {
shooter.moveTurret(0.31);
double time = getRuntime()-stamp;
if (time < 0.3) {
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 1) {
spindexer.outtake3();
} else if (time < 1.4) {
transfer.transferIn();
} else if (time < 2.6) {
transfer.transferOut();
spindexer.outtake2();
} else if (time < 3.0) {
transfer.transferIn();
} else if (time < 4.0) {
transfer.transferOut();
spindexer.outtake1();
} else if (time < 4.4) {
transfer.transferIn();
} else {
transfer.transferOut();
spindexer.outtake3();
shooter.setManualPower(0);
}
transfer.update();
shooter.update();
spindexer.update();
}
spindexer.outtake3();
Actions.runBlocking(
traj6.build()
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addLine("finished");
TELE.update();
sleep (2000);
}
}
}

29
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java
View File

@@ -1,29 +0,0 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
@Config
public class Poses {
public static double goalHeight = 42; //in inches
public static double turretHeight = 12;
public static double relativeGoalHeight = goalHeight - turretHeight;
public static double x1 = 50, y1 = 0, h1 = 0;
public static double x2 = 31, y2 = 32, h2 = Math.toRadians(135);
public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(135);
public static double x3 = 34, y3 = 58, h3 = Math.toRadians(135);
public static Pose2d teleStart = new Pose2d(x1,-10,0);
}

23
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
View File

@@ -1,23 +0,0 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
@Config
public class ServoPositions {
public static double spindexer_intakePos = 0.665;
public static double spindexer_outtakeBall3 = 0.845;
public static double spindexer_outtakeBall2 = 0.48;
public static double spindexer_outtakeBall1 = 0.1;
public static double transferServo_out = 0.13;
public static double transferServo_in = 0.4;
public static double hoodDefault = 0.35;
}

238
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java
View File

@@ -1,238 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystems;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
public class AprilTag implements Subsystem {
private AprilTagProcessor aprilTag;
private VisionPortal visionPortal;
private MultipleTelemetry TELE;
private boolean teleOn = false;
private int detections = 0;
List<AprilTagDetection> currentDetections;
ArrayList<ArrayList<Double>> Data = new ArrayList<>();
public AprilTag(Robot robot, MultipleTelemetry tele) {
this.aprilTag = robot.aprilTagProcessor;
this.TELE = tele;
}
@Override
public void update() {
currentDetections = aprilTag.getDetections();
UpdateData();
if(teleOn){
tagTELE();
initTelemetry();
}
}
public void initTelemetry (){
TELE.addData("Camera Preview", "Check Driver Station for stream");
TELE.addData("Status", "Initialized - Press START");
}
public void tagTELE () {
TELE.addData("# AprilTags Detected", detections);
// Display info for each detected tag
for (ArrayList<Double> detection : Data) {
if (detection.get(0) ==1) {
// Known AprilTag with metadata
TELE.addLine(String.format("\n==== (ID %d) %s ====",
detection.get(1).intValue(), ""));
TELE.addLine(String.format("XYZ: %6.1f %6.1f %6.1f (inch)",
detection.get(2),
detection.get(3),
detection.get(4)));
TELE.addData("Distance", getDistance(detection.get(1).intValue()));
TELE.addLine(String.format("PRY: %6.1f %6.1f %6.1f (deg)",
detection.get(5),
detection.get(6),
detection.get(7)));
TELE.addLine(String.format("RBE: %6.1f %6.1f %6.1f (inch, deg, deg)",
detection.get(8),
detection.get(9),
detection.get(10)));
}
}
}
public void turnTelemetryOn(boolean bool) {
teleOn = bool;
}
public void UpdateData () {
Data.clear(); // <--- THIS FIXES YOUR ISSUE
detections = currentDetections.size();
for (AprilTagDetection detection : currentDetections) {
ArrayList<Double> detectionData = new ArrayList<Double>();
if (detection.metadata != null) {
detectionData.add(1.0);
// Known AprilTag with metadata
detectionData.add( (double) detection.id);
detectionData.add(detection.ftcPose.x);
detectionData.add(detection.ftcPose.y);
detectionData.add(detection.ftcPose.z);
detectionData.add(detection.ftcPose.pitch);
detectionData.add(detection.ftcPose.roll);
detectionData.add(detection.ftcPose.yaw);
detectionData.add(detection.ftcPose.range);
detectionData.add(detection.ftcPose.bearing);
detectionData.add(detection.ftcPose.elevation);
} else {
detectionData.add(0, 0.0);
}
Data.add(detectionData);
}
}
public int getDetectionCount() {
return detections;
}
public boolean isDetected (int id){
return (!filterID(Data, (double) id ).isEmpty());
}
public double getDistance(int id) {
ArrayList<Double> d = filterID(Data, (double) id);
if (d.size() >= 5) {
double x = d.get(2);
double y = d.get(3);
double z = d.get(4);
return Math.sqrt(x*x + y*y + z*z);
}
return -1; // tag not found
}
// Returns the position as [x, y, z]
public List<Double> getPosition(int id) {
ArrayList<Double> d = filterID(Data, (double) id);
if (d.size() >= 5) {
List<Double> pos = new ArrayList<>();
pos.add(d.get(2));
pos.add(d.get(3));
pos.add(d.get(4));
return pos;
}
return Collections.emptyList();
}
// Returns orientation as [pitch, roll, yaw]
public List<Double> getOrientation(int id) {
ArrayList<Double> d = filterID(Data, (double) id);
if (d.size() >= 8) {
List<Double> ori = new ArrayList<>();
ori.add(d.get(5));
ori.add(d.get(6));
ori.add(d.get(7));
return ori;
}
return Collections.emptyList();
}
// Returns range, bearing, elevation as [range, bearing, elevation]
public List<Double> getRBE(int id) {
ArrayList<Double> d = filterID(Data, (double) id);
if (d.size() >= 11) {
List<Double> rbe = new ArrayList<>();
rbe.add(d.get(8));
rbe.add(d.get(9));
rbe.add(d.get(10));
return rbe;
}
return Collections.emptyList();
}
// Returns full raw data for debugging or custom processing
public ArrayList<Double> getRawData(int id) {
return filterID(Data, (double) id);
}
public static ArrayList<Double> filterID(ArrayList<ArrayList<Double>> data, double x) {
for (ArrayList<Double> innerList : data) {
// Ensure it has a second element
if (innerList.size() > 1 && Math.abs(innerList.get(1) - x) < 1e-9) {
return innerList; // Return the first match
}
}
// Return an empty ArrayList if no match found
return new ArrayList<>();
}
}

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
View File

@@ -4,31 +4,25 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
import java.util.Objects; import java.util.Objects;
public class Drivetrain implements Subsystem { public class Drivetrain {
private GamepadEx gamepad;
private final GamepadEx gamepad;
private final DcMotorEx fl;
private final DcMotorEx fr;
private final DcMotorEx bl;
private final DcMotorEx br;
public MultipleTelemetry TELE; public MultipleTelemetry TELE;
private String Mode = "Default"; private String Mode = "Default";
private DcMotorEx fl;
private DcMotorEx fr;
private DcMotorEx bl;
private DcMotorEx br;
private double defaultSpeed = 0.7; private double defaultSpeed = 0.7;
private double slowSpeed = 0.3; private double slowSpeed = 0.3;
public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1){
public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1) {
this.fl = robot.frontLeft; this.fl = robot.frontLeft;
this.fr = robot.frontRight; this.fr = robot.frontRight;
@@ -39,23 +33,21 @@ public class Drivetrain implements Subsystem {
this.TELE = tele; this.TELE = tele;
} }
public void setMode (String mode){ public void setMode(String mode) {
this.Mode = mode; this.Mode = mode;
} }
public void setDefaultSpeed (double speed){ public void setDefaultSpeed(double speed) {
this.defaultSpeed = speed; this.defaultSpeed = speed;
} }
public void setSlowSpeed (double speed){ public void setSlowSpeed(double speed) {
this.slowSpeed = speed; this.slowSpeed = speed;
} }
public void RobotCentric(double fwd, double strafe, double turn, double turbo){ public void RobotCentric(double fwd, double strafe, double turn, double turbo) {
double y = -fwd; // Remember, Y stick value is reversed double y = -fwd; // Remember, Y stick value is reversed
double x = strafe * 1.1; // Counteract imperfect strafing double x = strafe * 1.1; // Counteract imperfect strafing
@@ -70,15 +62,13 @@ public class Drivetrain implements Subsystem {
double frontRightPower = (y - x - rx) / denominator; double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator; double backRightPower = (y + x - rx) / denominator;
fl.setPower(frontLeftPower*turbo); fl.setPower(frontLeftPower * turbo);
bl.setPower(backLeftPower*turbo); bl.setPower(backLeftPower * turbo);
fr.setPower(frontRightPower*turbo); fr.setPower(frontRightPower * turbo);
br.setPower(backRightPower*turbo); br.setPower(backRightPower * turbo);
} }
@Override
public void update() { public void update() {
if (Objects.equals(Mode, "Default")) { if (Objects.equals(Mode, "Default")) {
@@ -88,7 +78,7 @@ public class Drivetrain implements Subsystem {
gamepad.getRightX(), gamepad.getRightX(),
gamepad.getLeftX(), gamepad.getLeftX(),
(gamepad.getTrigger( (gamepad.getTrigger(
GamepadKeys.Trigger.RIGHT_TRIGGER) * (1-defaultSpeed) GamepadKeys.Trigger.RIGHT_TRIGGER) * (1 - defaultSpeed)
- gamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) * slowSpeed - gamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) * slowSpeed
+ defaultSpeed + defaultSpeed
) )
@@ -96,4 +86,4 @@ public class Drivetrain implements Subsystem {
} }
} }
} }

77
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
View File

@@ -1,77 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystems;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.utils.Robot;
public class Intake implements Subsystem {
private GamepadEx gamepad;
public MultipleTelemetry TELE;
private DcMotorEx intake;
private double intakePower = 1.0;
private int intakeState = 0;
public Intake (Robot robot){
this.intake = robot.intake;
}
public int getIntakeState() {
return intakeState;
}
public void toggle(){
if (intakeState !=0){
intakeState = 0;
} else {
intakeState = 1;
}
}
public void setIntakePower(double pow){
intakePower = pow;
}
public void intake(){
intakeState =1;
}
public void reverse(){
intakeState =-1;
}
public void stop(){
intakeState =-1;
}
@Override
public void update() {
if (intakeState == 1){
intake.setPower(intakePower);
} else if (intakeState == -1){
intake.setPower(-intakePower);
} else {
intake.setPower(0);
}
}
}

337
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
View File

@@ -1,337 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystems;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.controller.PIDController;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.PIDCoefficients;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
import org.firstinspires.ftc.teamcode.constants.Poses;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
import java.util.Objects;
public class Shooter implements Subsystem {
private final DcMotorEx fly1;
private final DcMotorEx fly2;
private final DcMotorEx encoder;
private final Servo hoodServo;
private final Servo turret1;
private final Servo turret2;
private final MultipleTelemetry telemetry;
private boolean telemetryOn = false;
private double manualPower = 0.0;
private double hoodPos = 0.0;
private double turretPos = 0.0;
private double velocity = 0.0;
private double posPower = 0.0;
private int targetPosition = 0;
private double p = 0.0003, i = 0, d = 0.00001;
private PIDController controller;
private String shooterMode = "AUTO";
private String turretMode = "AUTO";
public Shooter(Robot robot, MultipleTelemetry TELE) {
this.fly1 = robot.shooter1;
this.fly2 = robot.shooter2;
this.telemetry = TELE;
this.hoodServo = robot.hood;
// Reset encoders
fly1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
fly2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
controller = new PIDController(p, i, d);
controller.setPID(p, i, d);
this.turret1 = robot.turr1;
this.turret2 = robot.turr2;
this.encoder = robot.shooterEncoder;
}
public void telemetryUpdate() {
// Telemetry
telemetry.addData("Mode", shooterMode);
telemetry.addData("ManualPower", manualPower);
telemetry.addData("Position", getPosition());
telemetry.addData("TargetPosition", targetPosition);
telemetry.addData("Velocity", getVelocity());
telemetry.addData("TargetVelocity", velocity);
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));
}
public double gethoodPosition() {
return (hoodServo.getPosition());
}
public void sethoodPosition(double pos) {hoodPos = pos;}
public double getTurretPosition() {
return ((turret1.getPosition()+ (1-turret2.getPosition()))/2);
}
public void setTurretPosition(double pos) {turretPos = pos;}
public double getVelocity() {
return ((double) ((fly1.getVelocity(AngleUnit.DEGREES) + fly2.getVelocity(AngleUnit.DEGREES)) /2));
}
public void setVelocity(double vel){velocity = vel;}
public void setPosPower(double power){posPower = power;}
public void setTargetPosition(int pos){
targetPosition = pos;
}
public void setTolerance(int tolerance){
controller.setTolerance(tolerance);
}
public void setControllerCoefficients(double kp, double ki, double kd){
p = kp;
i = ki;
d = kd;
controller.setPID(p, i, d);
}
public PIDCoefficients getControllerCoefficients(){
return new PIDCoefficients(p, i, d);
}
public void setManualPower(double power){manualPower = power;}
public String getShooterMode(){return shooterMode;}
public String getTurretMode(){return turretMode;}
public double getPosition(){
return ((double) ((fly1.getCurrentPosition() + fly2.getCurrentPosition()) /2));
}
public void setShooterMode(String mode){ shooterMode = mode;}
public void setTurretMode(String mode){ turretMode = mode;}
public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset){
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
Pose2d deltaPose = new Pose2d(
goalPose.position.x - robotPose.position.x,
goalPose.position.y - robotPose.position.y,
goalPose.heading.toDouble() - (robotPose.heading.toDouble())
);
double distance = Math.sqrt(
deltaPose.position.x * deltaPose.position.x
+ deltaPose.position.y * deltaPose.position.y
+ Poses.relativeGoalHeight * Poses.relativeGoalHeight
);
telemetry.addData("dst", distance);
double shooterPow = getPowerByDist(distance);
double hoodAngle = getAngleByDist(distance);
// hoodServo.setPosition(hoodAngle);
moveTurret(getTurretPosByDeltaPose(deltaPose, offset));
return distance;
//0.9974 * 355
}
public double getTurretPosByDeltaPose (Pose2d dPose, double offset){
double deltaAngle = Math.toDegrees(dPose.heading.toDouble());
double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y/dPose.position.x));
telemetry.addData("deltaAngle", deltaAngle);
if (deltaAngle > 90) {
deltaAngle -=360;
}
// deltaAngle += aTanAngle;
deltaAngle /= (335);
telemetry.addData("dAngle", deltaAngle);
telemetry.addData("AtanAngle", aTanAngle);
return ((0.30-deltaAngle) + offset);
}
//62, 0.44
//56.5, 0.5
public double getPowerByDist(double dist){
//TODO: ADD LOGIC
return dist;
}
public double getAngleByDist(double dist){
double newDist = dist - 56.5;
double pos = newDist*((0.44-0.5)/(62-56.5)) + 0.46;
return pos;
}
public void setTelemetryOn(boolean state){telemetryOn = state;}
public void moveTurret(double pos){
turret1.setPosition(pos);
turret2.setPosition(1-pos);
}
@Override
public void update() {
if (Objects.equals(shooterMode, "MANUAL")){
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly1.setPower(manualPower);
fly2.setPower(manualPower);
}
else if (Objects.equals(shooterMode, "VEL")){
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly1.setVelocity(velocity);
fly2.setPower(fly1.getPower());
}
else if (Objects.equals(shooterMode, "POS")){
double powPID = controller.calculate(getPosition(), targetPosition);
fly1.setPower(powPID);
fly2.setPower(powPID);
}
if (Objects.equals(turretMode, "MANUAL")){
// hoodServo.setPosition(hoodPos);
moveTurret(turretPos);
}
if (telemetryOn) {telemetryUpdate();}
}
}

257
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java
View File

@@ -1,257 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.DigitalChannel;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.utils.Robot;
import java.util.ArrayList;
public class Spindexer implements Subsystem{
private Servo s1;
private Servo s2;
private DigitalChannel p0;
private DigitalChannel p1;
private DigitalChannel p2;
private DigitalChannel p3;
private DigitalChannel p4;
private DigitalChannel p5;
private AnalogInput input;
private AnalogInput input2;
private MultipleTelemetry TELE;
private double position = 0.501;
private boolean telemetryOn = false;
private boolean ball0 = false;
private boolean ball1 = false;
private boolean ball2 = false;
private boolean green0 = false;
private boolean green1 = false;
private boolean green2 = false;
public Spindexer (Robot robot, MultipleTelemetry tele){
this.s1 = robot.spin1;
this.s2 = robot.spin2;
this.p0 = robot.pin0;
this.p1 = robot.pin1;
this.p2 = robot.pin2;
this.p3 = robot.pin3;
this.p4 = robot.pin4;
this.p5 = robot.pin5;
this.input = robot.analogInput;
this.input2 = robot.analogInput2;
this.TELE = tele;
}
public void setTelemetryOn(boolean state){
telemetryOn = state;
}
public void colorSensorTelemetry() {
TELE.addData("ball0", ball0);
TELE.addData("ball1", ball1);
TELE.addData("ball2", ball2);
TELE.addData("green0", green0);
TELE.addData("green1", green1);
TELE.addData("green2", green2);
}
public void checkForBalls() {
if (p0.getState()){
ball0 = true;
green0 = p1.getState();
} else {
ball0 = false;
}
if (p2.getState()){
ball1 = true;
green1 = p3.getState();
} else {
ball1 = false;
}
if (p4.getState()){
ball2 = true;
green2 = p5.getState();
} else {
ball2 = false;
}
}
public void setPosition (double pos) {
position = pos;
}
public void intake () {
position = spindexer_intakePos;
}
public void intakeShake(double runtime) {
if ((runtime % 0.25) >0.125) {
position = spindexer_intakePos + 0.04;
} else {
position = spindexer_intakePos - 0.04;
}
}
public void outtake3Shake(double runtime) {
if ((runtime % 0.25) >0.125) {
position = spindexer_outtakeBall3 + 0.04;
} else {
position = spindexer_outtakeBall3 - 0.04;
}
}
public void outtake3 () {
position = spindexer_outtakeBall3;
}
public void outtake2 () {
position = spindexer_outtakeBall2;
}
public void outtake1 () {
position = spindexer_outtakeBall1;
}
public int outtakeGreen(int secLast, int Last) {
if (green2 && (secLast!=3) && (Last!=3)) {
outtake3();
return 3;
} else if (green1 && (secLast!=2) && (Last!=2)){
outtake2();
return 2;
} else if (green0 && (secLast!=1) && (Last!=1)) {
outtake1();
return 1;
} else {
if (secLast!=1 && Last!= 1){
outtake1();
return 1;
} else if (secLast!=2 && Last!=2){
outtake2();
return 2;
} else {
outtake3();
return 3;
}
}
}
public void outtakeGreenFs() {
if (green0 && ball0) {
outtake1();
} else if (green1 && ball1){
outtake2();
} else if (green2 && ball2) {
outtake3();
}
}
public int greens() {
int num = 0;
if (green0){num++;}
if (green1){num++;}
if (green2){num++;}
return num;
}
public int outtakePurple(int secLast, int Last) {
if (!green2 && (secLast!=3) && (Last!=3)) {
outtake3();
return 3;
} else if (!green1 && (secLast!=2) && (Last!=2)){
outtake2();
return 2;
} else if (!green0 && (secLast!=1) && (Last!=1)) {
outtake1();
return 1;
} else {
if (secLast!=1 && Last!= 1){
outtake1();
return 1;
} else if (secLast!=2 && Last!=2){
outtake2();
return 2;
} else {
outtake3();
return 3;
}
}
}
@Override
public void update() {
if (position !=0.501) {
s1.setPosition(position);
s2.setPosition(1 - position);
}
if (telemetryOn) {
colorSensorTelemetry();
}
}
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.java
View File

@@ -1,6 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystems;
public interface Subsystem {
public void update();
}

58
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java
View File

@@ -1,58 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.utils.Robot;
public class Transfer implements Subsystem{
private final Servo servo;
private final DcMotorEx transfer;
private double motorPow = 0.0;
private double servoPos = 0.501;
public Transfer (Robot robot){
this.servo = robot.transferServo;
this.transfer = robot.transfer;
}
public void setTransferPosition(double pos){
this.servoPos = pos;
}
public void setTransferPower (double pow){
this.motorPow = pow;
}
public void transferOut(){
this.setTransferPosition(transferServo_out);
}
public void transferIn(){
this.setTransferPosition(transferServo_in);
}
@Override
public void update() {
if (servoPos!=0.501){
servo.setPosition(servoPos);
}
transfer.setPower(motorPow);
}
}

80
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/HoldTest.java
View File

@@ -1,80 +0,0 @@
package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Poses.h1;
import static org.firstinspires.ftc.teamcode.constants.Poses.x1;
import static org.firstinspires.ftc.teamcode.constants.Poses.y1;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.ProfileAccelConstraint;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TranslationalVelConstraint;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.acmerobotics.roadrunner.ftc.PinpointIMU;
import com.pedropathing.ftc.localization.localizers.PinpointLocalizer;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Robot;
@TeleOp
@Config
public class HoldTest extends LinearOpMode {
Robot robot;
MecanumDrive drive;
public TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200);
public ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200);
@Override
public void runOpMode() throws InterruptedException {
MultipleTelemetry TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
robot = new Robot(hardwareMap);
drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(0.01, 0.01), 0, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
waitForStart();
while (opModeIsActive()){
drive.updatePoseEstimate();
Pose2d currentPos = drive.localizer.getPose();
TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos)
.strafeToLinearHeading(new Vector2d(0, 0), 0, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
if (Math.abs(currentPos.position.x) >0.5 || Math.abs(currentPos.position.y)>0.5) {
Actions.runBlocking(
traj2.build()
);
}
}
}
}

901
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java
View File

@@ -1,901 +0,0 @@
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 com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.arcrobotics.ftclib.gamepad.ToggleButtonReader;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
import org.firstinspires.ftc.teamcode.subsystems.Intake;
import org.firstinspires.ftc.teamcode.subsystems.Shooter;
import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
import org.firstinspires.ftc.teamcode.subsystems.Transfer;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
@TeleOp
public class TeleopV1 extends LinearOpMode {
Robot robot;
Drivetrain drivetrain;
Intake intake;
Spindexer spindexer;
Transfer transfer;
MultipleTelemetry TELE;
GamepadEx g1;
GamepadEx g2;
public static double defaultSpeed = 1;
public static double slowMoSpeed = 0.4;
public static double power = 0.0;
public static double pos = hoodDefault;
public boolean all = false;
public int ticker = 0;
ToggleButtonReader g1RightBumper;
ToggleButtonReader g2Circle;
ToggleButtonReader g2Square;
ToggleButtonReader g2Triangle;
ToggleButtonReader g2RightBumper;
ToggleButtonReader g1LeftBumper;
ToggleButtonReader g2LeftBumper;
ToggleButtonReader g2DpadUp;
ToggleButtonReader g2DpadDown;
ToggleButtonReader g2DpadRight;
ToggleButtonReader g2DpadLeft;
public boolean leftBumper = false;
public double g1RightBumperStamp = 0.0;
public double g1LeftBumperStamp = 0.0;
public double g2LeftBumperStamp = 0.0;
public static int spindexerPos = 0;
public boolean green = false;
Shooter shooter;
public boolean scoreAll = false;
MecanumDrive drive ;
public boolean autotrack = false;
public int last = 0;
public int second = 0;
public double offset = 0.0;
public static double rIn = 0.59;
public static double rOut = 0;
public boolean notShooting = true;
public boolean circle = false;
public boolean square = false;
public boolean tri = false;
@Override
public void runOpMode() throws InterruptedException {
drive = new MecanumDrive(hardwareMap, teleStart);
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(
FtcDashboard.getInstance().getTelemetry(),
telemetry
);
g1 = new GamepadEx(gamepad1);
g1RightBumper = new ToggleButtonReader(
g1, GamepadKeys.Button.RIGHT_BUMPER
);
g2 = new GamepadEx(gamepad2);
g1LeftBumper = new ToggleButtonReader(
g1, GamepadKeys.Button.LEFT_BUMPER
);
g2Circle = new ToggleButtonReader(
g2, GamepadKeys.Button.B
);
g2Triangle = new ToggleButtonReader(
g2, GamepadKeys.Button.Y
);
g2Square = new ToggleButtonReader(
g2, GamepadKeys.Button.X
);
g2RightBumper = new ToggleButtonReader(
g2, GamepadKeys.Button.RIGHT_BUMPER
);
g2LeftBumper = new ToggleButtonReader(
g2, GamepadKeys.Button.LEFT_BUMPER
);
g2DpadUp = new ToggleButtonReader(
g2, GamepadKeys.Button.DPAD_UP
);
g2DpadDown = new ToggleButtonReader(
g2, GamepadKeys.Button.DPAD_DOWN
);
g2DpadLeft = new ToggleButtonReader(
g2, GamepadKeys.Button.DPAD_LEFT
);
g2DpadRight = new ToggleButtonReader(
g2, GamepadKeys.Button.DPAD_RIGHT
);
drivetrain = new Drivetrain(robot, TELE, g1);
drivetrain.setMode("Default");
drivetrain.setDefaultSpeed(defaultSpeed);
drivetrain.setSlowSpeed(slowMoSpeed);
intake = new Intake(robot);
transfer = new Transfer(robot);
spindexer = new Spindexer(robot, TELE);
spindexer.setTelemetryOn(true);
shooter = new Shooter(robot, TELE);
shooter.setShooterMode("MANUAL");
robot.rejecter.setPosition(rIn);
waitForStart();
if (isStopRequested()) return;
drive = new MecanumDrive(hardwareMap, teleStart);
while(opModeIsActive()){
drive.updatePoseEstimate();
TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
TELE.addData("off", offset);
robot.hood.setPosition(pos);
g1LeftBumper.readValue();
if (g1LeftBumper.wasJustPressed()){
g2LeftBumperStamp = getRuntime();
spindexer.intakeShake(getRuntime());
leftBumper = true;
}
if (leftBumper){
double time = getRuntime() - g2LeftBumperStamp;
if (time < 1.0){
robot.rejecter.setPosition(rOut);
} else {
robot.rejecter.setPosition(rIn);
}
}
intake();
drivetrain.update();
TELE.update();
transfer.update();
g2RightBumper.readValue();
g2LeftBumper.readValue();
g2DpadDown.readValue();
g2DpadUp.readValue();
if (!scoreAll){
spindexer.checkForBalls();
}
if(g2DpadUp.wasJustPressed()){
pos -=0.02;
}
if(g2DpadDown.wasJustPressed()){
pos +=0.02;
}
g2DpadLeft.readValue();
g2DpadRight.readValue();
if(g2DpadLeft.wasJustPressed()){
offset -=0.02;
}
if(g2DpadRight.wasJustPressed()){
offset +=0.02;
}
TELE.addData("hood", pos);
if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) {
shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset);
} else {
autotrack = false;
shooter.moveTurret(0.3+offset);
}
if (gamepad2.right_stick_button){
autotrack = true;
}
if (g2RightBumper.wasJustPressed()){
transfer.setTransferPower(1);
transfer.transferIn();
shooter.setManualPower(1);
notShooting = false;
}
if (g2RightBumper.wasJustReleased()){
transfer.setTransferPower(1);
transfer.transferOut();
}
if (gamepad2.left_stick_y>0.5){
shooter.setManualPower(0);
} else if (gamepad2.left_stick_y<-0.5){
shooter.setManualPower(1);
}
if (g2LeftBumper.wasJustPressed()){
g2LeftBumperStamp = getRuntime();
notShooting = false;
scoreAll = true;
}
if (scoreAll) {
double time = getRuntime() - g2LeftBumperStamp;
shooter.setManualPower(1);
TELE.addData("greenImportant", green);
TELE.addData("last", last);
TELE.addData("2ndLast", second);
int numGreen = spindexer.greens();
if (square) {
if (time < 0.3) {
ticker = 0;
last = 0;
second = 0;
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 2) {
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakePurple(second, last);
second = last;
} else {
last = spindexer.outtakeGreen(second, last);
second = last;
}
}
second = last;
ticker++;
} else if (time < 2.5) {
ticker = 0;
second = last;
transfer.transferIn();
} else if (time < 4) {
transfer.transferOut();
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakeGreen(second, last);
} else {
last = spindexer.outtakePurple(second, last);
}
}
ticker++;
} else if (time < 4.5) {
ticker = 0;
transfer.transferIn();
} else if (time < 6) {
transfer.transferOut();
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakeGreen(second, last);
} else {
last = spindexer.outtakePurple(second, last);
}
}
ticker++;
} else if (time < 6.5) {
transfer.transferIn();
} else {
ticker = 0;
scoreAll = false;
transfer.transferOut();
shooter.setManualPower(0);
}
} else if (tri) {
if (time < 0.3) {
ticker = 0;
last = 0;
second = 0;
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 2) {
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakeGreen(second, last);
second = last;
} else {
last = spindexer.outtakePurple(second, last);
second = last;
}
}
second = last;
ticker++;
} else if (time < 2.5) {
ticker = 0;
second = last;
transfer.transferIn();
} else if (time < 4) {
transfer.transferOut();
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakePurple(second, last);
} else {
last = spindexer.outtakeGreen(second, last);
}
}
ticker++;
} else if (time < 4.5) {
ticker = 0;
transfer.transferIn();
} else if (time < 6) {
transfer.transferOut();
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakeGreen(second, last);
} else {
last = spindexer.outtakePurple(second, last);
}
}
ticker++;
} else if (time < 6.5) {
transfer.transferIn();
} else {
ticker = 0;
scoreAll = false;
transfer.transferOut();
shooter.setManualPower(0);
}
} else if (circle){
if (time < 0.3) {
ticker = 0;
last = 0;
second = 0;
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 2) {
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakeGreen(second, last);
second = last;
} else {
last = spindexer.outtakePurple(second, last);
second = last;
}
}
second = last;
ticker++;
} else if (time < 2.5) {
ticker = 0;
second = last;
transfer.transferIn();
} else if (time < 4) {
transfer.transferOut();
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakeGreen(second, last);
} else {
last = spindexer.outtakePurple(second, last);
}
}
ticker++;
} else if (time < 4.5) {
ticker = 0;
transfer.transferIn();
} else if (time < 6) {
transfer.transferOut();
if (ticker == 0) {
if (numGreen == 2) {
last = spindexer.outtakePurple(second, last);
} else {
last = spindexer.outtakeGreen(second, last);
}
}
ticker++;
} else if (time < 6.5) {
transfer.transferIn();
} else {
ticker = 0;
scoreAll = false;
transfer.transferOut();
shooter.setManualPower(0);
}
} else{
if (time < 0.3) {
ticker = 0;
last = 0;
second = 0;
if (gamepad2.right_trigger > 0.5) {
green = false;
all = gamepad2.left_trigger > 0.5;
} else if (gamepad2.left_trigger > 0.5) {
green = true;
all = false;
} else {
all = true;
}
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 2) {
if (ticker == 0) {
if (all) {
spindexer.outtake3();
last = 3;
second = 3;
} else if (green) {
last = spindexer.outtakeGreen(second, last);
second = last;
} else {
last = spindexer.outtakePurple(second, last);
second = last;
}
}
second = last;
ticker++;
} else if (time < 2.5) {
ticker = 0;
second = last;
if (gamepad2.right_trigger > 0.5) {
green = false;
all = gamepad2.left_trigger > 0.5;
} else if (gamepad2.left_trigger > 0.5) {
green = true;
all = false;
}
transfer.transferIn();
} else if (time < 4) {
transfer.transferOut();
if (ticker == 0) {
if (all) {
spindexer.outtake2();
last = 2;
} else if (green) {
last = spindexer.outtakeGreen(second, last);
} else {
last = spindexer.outtakePurple(second, last);
}
}
ticker++;
} else if (time < 4.5) {
ticker = 0;
if (gamepad2.right_trigger > 0.5) {
green = false;
all = gamepad2.left_trigger > 0.5;
} else if (gamepad2.left_trigger > 0.5) {
green = true;
all = false;
}
transfer.transferIn();
} else if (time < 6) {
transfer.transferOut();
if (ticker == 0) {
if (all) {
spindexer.outtake1();
} else if (green) {
last = spindexer.outtakeGreen(second, last);
} else {
last = spindexer.outtakePurple(second, last);
}
}
ticker++;
} else if (time < 6.5) {
transfer.transferIn();
} else {
ticker = 0;
scoreAll = false;
transfer.transferOut();
shooter.setManualPower(0);
}
}
}
shooter.update();
}
}
public void intake(){
g1RightBumper.readValue();
g2Circle.readValue();
g2Square.readValue();
g2Triangle.readValue();
if (g1RightBumper.wasJustPressed()){
notShooting = true;
if (getRuntime() - g1RightBumperStamp < 0.3){
intake.reverse();
} else {
intake.toggle();
}
if (intake.getIntakeState()==1){
shooter.setManualPower(0);
}
spindexer.intake();
transfer.transferOut();
g1RightBumperStamp = getRuntime();
}
if (intake.getIntakeState()==1 && notShooting) {
spindexer.intakeShake(getRuntime());
} else {
if (g2Circle.wasJustPressed()){
circle = true;
tri = false;
square = false;
}
if (g2Triangle.wasJustPressed()){
circle = false;
tri = true;
square = false;
}
if (g2Square.wasJustPressed()){
circle = false;
tri = false;
square = true;
}
if (gamepad2.x){
circle = false;
tri = false;
square = false;
}
}
intake.update();
spindexer.update();
}
}

35
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TransferTest.java
View File

@@ -1,35 +0,0 @@
package org.firstinspires.ftc.teamcode.teleop;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.subsystems.Transfer;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
@TeleOp
public class TransferTest extends LinearOpMode {
Robot robot;
Transfer transfer;
public static double pos = 0.40;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
transfer = new Transfer(robot);
waitForStart();
while (opModeIsActive()){
transfer.setTransferPosition(pos);
}
}
}

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/WebcamTest.java
View File

@@ -1,59 +0,0 @@
package org.firstinspires.ftc.teamcode.teleop;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
@TeleOp
@Config
public class WebcamTest extends LinearOpMode {
AprilTag webcam;
MultipleTelemetry TELE;
Robot robot;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
webcam = new AprilTag(robot, TELE);
webcam.turnTelemetryOn(true);
while(opModeInInit()){
webcam.initTelemetry();
TELE.update();
};
if(isStopRequested()) return;
while (opModeIsActive()){
webcam.update();
TELE.update();
}
}
}