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

Compare commits

base: KeshavAnandCode:daniel
Branches Tags
KeshavAnandCode:master KeshavAnandCode:daniel
...
compare: KeshavAnandCode:894a8d26fba081f4db02ec7ba79ccd683f753ab1
Branches Tags
KeshavAnandCode:master KeshavAnandCode:daniel

6 Commits
daniel ... 894a8d26fb

Author SHA1 Message Date
KeshavAnandCode
894a8d26fb Deleted files + Drivetrain.java 2025-11-24 17:19:18 -06:00
KeshavAnandCode
09d82c1e02 Merge branch 'master-backup' 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
 2025-11-24 17:10:21 -06:00
KeshavAnandCode
62b6d1cf81 Fix PID velocity 2025-11-24 17:01:02 -06:00
KeshavAnandCode
c7f9028011 Added April Tag Class and Working Example 2025-11-18 20:22:55 -06:00
KeshavAnandCode
6b17bd4d46 test 2025-11-18 19:16:32 -06:00
KeshavAnandCode
695361e95c Hold code added 2025-11-11 21:30:14 -06:00
21 changed files with 371 additions and 3124 deletions
Expand all files Collapse all files

13
TeamCode/build.gradle
View File

@@ -23,6 +23,19 @@ android {
} }
} }
repositories {
maven {
url = 'https://maven.brott.dev/'
}
}
dependencies { dependencies {
implementation project(':FtcRobotController') implementation project(':FtcRobotController')
implementation "com.acmerobotics.roadrunner:ftc:0.1.25"
implementation "com.acmerobotics.roadrunner:core:1.0.1"
implementation "com.acmerobotics.roadrunner:actions:1.0.1"
implementation "com.acmerobotics.dashboard:dashboard:0.5.1"
implementation 'org.ftclib.ftclib:core:2.1.1' // core
} }

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;
}

203
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md
View File

@@ -1,147 +1,130 @@
# Team FTC Git Workflow Guide ## TeamCode Module
Welcome!
## 1. Cloning the Repository This module, TeamCode, is the place where you will write/paste the code for your team's
robot controller App. This module is currently empty (a clean slate) but the
process for adding OpModes is straightforward.
1. Open a terminal (or the terminal inside Android Studio). ## Creating your own OpModes
2. Navigate to the folder where you want to keep the project.
3. Run:
```bash The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own.
git clone https://github.com/KeshavAnandCode/DecodeFTCMain.git
cd DecodeFTCMain
```
4. Verify your remotes: Sample opmodes exist in the FtcRobotController module.
To locate these samples, find the FtcRobotController module in the "Project/Android" tab.
```bash Expand the following tree elements:
git remote -v FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples
```
You should see: ### Naming of Samples
```
origin https://github.com/KeshavAnandCode/DecodeFTCMain.git (fetch)
origin https://github.com/KeshavAnandCode/DecodeFTCMain.git (push)
upstream https://github.com/FIRST-Tech-Challenge/FtcRobotController.git (fetch)
upstream https://github.com/FIRST-Tech-Challenge/FtcRobotController.git (push)
```
--- To gain a better understanding of how the samples are organized, and how to interpret the
naming system, it will help to understand the conventions that were used during their creation.
## 2. Keeping `master` Clean These conventions are described (in detail) in the sample_conventions.md file in this folder.
- `master` should only contain clean, tested code. To summarize: A range of different samples classes will reside in the java/external/samples.
- Nobody should ever code directly on `master`. The class names will follow a naming convention which indicates the purpose of each class.
- To stay up to date: The prefix of the name will be one of the following:
```bash Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure
git checkout master of a particular style of OpMode. These are bare bones examples.
git fetch upstream
git merge upstream/master
git push origin master
```
--- Sensor: This is a Sample OpMode that shows how to use a specific sensor.
It is not intended to drive a functioning robot, it is simply showing the minimal code
required to read and display the sensor values.
## 3. Creating a Feature Branch Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base.
It may be used to provide a common baseline driving OpMode, or
to demonstrate how a particular sensor or concept can be used to navigate.
Whenever you start a new task (feature, fix, experiment): Concept: This is a sample OpMode that illustrates performing a specific function or concept.
These may be complex, but their operation should be explained clearly in the comments,
or the comments should reference an external doc, guide or tutorial.
Each OpMode should try to only demonstrate a single concept so they are easy to
locate based on their name. These OpModes may not produce a drivable robot.
1. Update `master` (see above). After the prefix, other conventions will apply:
2. Create a new branch from `master`:
```bash * Sensor class names are constructed as: Sensor - Company - Type
git checkout master * Robot class names are constructed as: Robot - Mode - Action - OpModetype
git pull origin master * Concept class names are constructed as: Concept - Topic - OpModetype
git checkout -b feature/short-description
```
### Branch Naming Standard Once you are familiar with the range of samples available, you can choose one to be the
basis for your own robot. In all cases, the desired sample(s) needs to be copied into
your TeamCode module to be used.
Branches **must** follow the format: This is done inside Android Studio directly, using the following steps:
1) Locate the desired sample class in the Project/Android tree.
2) Right click on the sample class and select "Copy"
3) Expand the TeamCode/java folder
4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste"
5) You will be prompted for a class name for the copy.
Choose something meaningful based on the purpose of this class.
Start with a capital letter, and remember that there may be more similar classes later.
Once your copy has been created, you should prepare it for use on your robot.
This is done by adjusting the OpMode's name, and enabling it to be displayed on the
Driver Station's OpMode list.
Each OpMode sample class begins with several lines of code like the ones shown below:
``` ```
<type>/<short-description> @TeleOp(name="Template: Linear OpMode", group="Linear Opmode")
@Disabled
``` ```
Where `<type>` is one of: The name that will appear on the driver station's "opmode list" is defined by the code:
- `feature/` → new functionality ``name="Template: Linear OpMode"``
- `fix/` → bug fixes You can change what appears between the quotes to better describe your opmode.
- `experiment/` → prototypes or tests The "group=" portion of the code can be used to help organize your list of OpModes.
- `docs/` → documentation updates
- `chore/` → maintenance or cleanup
Examples: As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the
- `feature/autonomous-path` ``@Disabled`` annotation which has been included.
- `fix/motor-init` This line can simply be deleted , or commented out, to make the OpMode visible.
- `experiment/vision-test`
- `docs/setup-instructions`
- `chore/gradle-update`
**Rules for names:** ## ADVANCED Multi-Team App management: Cloning the TeamCode Module
- Use lowercase letters and hyphens (`-`) only.
- Keep it short but clear (3–5 words).
- One branch = one task. Never mix unrelated work.
--- In some situations, you have multiple teams in your club and you want them to all share
a common code organization, with each being able to *see* the others code but each having
their own team module with their own code that they maintain themselves.
## 4. Working on Your Branch In this situation, you might wish to clone the TeamCode module, once for each of these teams.
Each of the clones would then appear along side each other in the Android Studio module list,
together with the FtcRobotController module (and the original TeamCode module).
- Make changes in Android Studio. Selective Team phones can then be programmed by selecting the desired Module from the pulldown list
- Stage and commit your changes: prior to clicking to the green Run arrow.
```bash Warning: This is not for the inexperienced Software developer.
git add . You will need to be comfortable with File manipulations and managing Android Studio Modules.
git commit -m "short message about what changed" These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this.
```
- Push your branch to GitHub: Also.. Make a full project backup before you start this :)
```bash To clone TeamCode, do the following:
git push origin feature/short-description
```
--- Note: Some names start with "Team" and others start with "team". This is intentional.
## 5. Sharing Your Work 1) Using your operating system file management tools, copy the whole "TeamCode"
folder to a sibling folder with a corresponding new name, eg: "Team0417".
- Once your branch is ready: 2) In the new Team0417 folder, delete the TeamCode.iml file.
1. Open a Pull Request (PR) on GitHub to merge into `master`.
2. At least one teammate should review before merging.
--- 3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder
to a matching name with a lowercase 'team' eg: "team0417".
## 6. Branching Rules 4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that
contains
package="org.firstinspires.ftc.teamcode"
to be
package="org.firstinspires.ftc.team0417"
**Do:** 5) Add: include ':Team0417' to the "/settings.gradle" file.
- Always branch from `master`.
- Follow the naming standard exactly.
- Keep branches small and focused.
- Delete branches after they’re merged.
**Don’t:** 6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project""
- Don’t push commits directly to `master`.
- Don’t leave unfinished work on `master`.
- Don’t mix unrelated changes in one branch.
---
## 7. Example Workflow
```bash
# Get latest code
git checkout master
git fetch upstream
git merge upstream/master
git push origin master
# Start a new feature
git checkout -b feature/teleop-improvements
# Work on code, then commit
git add .
git commit -m "improved joystick scaling in TeleOp"
# Push branch
git push origin feature/teleop-improvements
```

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<>();
}
}

106
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagWebcam.java Normal file
View File

@@ -0,0 +1,106 @@
package org.firstinspires.ftc.teamcode.subsystems;
import android.util.Size;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
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.List;
public class AprilTagWebcam {
private AprilTagProcessor aprilTagProcessor;
private VisionPortal visionPortal;
private List<AprilTagDetection> detectedTags = new ArrayList<>();
private MultipleTelemetry telemetry;
public void init(Robot robot, MultipleTelemetry TELE){
this.telemetry = TELE;
aprilTagProcessor = new AprilTagProcessor.Builder()
.setDrawTagID(true)
.setDrawTagOutline(true)
.setDrawAxes(true)
.setDrawCubeProjection(true)
.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
.build();
VisionPortal.Builder builder = new VisionPortal.Builder();
builder.setCamera(robot.webcamName);
builder.setCameraResolution(new Size(640, 480));
builder.addProcessor(aprilTagProcessor);
visionPortal = builder.build();
}
public void update() {
detectedTags = aprilTagProcessor.getDetections();
}
public List<AprilTagDetection> getDetectedTags() {
return detectedTags;
}
public AprilTagDetection getTagById(int id){
for (AprilTagDetection detection : detectedTags) {
if (detection.id ==id){
return detection;
}
}
return null;
}
public void stop(){
if (visionPortal != null){
visionPortal.close();
}
}
//Helper Functions
public void displayDetectionTelemetry (AprilTagDetection detectedId){
if (detectedId ==null){return;}
if (detectedId.metadata != null) {
telemetry.addLine(String.format("\n==== (ID %d) %s", detectedId.id, detectedId.metadata.name));
telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detectedId.ftcPose.x, detectedId.ftcPose.y, detectedId.ftcPose.z));
telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detectedId.ftcPose.pitch, detectedId.ftcPose.roll, detectedId.ftcPose.yaw));
telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detectedId.ftcPose.range, detectedId.ftcPose.bearing, detectedId.ftcPose.elevation));
} else {
telemetry.addLine(String.format("\n==== (ID %d) Unknown", detectedId.id));
telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detectedId.center.x, detectedId.center.y));
}
}
public void displayAllTelemetry (){
if (detectedTags ==null){return;}
telemetry.addData("# AprilTags Detected", detectedTags.size());
for (AprilTagDetection detection : detectedTags) {
if (detection.metadata != null) {
telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation));
} else {
telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id));
telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y));
}
} // end for() loop
}
}

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

@@ -4,30 +4,24 @@ 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;
@@ -39,8 +33,6 @@ public class Drivetrain implements Subsystem {
this.TELE = tele; this.TELE = tele;
} }
public void setMode(String mode) { public void setMode(String mode) {
@@ -77,8 +69,6 @@ public class Drivetrain implements Subsystem {
} }
@Override
public void update() { public void update() {
if (Objects.equals(Mode, "Default")) { if (Objects.equals(Mode, "Default")) {

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);
}
}
}

25
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Robot.java Normal file
View File

@@ -0,0 +1,25 @@
package org.firstinspires.ftc.teamcode.subsystems;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
public class Robot {
public DcMotorEx shooter1;
public DcMotorEx shooter2;
public WebcamName webcamName;
public Robot(HardwareMap hardwareMap) {
webcamName = hardwareMap.get(WebcamName.class, "Webcam 1");
shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
}
}

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);
}
}

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();
}
}
}

37
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java Normal file
View File

@@ -0,0 +1,37 @@
package org.firstinspires.ftc.teamcode.tests;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.subsystems.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.subsystems.Robot;
@Config
@TeleOp
public class AprilTagWebcamExample extends OpMode {
MultipleTelemetry TELE;
AprilTagWebcam aprilTagWebcam = new AprilTagWebcam();
@Override
public void init() {
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
aprilTagWebcam.init(new Robot(hardwareMap), TELE);
}
@Override
public void loop() {
aprilTagWebcam.update();
aprilTagWebcam.displayAllTelemetry();
TELE.update();
}
}

128
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
View File

@@ -1,88 +1,108 @@
package org.firstinspires.ftc.teamcode.tests; package org.firstinspires.ftc.teamcode.tests;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.arcrobotics.ftclib.controller.PIDController;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.subsystems.Robot;
import org.firstinspires.ftc.teamcode.subsystems.Shooter;
@TeleOp
@Config @Config
@TeleOp
public class ShooterTest extends LinearOpMode { public class ShooterTest extends LinearOpMode {
public static int mode = 0;
public static double parameter = 0.0;
Robot robot; Robot robot;
private DcMotorEx leftShooter;
public static double pow = 0.0; private DcMotorEx rightShooter;
public static double vel = 0.0; private DcMotorEx encoder;
private double encoderRevolutions = 0.0;
public static int pos = 0; private double lastEncoderRevolutions = 0.0;
public static double posPower = 0.0; private double timeStamp = 0.0;
private double lastTimeStamp = 0.0;
public static double p = 0.000003, i = 0, d = 0.000001;
public static String flyMode = "MANUAL"; // --- CONSTANTS YOU TUNE ---
public static double MAX_RPM = 2500; // your measured max RPM
public static double kP = 0.01; // small proportional gain (tune this)
public static double maxStep = 0.2; // prevents sudden jumps
public static String turrMode = "MANUAL";
public static int posTolerance = 40;
public static double servoPosition = 0.501;
MultipleTelemetry TELE;
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
leftShooter = robot.shooter1;
rightShooter = robot.shooter2;
encoder = robot.shooter1;
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); MultipleTelemetry TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
Shooter shooter = new Shooter(robot, TELE);
shooter.setTelemetryOn(true);
shooter.setTurretMode(turrMode);
shooter.setShooterMode(flyMode);
shooter.setControllerCoefficients(p, i, d);
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
while (opModeIsActive()) { while (opModeIsActive()) {
shooter.setControllerCoefficients(p, i, d); double kF = 1.0 / MAX_RPM; // baseline feedforward
shooter.setTurretMode(turrMode);
shooter.setShooterMode(flyMode);
encoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
shooter.setManualPower(pow);
double velocity = -60*(encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp);
shooter.setVelocity(vel);
TELE.addLine("Mode: 0 = Manual, 1 = Vel, 2 = Pos");
shooter.setTargetPosition(pos); TELE.addLine("Parameter = pow, vel, or pos");
TELE.addData("leftShooterPower", leftShooter.getPower());
shooter.setTolerance(posTolerance); TELE.addData("rightShooterPower", rightShooter.getPower());
TELE.addData("shaftEncoderPos", encoderRevolutions);
shooter.setPosPower(posPower); TELE.addData("shaftEncoderVel", velocity);
if (servoPosition!=0.501) {shooter.sethoodPosition(servoPosition);} double velPID = 0.0;
if (mode == 0) {
rightShooter.setPower(parameter);
leftShooter.setPower(parameter);
} else if (mode == 1) {
// --- FEEDFORWARD BASE POWER ---
double feed = kF * parameter; // Example: vel=2500 → feed=0.5
// --- PROPORTIONAL CORRECTION ---
double error = parameter - velocity;
double correction = kP * error;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
velPID = feed + correction;
// clamp to allowed range
velPID = Math.max(0, Math.min(1, velPID));
rightShooter.setPower(velPID);
leftShooter.setPower(velPID);
}
lastTimeStamp = getRuntime();
lastEncoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
shooter.update();
TELE.update(); TELE.update();
@@ -90,7 +110,5 @@ public class ShooterTest extends LinearOpMode {
} }
} }
} }

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TrackingTest.java Normal file
View File

@@ -0,0 +1,4 @@
package org.firstinspires.ftc.teamcode.tests;
public class TrackingTest {
}