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

Compare commits

merge into: KeshavAnandCode:daniel
Branches Tags
KeshavAnandCode:master KeshavAnandCode:daniel
...
pull from: KeshavAnandCode:master
Branches Tags
KeshavAnandCode:master KeshavAnandCode:daniel

36 Commits
daniel ... master

Author SHA1 Message Date
KeshavAnandCode
d586e3b4df yayyyyy 2025-12-05 22:48:05 -06:00
KeshavAnandCode
2f5d4638ec Add coloooor sensooooooer!!!! 2025-12-05 21:57:23 -06:00
KeshavAnandCode
1642e161c5 fixed???? 2025-12-05 20:56:51 -06:00
KeshavAnandCode
46a565c2c8 Working hood angle regression 2025-12-05 20:46:52 -06:00
KeshavAnandCode
0838fc35f9 Merged all branches...thx Daniel for ur hard work 2025-12-05 18:40:57 -06:00
KeshavAnandCode
17643535ae Added placeholder for webcam logic 
Signed-off-by: KeshavAnandCode <keshavanandofficial@gmail.com>
 2025-12-05 17:08:39 +00:00
KeshavAnandCode
5d93e3fc59 Add aujto offset and made left bumper g2 a super key 
Signed-off-by: KeshavAnandCode <keshavanandofficial@gmail.com>
 2025-12-05 16:55:01 +00:00
KeshavAnandCode
fb8a4fae95 Update TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java 
helped manual turret/hood setting with left stick

Signed-off-by: KeshavAnandCode <keshavanandofficial@gmail.com>
 2025-12-05 16:48:15 +00:00
KeshavAnandCode
b68f7eb6e7 Update TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java 
fixed velocity function hopeuflly

Signed-off-by: KeshavAnandCode <keshavanandofficial@gmail.com>
 2025-12-05 16:38:00 +00:00
KeshavAnandCode
d1f658cb5b 12/4 2025-12-04 22:41:11 -06:00
KeshavAnandCode
263bd46320 Sattempt for spindexer 2025-12-03 21:07:44 -06:00
KeshavAnandCode
3f25463181 stash 2025-12-03 19:31:45 -06:00
KeshavAnandCode
705eee180f stash 2025-12-03 19:24:06 -06:00
KeshavAnandCode
ef08883014 update 2025-12-03 18:07:16 -06:00
KeshavAnandCode
335e62ee3c stash 2025-12-02 20:27:33 -06:00
KeshavAnandCode
cdec64eb8f stash 2025-12-02 20:21:19 -06:00
KeshavAnandCode
fba9c7b114 added PSP 2025-12-02 19:46:15 -06:00
KeshavAnandCode
873d0c5134 stash 2025-12-02 19:45:15 -06:00
KeshavAnandCode
55dbfaaa98 update 2025-12-01 21:43:03 -06:00
KeshavAnandCode
0752c7c5f5 oops...uujj 2025-12-01 19:58:12 -06:00
KeshavAnandCode
3440ff1783 oops...hehe 2025-11-30 19:34:12 -06:00
KeshavAnandCode
0c3fd6fc83 s 2025-11-30 19:22:25 -06:00
KeshavAnandCode
8686b79314 ok 2025-11-30 19:20:44 -06:00
KeshavAnandCode
03ae41c19b yES 2025-11-30 18:16:08 -06:00
KeshavAnandCode
e04c5fa830 stash 2025-11-30 17:31:37 -06:00
KeshavAnandCode
f9a220bf51 daniel files added 2025-11-30 16:59:23 -06:00
KeshavAnandCode
4b96775161 Telelop drivetrain 2025-11-30 16:55:20 -06:00
KeshavAnandCode
9a884885a9 ehh 2025-11-30 16:47:30 -06:00
KeshavAnandCode
36ac31b3ec Auto track implemented with tunable constants 2025-11-26 22:58:31 -06:00
KeshavAnandCode
a1585e605f Shooter Test 2025-11-25 15:54:15 -06:00
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
30 changed files with 3596 additions and 2432 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);
}
}
}

804
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java Normal file
View File

@@ -0,0 +1,804 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
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.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
@Autonomous
public class Blue_V2 extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
AprilTagWebcam aprilTag;
Flywheel flywheel;
double velo = 0.0;
double targetVelocity = 0.0;
public static double intake1Time = 6.5;
public static double intake2Time = 6.5;
public static double colorDetect = 3.0;
boolean gpp = false;
boolean pgp = false;
boolean ppg = false;
double powPID = 0.0;
int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
boolean spindexPosEqual(double spindexer) {
TELE.addData("Velocity", velo);
TELE.addLine("spindex equal");
TELE.update();
return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 &&
scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01);
}
public Action initShooter(int vel) {
return new Action() {
double initPos = 0.0;
double stamp = 0.0;
double stamp1 = 0.0;
double ticker = 0.0;
double stamp2 = 0.0;
double currentPos = 0.0;
boolean steady = false;
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp2 = getRuntime();
}
targetVelocity = (double) vel;
ticker++;
if (ticker % 16 == 0) {
stamp = getRuntime();
stamp1 = stamp;
}
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.transfer.setPower(1);
TELE.addData("Velocity", velo);
TELE.update();
if (vel < velo && getRuntime() - stamp2 > 3.0 && !steady){
steady = true;
stamp2 = getRuntime();
return true;
} else if (steady && getRuntime() - stamp2 > 1.5){
TELE.addData("Velocity", velo);
TELE.addLine("finished init");
TELE.update();
return false;
} else {
return true;
}
}
};
}
public Action steadyShooter(int vel, boolean last) {
return new Action() {
double stamp = 0.0;
boolean steady = false;
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
steady = flywheel.getSteady();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.transfer.setPower(1);
TELE.addData("Velocity", velo);
TELE.update();
if (last && !steady){
stamp = getRuntime();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
return false;
} else if (steady) {
stamp = getRuntime();
return true;
} else {
return true;
}
}
};
}
public Action Obelisk() {
return new Action() {
double stamp = getRuntime();
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if (aprilTag.getTagById(21) != null) {
gpp = true;
} else if (aprilTag.getTagById(22) != null) {
pgp = true;
} else if (aprilTag.getTagById(23) != null) {
ppg = true;
}
aprilTag.update();
TELE.addData("Velocity", velo);
TELE.addData("21", gpp);
TELE.addData("22", pgp);
TELE.addData("23", ppg);
TELE.update();
if (gpp || pgp || ppg){
robot.turr1.setPosition(turret_blue);
robot.turr2.setPosition(1 - turret_blue);
return false;
} else {
return true;
}
}
};
}
public Action spindex (double spindexer, double vel){
return new Action() {
double currentPos = 0.0;
double stamp = 0.0;
double initPos = 0.0;
double stamp1 = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
ticker++;
if (ticker % 8 == 0) {
currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
stamp = getRuntime();
velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
initPos = currentPos;
stamp1 = stamp;
}
if (vel - velo > 500 && ticker > 16) {
powPID = 1.0;
} else if (velo - vel > 500 && ticker > 16){
powPID = 0.0;
} else if (Math.abs(vel - velo) < 100 && ticker > 16){
double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
// --- PROPORTIONAL CORRECTION ---
double error = vel - velo;
double correction = kP * error;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
powPID = feed + correction;
// clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID));
}
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.spin1.setPosition(spindexer);
robot.spin2.setPosition(1-spindexer);
TELE.addData("Velocity", velo);
TELE.addLine("spindex");
TELE.update();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
return !spindexPosEqual(spindexer);
}
};
}
public Action Shoot(double vel) {
return new Action() {
double transferStamp = 0.0;
int ticker = 1;
boolean transferIn = false;
double currentPos = 0.0;
double stamp = 0.0;
double initPos = 0.0;
double stamp1 = 0.0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", velo);
TELE.addLine("shooting");
TELE.update();
if (ticker % 8 == 0) {
currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
stamp = getRuntime();
velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
initPos = currentPos;
stamp1 = stamp;
}
if (vel - velo > 500 && ticker > 16) {
powPID = 1.0;
} else if (velo - vel > 500 && ticker > 16){
powPID = 0.0;
} else if (Math.abs(vel - velo) < 100 && ticker > 16){
double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
// --- PROPORTIONAL CORRECTION ---
double error = vel - velo;
double correction = kP * error;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
powPID = feed + correction;
// clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID));
}
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (ticker == 1) {
transferStamp = getRuntime();
ticker++;
}
if (getRuntime() - transferStamp > waitTransfer && !transferIn) {
robot.transferServo.setPosition(transferServo_in);
TELE.addData("Velocity", velo);
TELE.addData("ticker", ticker);
TELE.update();
transferIn = true;
return true;
} else if (getRuntime() - transferStamp > waitTransfer+waitTransferOut && transferIn){
robot.transferServo.setPosition(transferServo_out);
TELE.addData("Velocity", velo);
TELE.addLine("shot once");
TELE.update();
return false;
} else {
return true;
}
}
};
}
public Action intake(double intakeTime) {
return new Action() {
double position = 0.0;
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM);
if ((getRuntime() % 0.3) > 0.15) {
position = spindexer_intakePos1 + 0.02;
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
TELE.addData("Velocity", velo);
TELE.addLine("Intaking");
TELE.update();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
robot.intake.setPower(1);
if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) {
robot.intake.setPower(0);
return false;
} else {
return true;
}
}
};
}
public Action ColorDetect() {
return new Action() {
double stamp = 0.0;
int ticker = 0;
double position = 0.0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if ((getRuntime() % 0.3) > 0.15) {
position = spindexer_intakePos1 + 0.02;
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (s1D < 40) {
double green = robot.color1.getNormalizedColors().green;
double red = robot.color1.getNormalizedColors().red;
double blue = robot.color1.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b1 = 2;
} else {
b1 = 1;
}
}
if (s2D < 40) {
double green = robot.color2.getNormalizedColors().green;
double red = robot.color2.getNormalizedColors().red;
double blue = robot.color2.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b2 = 2;
} else {
b2 = 1;
}
}
if (s3D < 30) {
double green = robot.color3.getNormalizedColors().green;
double red = robot.color3.getNormalizedColors().red;
double blue = robot.color3.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b3 = 2;
} else {
b3 = 1;
}
}
TELE.addData("Velocity", velo);
TELE.addLine("Detecting");
TELE.addData("Distance 1", s1D);
TELE.addData("Distance 2", s2D);
TELE.addData("Distance 3", s3D);
TELE.addData("B1", b1);
TELE.addData("B2", b2);
TELE.addData("B3", b3);
TELE.update();
return (b1 + b2 + b3 < 4) && !(getRuntime() - stamp > colorDetect);
}
};
}
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
flywheel = new Flywheel();
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
drive = new MecanumDrive(hardwareMap, new Pose2d(
0, 0, 0
));
aprilTag = new AprilTagWebcam();
TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
.strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a)
.strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b);
TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(bx2b, by2b, bh2b))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
.strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a)
.strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b);
TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b))
.strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
aprilTag.init(robot, TELE);
while (opModeInInit()) {
if (gamepad2.dpadUpWasPressed()) {
hoodAuto-= 0.01;
}
if (gamepad2.dpadDownWasPressed()) {
hoodAuto += 0.01;
}
robot.hood.setPosition(hoodAuto);
robot.turr1.setPosition(turret_detectBlue);
robot.turr2.setPosition(1 - turret_detectBlue);
robot.transferServo.setPosition(transferServo_out);
robot.spin1.setPosition(spindexer_intakePos1);
robot.spin2.setPosition(1 - spindexer_intakePos1);
aprilTag.update();
TELE.addData("Velocity", velo);
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
if (opModeIsActive()) {
robot.hood.setPosition(hoodAuto);
Actions.runBlocking(
new ParallelAction(
shoot0.build(),
initShooter(AUTO_CLOSE_VEL),
Obelisk()
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
shootingSequence();
robot.hood.setPosition(hoodAuto);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
pickup1.build(),
intake(intake1Time)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
shoot1.build(),
ColorDetect(),
steadyShooter(AUTO_CLOSE_VEL, true)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
shootingSequence();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
pickup2.build(),
intake(intake2Time)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
shoot2.build(),
ColorDetect(),
steadyShooter(AUTO_CLOSE_VEL, true)
)
);
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
shootingSequence();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", velo);
TELE.addLine("finished");
TELE.update();
sleep(2000);
}
}
public void shootingSequence() {
TELE.addData("Velocity", velo);
if (gpp) {
if (b1 + b2 + b3 == 4) {
if (b1 == 2 && b2 - b3 == 0) {
sequence1();
TELE.addLine("sequence1");
} else if (b2 == 2 && b1 - b3 == 0) {
sequence3();
TELE.addLine("sequence3");
} else if (b3 == 2 && b1 - b2 == 0) {
sequence6();
TELE.addLine("sequence6");
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (b1 + b2 + b3 >= 5) {
if (b1 == 2) {
sequence1();
TELE.addLine("sequence1");
} else if (b2 == 2) {
sequence3();
TELE.addLine("sequence3");
} else if (b3 == 2) {
sequence6();
TELE.addLine("sequence6");
}
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (pgp) {
if (b1 + b2 + b3 == 4) {
if (b1 == 2 && b2 - b3 == 0) {
sequence3();
TELE.addLine("sequence3");
} else if (b2 == 2 && b1 - b3 == 0) {
sequence1();
TELE.addLine("sequence1");
} else if (b3 == 2 && b1 - b2 == 0) {
sequence4();
TELE.addLine("sequence4");
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (b1 + b2 + b3 >= 5) {
if (b1 == 2) {
sequence3();
TELE.addLine("sequence3");
} else if (b2 == 2) {
sequence1();
TELE.addLine("sequence1");
} else if (b3 == 2) {
sequence4();
TELE.addLine("sequence4");
}
} else {
sequence3();
TELE.addLine("sequence3");
}
} else if (ppg) {
if (b1 + b2 + b3 == 4) {
if (b1 == 2 && b2 - b3 == 0) {
sequence6();
TELE.addLine("sequence6");
} else if (b2 == 2 && b1 - b3 == 0) {
sequence5();
TELE.addLine("sequence5");
} else if (b3 == 2 && b1 - b2 == 0) {
sequence1();
TELE.addLine("sequence1");
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (b1 + b2 + b3 >= 5) {
if (b1 == 2) {
sequence6();
TELE.addLine("sequence6");
} else if (b2 == 2) {
sequence5();
TELE.addLine("sequence5");
} else if (b3 == 2) {
sequence1();
TELE.addLine("sequence1");
}
} else {
sequence6();
TELE.addLine("sequence6");
}
} else {
sequence1();
TELE.addLine("sequence1");
}
TELE.update();
}
public void sequence1() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence2() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence3() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence4() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence5() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence6() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
}

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

804
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java Normal file
View File

@@ -0,0 +1,804 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
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.ParallelAction;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ftc.Actions;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
@Autonomous
public class Red_V2 extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
AprilTagWebcam aprilTag;
Flywheel flywheel;
double velo = 0.0;
double targetVelocity = 0.0;
public static double intake1Time = 6.5;
public static double intake2Time = 6.5;
public static double colorDetect = 3.0;
boolean gpp = false;
boolean pgp = false;
boolean ppg = false;
double powPID = 0.0;
int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
boolean spindexPosEqual(double spindexer) {
TELE.addData("Velocity", velo);
TELE.addLine("spindex equal");
TELE.update();
return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 &&
scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01);
}
public Action initShooter(int vel) {
return new Action() {
double initPos = 0.0;
double stamp = 0.0;
double stamp1 = 0.0;
double ticker = 0.0;
double stamp2 = 0.0;
double currentPos = 0.0;
boolean steady = false;
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp2 = getRuntime();
}
targetVelocity = (double) vel;
ticker++;
if (ticker % 16 == 0) {
stamp = getRuntime();
stamp1 = stamp;
}
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.transfer.setPower(1);
TELE.addData("Velocity", velo);
TELE.update();
if (vel < velo && getRuntime() - stamp2 > 3.0 && !steady){
steady = true;
stamp2 = getRuntime();
return true;
} else if (steady && getRuntime() - stamp2 > 1.5){
TELE.addData("Velocity", velo);
TELE.addLine("finished init");
TELE.update();
return false;
} else {
return true;
}
}
};
}
public Action steadyShooter(int vel, boolean last) {
return new Action() {
double stamp = 0.0;
boolean steady = false;
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
steady = flywheel.getSteady();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.transfer.setPower(1);
TELE.addData("Velocity", velo);
TELE.update();
if (last && !steady){
stamp = getRuntime();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
return false;
} else if (steady) {
stamp = getRuntime();
return true;
} else {
return true;
}
}
};
}
public Action Obelisk() {
return new Action() {
double stamp = getRuntime();
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if (aprilTag.getTagById(21) != null) {
gpp = true;
} else if (aprilTag.getTagById(22) != null) {
pgp = true;
} else if (aprilTag.getTagById(23) != null) {
ppg = true;
}
aprilTag.update();
TELE.addData("Velocity", velo);
TELE.addData("21", gpp);
TELE.addData("22", pgp);
TELE.addData("23", ppg);
TELE.update();
if (gpp || pgp || ppg){
robot.turr1.setPosition(turret_red);
robot.turr2.setPosition(1 - turret_red);
return false;
} else {
return true;
}
}
};
}
public Action spindex (double spindexer, double vel){
return new Action() {
double currentPos = 0.0;
double stamp = 0.0;
double initPos = 0.0;
double stamp1 = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
ticker++;
if (ticker % 8 == 0) {
currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
stamp = getRuntime();
velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
initPos = currentPos;
stamp1 = stamp;
}
if (vel - velo > 500 && ticker > 16) {
powPID = 1.0;
} else if (velo - vel > 500 && ticker > 16){
powPID = 0.0;
} else if (Math.abs(vel - velo) < 100 && ticker > 16){
double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
// --- PROPORTIONAL CORRECTION ---
double error = vel - velo;
double correction = kP * error;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
powPID = feed + correction;
// clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID));
}
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.spin1.setPosition(spindexer);
robot.spin2.setPosition(1-spindexer);
TELE.addData("Velocity", velo);
TELE.addLine("spindex");
TELE.update();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
return !spindexPosEqual(spindexer);
}
};
}
public Action Shoot(double vel) {
return new Action() {
double transferStamp = 0.0;
int ticker = 1;
boolean transferIn = false;
double currentPos = 0.0;
double stamp = 0.0;
double initPos = 0.0;
double stamp1 = 0.0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
TELE.addData("Velocity", velo);
TELE.addLine("shooting");
TELE.update();
if (ticker % 8 == 0) {
currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
stamp = getRuntime();
velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
initPos = currentPos;
stamp1 = stamp;
}
if (vel - velo > 500 && ticker > 16) {
powPID = 1.0;
} else if (velo - vel > 500 && ticker > 16){
powPID = 0.0;
} else if (Math.abs(vel - velo) < 100 && ticker > 16){
double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
// --- PROPORTIONAL CORRECTION ---
double error = vel - velo;
double correction = kP * error;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
powPID = feed + correction;
// clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID));
}
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (ticker == 1) {
transferStamp = getRuntime();
ticker++;
}
if (getRuntime() - transferStamp > waitTransfer && !transferIn) {
robot.transferServo.setPosition(transferServo_in);
TELE.addData("Velocity", velo);
TELE.addData("ticker", ticker);
TELE.update();
transferIn = true;
return true;
} else if (getRuntime() - transferStamp > waitTransfer+waitTransferOut && transferIn){
robot.transferServo.setPosition(transferServo_out);
TELE.addData("Velocity", velo);
TELE.addLine("shot once");
TELE.update();
return false;
} else {
return true;
}
}
};
}
public Action intake(double intakeTime) {
return new Action() {
double position = 0.0;
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM);
if ((getRuntime() % 0.3) > 0.15) {
position = spindexer_intakePos1 + 0.02;
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
TELE.addData("Velocity", velo);
TELE.addLine("Intaking");
TELE.update();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
robot.intake.setPower(1);
if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) {
robot.intake.setPower(0);
return false;
} else {
return true;
}
}
};
}
public Action ColorDetect() {
return new Action() {
double stamp = 0.0;
int ticker = 0;
double position = 0.0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if ((getRuntime() % 0.3) > 0.15) {
position = spindexer_intakePos1 + 0.02;
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (s1D < 40) {
double green = robot.color1.getNormalizedColors().green;
double red = robot.color1.getNormalizedColors().red;
double blue = robot.color1.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b1 = 2;
} else {
b1 = 1;
}
}
if (s2D < 40) {
double green = robot.color2.getNormalizedColors().green;
double red = robot.color2.getNormalizedColors().red;
double blue = robot.color2.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b2 = 2;
} else {
b2 = 1;
}
}
if (s3D < 30) {
double green = robot.color3.getNormalizedColors().green;
double red = robot.color3.getNormalizedColors().red;
double blue = robot.color3.getNormalizedColors().blue;
double gP = green / (green + red + blue);
if (gP >= 0.4) {
b3 = 2;
} else {
b3 = 1;
}
}
TELE.addData("Velocity", velo);
TELE.addLine("Detecting");
TELE.addData("Distance 1", s1D);
TELE.addData("Distance 2", s2D);
TELE.addData("Distance 3", s3D);
TELE.addData("B1", b1);
TELE.addData("B2", b2);
TELE.addData("B3", b3);
TELE.update();
return (b1 + b2 + b3 < 4) && !(getRuntime() - stamp > colorDetect);
}
};
}
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
flywheel = new Flywheel();
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
drive = new MecanumDrive(hardwareMap, new Pose2d(
0, 0, 0
));
aprilTag = new AprilTagWebcam();
TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1))
.strafeToLinearHeading(new Vector2d(rx2a, ry2a), rh2a)
.strafeToLinearHeading(new Vector2d(rx2b, ry2b), rh2b);
TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(rx2b, ry2b, rh2b))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1))
.strafeToLinearHeading(new Vector2d(rx3a, ry3a), rh3a)
.strafeToLinearHeading(new Vector2d(rx3b, ry3b), rh3b);
TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b))
.strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
aprilTag.init(robot, TELE);
while (opModeInInit()) {
if (gamepad2.dpadUpWasPressed()) {
hoodAuto-= 0.01;
}
if (gamepad2.dpadDownWasPressed()) {
hoodAuto += 0.01;
}
robot.hood.setPosition(hoodAuto);
robot.turr1.setPosition(turret_detectRed);
robot.turr2.setPosition(1 - turret_detectRed);
robot.transferServo.setPosition(transferServo_out);
robot.spin1.setPosition(spindexer_intakePos1);
robot.spin2.setPosition(1 - spindexer_intakePos1);
aprilTag.update();
TELE.addData("Velocity", velo);
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
if (opModeIsActive()) {
robot.hood.setPosition(hoodAuto);
Actions.runBlocking(
new ParallelAction(
shoot0.build(),
initShooter(AUTO_CLOSE_VEL),
Obelisk()
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
shootingSequence();
robot.hood.setPosition(hoodAuto);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
pickup1.build(),
intake(intake1Time)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
shoot1.build(),
ColorDetect(),
steadyShooter(AUTO_CLOSE_VEL, true)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
shootingSequence();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
pickup2.build(),
intake(intake2Time)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
shoot2.build(),
ColorDetect(),
steadyShooter(AUTO_CLOSE_VEL, true)
)
);
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
shootingSequence();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addData("Velocity", velo);
TELE.addLine("finished");
TELE.update();
sleep(2000);
}
}
public void shootingSequence() {
TELE.addData("Velocity", velo);
if (gpp) {
if (b1 + b2 + b3 == 4) {
if (b1 == 2 && b2 - b3 == 0) {
sequence1();
TELE.addLine("sequence1");
} else if (b2 == 2 && b1 - b3 == 0) {
sequence3();
TELE.addLine("sequence3");
} else if (b3 == 2 && b1 - b2 == 0) {
sequence6();
TELE.addLine("sequence6");
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (b1 + b2 + b3 >= 5) {
if (b1 == 2) {
sequence1();
TELE.addLine("sequence1");
} else if (b2 == 2) {
sequence3();
TELE.addLine("sequence3");
} else if (b3 == 2) {
sequence6();
TELE.addLine("sequence6");
}
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (pgp) {
if (b1 + b2 + b3 == 4) {
if (b1 == 2 && b2 - b3 == 0) {
sequence3();
TELE.addLine("sequence3");
} else if (b2 == 2 && b1 - b3 == 0) {
sequence1();
TELE.addLine("sequence1");
} else if (b3 == 2 && b1 - b2 == 0) {
sequence4();
TELE.addLine("sequence4");
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (b1 + b2 + b3 >= 5) {
if (b1 == 2) {
sequence3();
TELE.addLine("sequence3");
} else if (b2 == 2) {
sequence1();
TELE.addLine("sequence1");
} else if (b3 == 2) {
sequence4();
TELE.addLine("sequence4");
}
} else {
sequence3();
TELE.addLine("sequence3");
}
} else if (ppg) {
if (b1 + b2 + b3 == 4) {
if (b1 == 2 && b2 - b3 == 0) {
sequence6();
TELE.addLine("sequence6");
} else if (b2 == 2 && b1 - b3 == 0) {
sequence5();
TELE.addLine("sequence5");
} else if (b3 == 2 && b1 - b2 == 0) {
sequence1();
TELE.addLine("sequence1");
} else {
sequence1();
TELE.addLine("sequence1");
}
} else if (b1 + b2 + b3 >= 5) {
if (b1 == 2) {
sequence6();
TELE.addLine("sequence6");
} else if (b2 == 2) {
sequence5();
TELE.addLine("sequence5");
} else if (b3 == 2) {
sequence1();
TELE.addLine("sequence1");
}
} else {
sequence6();
TELE.addLine("sequence6");
}
} else {
sequence1();
TELE.addLine("sequence1");
}
TELE.update();
}
public void sequence1() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence2() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence3() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence4() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence5() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
public void sequence6() {
Actions.runBlocking(
new SequentialAction(
spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL),
spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL),
Shoot(AUTO_CLOSE_VEL)
)
);
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java Normal file
View File

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

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

@@ -1,6 +1,5 @@
package org.firstinspires.ftc.teamcode.constants; package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.Pose2d;
@@ -15,15 +14,38 @@ public class Poses {
public static double x1 = 50, y1 = 0, h1 = 0; public static double x1 = 50, y1 = 0, h1 = 0;
public static double x2 = 31, y2 = 32, h2 = Math.toRadians(135); public static double x2 = 31, y2 = 32, h2 = Math.toRadians(140);
public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(135); public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(140);
public static double x3 = 34, y3 = 58, h3 = Math.toRadians(140);
public static double tx = 0, ty = 0, th = 0;
public static Pose2d goalPose = new Pose2d(-15, 0, 0);
public static double x3 = 34, y3 = 58, h3 = Math.toRadians(135);
public static Pose2d teleStart = new Pose2d(x1,-10,0); public static double rx1 = 46, ry1 = -4, rh1 = 0;
public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140);
public static double rx2b = 31, ry2b = 32, rh2b = Math.toRadians(140);
public static double rx3a = 58, ry3a = 42, rh3a = Math.toRadians(140);
public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140);
public static double bx1 = 46, by1 = 4, bh1 = 0;
public static double bx2a = 45, by2a = -5, bh2a = Math.toRadians(-140);
public static double bx2b = 31, by2b = -32, bh2b = Math.toRadians(-140);
public static double bx3a = 58, by3a = -42, bh3a = Math.toRadians(-140);
public static double bx3b = 34, by3b = -58, bh3b = Math.toRadians(-140);
public static Pose2d teleStart = new Pose2d(rx1, 10, 0);
} }

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

@@ -1,23 +1,41 @@
package org.firstinspires.ftc.teamcode.constants; package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
@Config @Config
public class ServoPositions { public class ServoPositions {
public static double spindexer_intakePos = 0.665; public static double spindexer_intakePos1 = 0.665;
public static double spindexer_intakePos3 = 0.29;
public static double spindexer_intakePos2 = 0.99;
public static double spindexer_outtakeBall3 = 0.845; public static double spindexer_outtakeBall3 = 0.845;
public static double spindexer_outtakeBall2 = 0.48; public static double spindexer_outtakeBall2 = 0.48;
public static double spindexer_outtakeBall1 = 0.1; public static double spindexer_outtakeBall1 = 0.1;
public static double transferServo_out = 0.13; public static double transferServo_out = 0.15;
public static double transferServo_in = 0.4; public static double transferServo_in = 0.38;
public static double hoodDefault = 0.35; public static double turret_range = 0.9;
public static double hoodDefault = 0.6;
public static double hoodAuto = 0.59;
public static double hoodHigh = 0.21;
public static double hoodLow = 1.0;
public static double turret_red = 0.4;
public static double turret_blue = 0.38;
public static double turret_detectRed = 0.2;
public static double turret_detectBlue = 0.6;
public static double turrDefault = 0.40;
} }

23
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java Normal file
View File

@@ -0,0 +1,23 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
@Config
public class ShooterVars {
public static double turret_GearRatio = 0.9974;
public static double turret_Range = 355;
public static int velTolerance = 300;
public static int initTolerance = 1000;
public static int maxVel = 4500;
public static double waitTransferOut = 0.3;
public static double waitTransfer = 0.4;
public static double kP = 0.001; // small proportional gain (tune this)
public static double maxStep = 0.06; // prevents sudden jumps
// VELOCITY CONSTANTS
public static int AUTO_CLOSE_VEL = 3025; //3300;
}

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

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

@@ -1,99 +0,0 @@
package org.firstinspires.ftc.teamcode.subsystems;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
import java.util.Objects;
public class Drivetrain implements Subsystem {
private GamepadEx gamepad;
public MultipleTelemetry TELE;
private String Mode = "Default";
private DcMotorEx fl;
private DcMotorEx fr;
private DcMotorEx bl;
private DcMotorEx br;
private double defaultSpeed = 0.7;
private double slowSpeed = 0.3;
public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1){
this.fl = robot.frontLeft;
this.fr = robot.frontRight;
this.br = robot.backRight;
this.bl = robot.backLeft;
this.gamepad = gamepad1;
this.TELE = tele;
}
public void setMode (String mode){
this.Mode = mode;
}
public void setDefaultSpeed (double speed){
this.defaultSpeed = speed;
}
public void setSlowSpeed (double speed){
this.slowSpeed = speed;
}
public void RobotCentric(double fwd, double strafe, double turn, double turbo){
double y = -fwd; // Remember, Y stick value is reversed
double x = strafe * 1.1; // Counteract imperfect strafing
double rx = turn;
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio,
// but only if at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
fl.setPower(frontLeftPower*turbo);
bl.setPower(backLeftPower*turbo);
fr.setPower(frontRightPower*turbo);
br.setPower(backRightPower*turbo);
}
@Override
public void update() {
if (Objects.equals(Mode, "Default")) {
RobotCentric(
gamepad.getRightY(),
gamepad.getRightX(),
gamepad.getLeftX(),
(gamepad.getTrigger(
GamepadKeys.Trigger.RIGHT_TRIGGER) * (1-defaultSpeed)
- gamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) * slowSpeed
+ defaultSpeed
)
);
}
}
}

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

949
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java Normal file
View File

@@ -0,0 +1,949 @@
package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.turrDefault;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransfer;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransferOut;
import static org.firstinspires.ftc.teamcode.tests.ShooterTest.kP;
import static org.firstinspires.ftc.teamcode.tests.ShooterTest.maxStep;
import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.restPos;
import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.scalar;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import java.util.ArrayList;
import java.util.List;
@TeleOp
@Config
public class TeleopV2 extends LinearOpMode {
public static double manualVel = 3000;
public static double hood = 0.5;
public static double hoodDefaultPos = 0.5;
public static double desiredTurretAngle = 180;
public static double velMultiplier = 20;
public static double shootStamp2 = 0.0;
public double vel = 3000;
public boolean autoVel = true;
public double manualOffset = 0.0;
public boolean autoHood = true;
public boolean green1 = false;
public boolean green2 = false;
public boolean green3 = false;
public double shootStamp = 0.0;
public boolean circle = false;
public boolean square = false;
public boolean triangle = false;
double autoHoodOffset = 0.0;
Robot robot;
MultipleTelemetry TELE;
boolean intake = false;
boolean reject = false;
double xOffset = 0.0;
double yOffset = 0.0;
double headingOffset = 0.0;
int ticker = 0;
List<Double> s1G = new ArrayList<>();
List<Double> s2G = new ArrayList<>();
List<Double> s3G = new ArrayList<>();
List<Double> s1T = new ArrayList<>();
List<Double> s2T = new ArrayList<>();
List<Double> s3T = new ArrayList<>();
List<Boolean> s1 = new ArrayList<>();
List<Boolean> s2 = new ArrayList<>();
List<Boolean> s3 = new ArrayList<>();
boolean oddBallColor = false;
AprilTagWebcam aprilTagWebcam = new AprilTagWebcam();
MecanumDrive drive;
double hoodOffset = 0.0;
boolean shoot1 = false;
// Make these class-level flags
boolean shootA = true;
boolean shootB = true;
boolean shootC = true;
boolean manualTurret = false;
boolean outtake1 = false;
boolean outtake2 = false;
boolean outtake3 = false;
boolean overrideTurr = false;
List<Integer> shootOrder = new ArrayList<>();
boolean emergency = false;
private double lastEncoderRevolutions = 0.0;
private double lastTimeStamp = 0.0;
private double velo1, velo;
private double stamp1, stamp, initPos;
private boolean shootAll = false;
private double transferStamp = 0.0;
private int tickerA = 1;
private boolean transferIn = false;
public static double velPrediction(double distance) {
if (distance < 30) {
return 2750;
} else if (distance > 100) {
if (distance > 160) {
return 4200;
}
return 3700;
} else {
// linear interpolation between 40->2650 and 120->3600
double slope = (3700.0 - 2750.0) / (100.0 - 30);
return (int) Math.round(2750 + slope * (distance - 30));
}
}
@Override
public void runOpMode() throws InterruptedException {
List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
for (LynxModule hub : allHubs) {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
}
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
drive = new MecanumDrive(hardwareMap, teleStart);
Pose2d shootPos = teleStart;
aprilTagWebcam.init(new Robot(hardwareMap), TELE);
robot.turr1.setPosition(0.4);
robot.turr2.setPosition(1 - 0.4);
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
//DRIVETRAIN:
double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed
double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
double rx = gamepad1.left_stick_x;
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
robot.frontLeft.setPower(frontLeftPower);
robot.backLeft.setPower(backLeftPower);
robot.frontRight.setPower(frontRightPower);
robot.backRight.setPower(backRightPower);
//INTAKE:
if (gamepad1.rightBumperWasPressed()) {
intake = !intake;
reject = false;
shootAll = false;
emergency = false;
overrideTurr = false;
}
if (gamepad1.leftBumperWasPressed()) {
intake = false;
reject = true;
shootAll = false;
overrideTurr = false;
}
if (intake) {
robot.transferServo.setPosition(transferServo_out);
robot.intake.setPower(1);
double position;
if ((getRuntime() % 0.3) > 0.15) {
position = spindexer_intakePos1 + 0.015;
} else {
position = spindexer_intakePos1 - 0.015;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
} else if (reject) {
robot.intake.setPower(-1);
double position = spindexer_intakePos1;
robot.spin1.setPosition(position);
robot.spin2.setPosition(1 - position);
} else {
robot.intake.setPower(0);
}
//COLOR:
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM);
if (s1D < 40) {
double green = robot.color1.getNormalizedColors().green;
double red = robot.color1.getNormalizedColors().red;
double blue = robot.color1.getNormalizedColors().blue;
double gP = green / (green + red + blue);
s1G.add(gP);
if (gP >= 0.43) {
s1.add(true);
} else {
s1.add(false);
}
s1T.add(getRuntime());
}
if (s2D < 40) {
double green = robot.color2.getNormalizedColors().green;
double red = robot.color2.getNormalizedColors().red;
double blue = robot.color2.getNormalizedColors().blue;
double gP = green / (green + red + blue);
s2G.add(gP);
if (gP >= 0.43) {
s2.add(true);
} else {
s2.add(false);
}
s2T.add(getRuntime());
}
if (s3D < 30) {
double green = robot.color3.getNormalizedColors().green;
double red = robot.color3.getNormalizedColors().red;
double blue = robot.color3.getNormalizedColors().blue;
double gP = green / (green + red + blue);
s3G.add(gP);
if (gP >= 0.43) {
s3.add(true);
} else {
s3.add(false);
}
s3T.add(getRuntime());
}
if (!s1.isEmpty()) {
green1 = checkGreen(s1, s1T);
}
if (!s2.isEmpty()) {
green2 = checkGreen(s2, s2T);
}
if (!s3.isEmpty()) {
green3 = checkGreen(s3, s3T);
}
//SHOOTER:
double penguin = 0;
if (ticker % 8 == 0) {
penguin = (double) robot.shooterEncoder.getCurrentPosition() / 2048;
double stamp = getRuntime();
velo1 = -60 * ((penguin - initPos) / (stamp - stamp1));
initPos = penguin;
stamp1 = stamp;
}
velo = velo1;
double feed = vel / 4500;
if (vel > 500) {
feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
}
// --- PROPORTIONAL CORRECTION ---
double error = vel - velo1;
double correction = kP * error;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
double powPID = feed + correction;
// clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID));
if (vel - velo > 1000) {
powPID = 1;
} else if (velo - vel > 1000) {
powPID = 0;
}
TELE.addData("PIDPower", powPID);
TELE.addData("vel", velo1);
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.transfer.setPower(1);
//TURRET:
double offset;
double robotX = drive.localizer.getPose().position.x - xOffset;
double robotY = drive.localizer.getPose().position.y - xOffset;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -10;
double goalY = 0;
double dx = goalX - robotX; // delta x from robot to goal
double dy = goalY - robotY; // delta y from robot to goal
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
desiredTurretAngle = (Math.toDegrees(Math.atan2(dy, dx)) + 360) % 360;
desiredTurretAngle += manualOffset;
offset = desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset));
if (offset > 135) {
offset -= 360;
}
double pos = turrDefault;
TELE.addData("offset", offset);
pos -= offset * (0.9 / 360);
if (pos < 0.02) {
pos = 0.02;
} else if (pos > 0.97) {
pos = 0.97;
}
if (manualTurret) {
pos = turrDefault + (manualOffset / 100);
}
if (!overrideTurr) {
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1 - pos);
}
if (gamepad2.dpad_right) {
manualOffset -= 2;
} else if (gamepad2.dpad_left) {
manualOffset += 2;
}
//VELOCITY AUTOMATIC
if (autoVel) {
vel = velPrediction(distanceToGoal);
} else {
vel = manualVel;
}
if (gamepad2.right_stick_button) {
autoVel = true;
} else if (gamepad2.right_stick_y < -0.5) {
autoVel = false;
manualVel = 4100;
} else if (gamepad2.right_stick_y > 0.5) {
autoVel = false;
manualVel = 2700;
} else if (gamepad2.right_stick_x > 0.5) {
autoVel = false;
manualVel = 3600;
} else if (gamepad2.right_stick_x < -0.5) {
autoVel = false;
manualVel = 3100;
}
//HOOD:
if (autoHood) {
robot.hood.setPosition(hoodAnglePrediction(distanceToGoal) + autoHoodOffset);
} else {
robot.hood.setPosition(hoodDefaultPos + hoodOffset);
}
if (gamepad2.dpadUpWasPressed()) {
hoodOffset -= 0.03;
autoHoodOffset -= 0.02;
} else if (gamepad2.dpadDownWasPressed()) {
hoodOffset += 0.03;
autoHoodOffset += 0.02;
}
if (gamepad2.left_stick_x > 0.5) {
manualTurret = false;
} else if (gamepad2.left_stick_x < -0.5) {
manualTurret = true;
manualOffset = 0;
autoHoodOffset = 0;
if (gamepad2.left_bumper) {
xOffset = robotX;
yOffset = robotY;
headingOffset = robotHeading;
}
}
if (gamepad2.left_stick_y < -0.5) {
autoHood = true;
} else if (gamepad2.left_stick_y > 0.5) {
autoHood = false;
hoodOffset = 0;
if (gamepad2.left_bumper) {
xOffset = robotX;
yOffset = robotY;
headingOffset = robotHeading;
}
}
//SHOOT ALL:]
if (emergency) {
intake = false;
reject = false;
if (getRuntime() % 3 > 1.5) {
robot.spin1.setPosition(0);
robot.spin2.setPosition(1);
} else {
robot.spin1.setPosition(1);
robot.spin2.setPosition(0);
}
if (getRuntime() % 1 < 0.5) {
robot.transferServo.setPosition(transferServo_in);
} else {
robot.transferServo.setPosition(transferServo_out);
}
robot.transfer.setPower(1);
} else if (shootAll) {
TELE.addData("100% works", shootOrder);
intake = false;
reject = false;
if (!shootOrder.isEmpty() && (getRuntime() - shootStamp < 12)) {
int currentSlot = shootOrder.get(0); // Peek, do NOT remove yet
boolean shootingDone = false;
AprilTagDetection d20 = aprilTagWebcam.getTagById(20);
AprilTagDetection d24 = aprilTagWebcam.getTagById(24);
if (d20 != null) {
overrideTurr = true;
double bearing = d20.ftcPose.bearing;
double finalPos =robot.turr1.getPosition() - (bearing/1300);
robot.turr1.setPosition(finalPos);
robot.turr2.setPosition(1-finalPos);
TELE.addData("Bear", bearing);
}
if (d24 != null) {
overrideTurr = true;
double bearing = d24.ftcPose.bearing;
double finalPos = turretPos() + (bearing/720);
robot.turr1.setPosition(finalPos);
robot.turr2.setPosition(1-finalPos);
}
if (!outtake1) {
outtake1 = (spindexPosEqual(spindexer_outtakeBall1));
}
if (!outtake2) {
outtake2 = (spindexPosEqual(spindexer_outtakeBall2));
}
if (!outtake3) {
outtake3 = (spindexPosEqual(spindexer_outtakeBall3));
}
switch (currentSlot) {
case 1:
shootA = shootTeleop(spindexer_outtakeBall1, outtake1, shootStamp2);
TELE.addData("shootA", shootA);
if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
shootingDone = !shootA;
} else {
shootingDone = true;
}
break;
case 2:
shootB = shootTeleop(spindexer_outtakeBall2, outtake2, shootStamp2);
TELE.addData("shootB", shootB);
if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
shootingDone = !shootB;
} else {
shootingDone = true;
}
break;
case 3:
shootC = shootTeleop(spindexer_outtakeBall3, outtake3, shootStamp2);
TELE.addData("shootC", shootC);
if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
shootingDone = !shootC;
} else {
shootingDone = true;
}
break;
}
// Remove from the list only if shooting is complete
if (shootingDone) {
shootOrder.remove(0);
shootStamp2 = getRuntime();
}
} else {
// Finished shooting all balls
robot.spin1.setPosition(spindexer_intakePos1);
robot.spin2.setPosition(1 - spindexer_intakePos1);
shootA = true;
shootB = true;
shootC = true;
reject = false;
intake = true;
shootAll = false;
outtake1 = false;
outtake2 = false;
outtake3 = false;
overrideTurr = false;
}
}
if (gamepad2.squareWasPressed()) {
square = true;
shootStamp = getRuntime();
shootStamp2 = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
}
if (gamepad2.circleWasPressed()) {
circle = true;
shootStamp = getRuntime();
shootStamp2 = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
}
if (gamepad2.triangleWasPressed()) {
triangle = true;
shootStamp = getRuntime();
shootStamp2 = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
}
if (square || circle || triangle) {
// Count green balls
int greenCount = 0;
if (green1) greenCount++;
if (green2) greenCount++;
if (green3) greenCount++;
// Determine the odd ball color
oddBallColor = greenCount < 2; // true = green, false = purple
shootOrder.clear();
// Determine shooting order based on button pressed
// square = odd ball first, triangle = odd ball second, circle = odd ball third
if (square) {
// Odd ball first
addOddThenRest(shootOrder, oddBallColor);
} else if (triangle) {
// Odd ball second
addOddInMiddle(shootOrder, oddBallColor);
} else if (circle) {
// Odd ball last
addOddLast(shootOrder, oddBallColor);
}
circle = false;
square = false;
triangle = false;
}
// Right bumper shoots all balls fastest, ignoring colors
if (gamepad2.rightBumperWasPressed()) {
shootOrder.clear();
shootStamp = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
// Fastest order (example: slot 3 → 2 → 1)
if (ballIn(3)){
shootOrder.add(3);
}
if (ballIn(2)){
shootOrder.add(2);
}
if (ballIn(1)){
shootOrder.add(1);
}
if (!shootOrder.contains(3)) {
shootOrder.add(3);
}
if (!shootOrder.contains(2)) {
shootOrder.add(2);
}
if (!shootOrder.contains(1)) {
shootOrder.add(1);
}
shootAll = true;
shootPos = drive.localizer.getPose();
}
// Right bumper shoots all balls fastest, ignoring colors
if (gamepad2.leftBumperWasPressed()) {
shootOrder.clear();
shootStamp = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
// Fastest order (example: slot 3 → 2 → 1)
if (ballIn(3)) {
shootOrder.add(3);
}
if (ballIn(2)) {
shootOrder.add(2);
}
if (ballIn(1)) {
shootOrder.add(1);
}
shootAll = true;
shootPos = drive.localizer.getPose();
}
if (gamepad2.crossWasPressed()) {
emergency = !emergency;
}
//MISC:
drive.updatePoseEstimate();
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
}
TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
TELE.addData("Spin3Green", green3 + ": " + ballIn(3));
TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
TELE.addData("distanceToGoal", distanceToGoal);
TELE.addData("hood", robot.hood.getPosition());
TELE.addData("targetVel", vel);
TELE.addData("shootOrder", shootOrder);
TELE.addData("oddColor", oddBallColor);
aprilTagWebcam.update();
TELE.update();
ticker++;
}
}
// Helper method
private boolean checkGreen(List<Boolean> s, List<Double> sT) {
if (s.isEmpty()) return false;
double lastTime = sT.get(sT.size() - 1);
int countTrue = 0;
int countWindow = 0;
for (int i = 0; i < s.size(); i++) {
if (lastTime - sT.get(i) <= 3.0) { // element is within 2s of last
countWindow++;
if (s.get(i)) {
countTrue++;
}
}
}
if (countWindow == 0) return false; // avoid divide by zero
return countTrue > countWindow / 2.0; // more than 50% true
}
boolean spindexPosEqual(double spindexer) {
return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 &&
scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01);
}
public boolean shootTeleop(double spindexer, boolean spinOk, double stamp) {
// Set spin positions
robot.spin1.setPosition(spindexer);
robot.spin2.setPosition(1 - spindexer);
// Check if spindexer has reached the target position
if (spinOk || getRuntime() - stamp > 1.5) {
if (tickerA == 1) {
transferStamp = getRuntime();
tickerA++;
TELE.addLine("tickerSet");
}
if (getRuntime() - transferStamp > waitTransfer && !transferIn) {
robot.transferServo.setPosition(transferServo_in);
transferIn = true;
TELE.addLine("transferring");
return true; // still in progress
} else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && transferIn) {
robot.transferServo.setPosition(transferServo_out);
transferIn = false; // reset for next shot
tickerA = 1; // reset ticker
transferStamp = 0.0;
TELE.addLine("shotFinished");
return false; // finished shooting
} else {
TELE.addLine("sIP");
return true; // still in progress
}
} else {
robot.transferServo.setPosition(transferServo_out);
tickerA = 1;
transferStamp = getRuntime();
transferIn = false;
return true; // still moving spin
}
}
public double hoodAnglePrediction(double x) {
if (x < 34) {
double L = 1.04471;
double U = 0.711929;
double Q = 120.02263;
double B = 0.780982;
double M = 20.61191;
double v = 10.40506;
double inner = 1 + Q * Math.exp(-B * (x - M));
return L + (U - L) / Math.pow(inner, 1.0 / v);
} else {
// x >= 34
return 1.94372 * Math.exp(-0.0528731 * x) + 0.39;
}
}
void addOddThenRest(List<Integer> order, boolean oddColor) {
// Odd ball first
for (int i = 1; i <= 3; i++) if (getBallColor(i) == oddColor) order.add(i);
TELE.addData("1", shootOrder);
for (int i = 1; i <= 3; i++) if (getBallColor(i) != oddColor) order.add(i);
TELE.addData("works", shootOrder);
TELE.addData("oddBall", oddColor);
shootAll = true;
}
void addOddInMiddle(List<Integer> order, boolean oddColor) {
boolean[] used = new boolean[4]; // index 1..3
// 1) Add a NON-odd ball first
for (int i = 1; i <= 3; i++) {
if (getBallColor(i) != oddColor) {
order.add(i);
used[i] = true;
break;
}
}
// 2) Add the odd ball second
for (int i = 1; i <= 3; i++) {
if (!used[i] && getBallColor(i) == oddColor) {
order.add(i);
used[i] = true;
break;
}
}
// 3) Add the remaining non-odd ball third
for (int i = 1; i <= 3; i++) {
if (!used[i] && getBallColor(i) != oddColor) {
order.add(i);
used[i] = true;
break;
}
}
TELE.addData("works", order);
TELE.addData("oddBall", oddColor);
shootAll = true;
}
void addOddLast(List<Integer> order, boolean oddColor) {
// Odd ball last
for (int i = 1; i <= 3; i++) if (getBallColor(i) != oddColor) order.add(i);
TELE.addData("1", shootOrder);
for (int i = 1; i <= 3; i++) if (getBallColor(i) == oddColor) order.add(i);
TELE.addData("works", shootOrder);
TELE.addData("oddBall", oddColor);
shootAll = true;
}
// Returns color of ball in slot i (1-based)
boolean getBallColor(int slot) {
switch (slot) {
case 1:
return green1;
case 2:
return green2;
case 3:
return green3;
}
return false; // default
}
boolean ballIn(int slot) {
switch (slot) {
case 1:
if (!s1T.isEmpty()) {
return !(s1T.get(s1T.size() - 1) < (getRuntime()) - 3);
}
case 2:
if (!s2T.isEmpty()) {
return !(s2T.get(s2T.size() - 1) < (getRuntime()) - 3);
}
case 3:
if (!s3T.isEmpty()) {
return !(s3T.get(s3T.size() - 1) < (getRuntime()) - 3);
}
}
return true; // default
}
public double turretPos() {
return (scalar*((robot.turr1Pos.getVoltage()-restPos) /3.3 ));
}
}

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

186
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/old.txt
View File

@@ -1,7 +1,7 @@
package org.firstinspires.ftc.teamcode.teleop; package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodDefault; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
@@ -10,6 +10,7 @@ import com.acmerobotics.roadrunner.Pose2d;
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.arcrobotics.ftclib.gamepad.ToggleButtonReader; import com.arcrobotics.ftclib.gamepad.ToggleButtonReader;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
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;
@@ -22,12 +23,10 @@ import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
import org.firstinspires.ftc.teamcode.subsystems.Transfer; import org.firstinspires.ftc.teamcode.subsystems.Transfer;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;
@Config @Config
@TeleOp @TeleOp
@Disabled
public class TeleopV1 extends LinearOpMode { public class old extends LinearOpMode {
Robot robot; Robot robot;
@@ -63,7 +62,6 @@ public class TeleopV1 extends LinearOpMode {
ToggleButtonReader g2Square; ToggleButtonReader g2Square;
ToggleButtonReader g2Triangle; ToggleButtonReader g2Triangle;
ToggleButtonReader g2RightBumper; ToggleButtonReader g2RightBumper;
@@ -85,7 +83,6 @@ public class TeleopV1 extends LinearOpMode {
public double g1LeftBumperStamp = 0.0; public double g1LeftBumperStamp = 0.0;
public double g2LeftBumperStamp = 0.0; public double g2LeftBumperStamp = 0.0;
public static int spindexerPos = 0; public static int spindexerPos = 0;
@@ -96,7 +93,7 @@ public class TeleopV1 extends LinearOpMode {
public boolean scoreAll = false; public boolean scoreAll = false;
MecanumDrive drive ; MecanumDrive drive;
public boolean autotrack = false; public boolean autotrack = false;
@@ -117,17 +114,11 @@ public class TeleopV1 extends LinearOpMode {
public boolean tri = false; public boolean tri = false;
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
drive = new MecanumDrive(hardwareMap, teleStart); drive = new MecanumDrive(hardwareMap, teleStart);
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry( TELE = new MultipleTelemetry(
@@ -163,7 +154,6 @@ public class TeleopV1 extends LinearOpMode {
g2, GamepadKeys.Button.RIGHT_BUMPER g2, GamepadKeys.Button.RIGHT_BUMPER
); );
g2LeftBumper = new ToggleButtonReader( g2LeftBumper = new ToggleButtonReader(
g2, GamepadKeys.Button.LEFT_BUMPER g2, GamepadKeys.Button.LEFT_BUMPER
); );
@@ -172,7 +162,6 @@ public class TeleopV1 extends LinearOpMode {
g2, GamepadKeys.Button.DPAD_UP g2, GamepadKeys.Button.DPAD_UP
); );
g2DpadDown = new ToggleButtonReader( g2DpadDown = new ToggleButtonReader(
g2, GamepadKeys.Button.DPAD_DOWN g2, GamepadKeys.Button.DPAD_DOWN
); );
@@ -181,14 +170,10 @@ public class TeleopV1 extends LinearOpMode {
g2, GamepadKeys.Button.DPAD_LEFT g2, GamepadKeys.Button.DPAD_LEFT
); );
g2DpadRight = new ToggleButtonReader( g2DpadRight = new ToggleButtonReader(
g2, GamepadKeys.Button.DPAD_RIGHT g2, GamepadKeys.Button.DPAD_RIGHT
); );
drivetrain = new Drivetrain(robot, TELE, g1); drivetrain = new Drivetrain(robot, TELE, g1);
drivetrain.setMode("Default"); drivetrain.setMode("Default");
@@ -201,7 +186,6 @@ public class TeleopV1 extends LinearOpMode {
transfer = new Transfer(robot); transfer = new Transfer(robot);
spindexer = new Spindexer(robot, TELE); spindexer = new Spindexer(robot, TELE);
spindexer.setTelemetryOn(true); spindexer.setTelemetryOn(true);
@@ -212,18 +196,13 @@ public class TeleopV1 extends LinearOpMode {
robot.rejecter.setPosition(rIn); robot.rejecter.setPosition(rIn);
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
drive = new MecanumDrive(hardwareMap, teleStart); drive = new MecanumDrive(hardwareMap, teleStart);
while (opModeIsActive()) {
while(opModeIsActive()){
drive.updatePoseEstimate(); drive.updatePoseEstimate();
@@ -231,28 +210,24 @@ public class TeleopV1 extends LinearOpMode {
TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
TELE.addData("off", offset); TELE.addData("off", offset);
robot.hood.setPosition(pos); robot.hood.setPosition(pos);
g1LeftBumper.readValue(); g1LeftBumper.readValue();
if (g1LeftBumper.wasJustPressed()){ if (g1LeftBumper.wasJustPressed()) {
g2LeftBumperStamp = getRuntime(); g2LeftBumperStamp = getRuntime();
spindexer.intakeShake(getRuntime()); spindexer.intakeShake(getRuntime());
leftBumper = true; leftBumper = true;
} }
if (leftBumper){ if (leftBumper) {
double time = getRuntime() - g2LeftBumperStamp; double time = getRuntime() - g2LeftBumperStamp;
if (time < 1.0){ if (time < 1.0) {
robot.rejecter.setPosition(rOut); robot.rejecter.setPosition(rOut);
} else { } else {
robot.rejecter.setPosition(rIn); robot.rejecter.setPosition(rIn);
@@ -260,9 +235,6 @@ public class TeleopV1 extends LinearOpMode {
} }
intake(); intake();
drivetrain.update(); drivetrain.update();
@@ -279,63 +251,49 @@ public class TeleopV1 extends LinearOpMode {
g2DpadUp.readValue(); g2DpadUp.readValue();
if (!scoreAll){ if (!scoreAll) {
spindexer.checkForBalls(); spindexer.checkForBalls();
} }
if(g2DpadUp.wasJustPressed()){ if (g2DpadUp.wasJustPressed()) {
pos -=0.02; pos -= 0.02;
} }
if(g2DpadDown.wasJustPressed()){ if (g2DpadDown.wasJustPressed()) {
pos +=0.02; pos += 0.02;
} }
g2DpadLeft.readValue(); g2DpadLeft.readValue();
g2DpadRight.readValue(); g2DpadRight.readValue();
if(g2DpadLeft.wasJustPressed()){ if (g2DpadLeft.wasJustPressed()) {
offset -=0.02; offset -= 0.02;
} }
if(g2DpadRight.wasJustPressed()){ if (g2DpadRight.wasJustPressed()) {
offset +=0.02; offset += 0.02;
} }
TELE.addData("hood", pos); TELE.addData("hood", pos);
if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) { if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) {
shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset); shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset);
} else { } else {
autotrack = false; autotrack = false;
shooter.moveTurret(0.3+offset); shooter.moveTurret(0.3 + offset);
} }
if (gamepad2.right_stick_button){ if (gamepad2.right_stick_button) {
autotrack = true; autotrack = true;
} }
if (g2RightBumper.wasJustPressed()) {
if (g2RightBumper.wasJustPressed()){
transfer.setTransferPower(1); transfer.setTransferPower(1);
transfer.transferIn(); transfer.transferIn();
shooter.setManualPower(1); shooter.setManualPower(1);
@@ -344,19 +302,19 @@ public class TeleopV1 extends LinearOpMode {
} }
if (g2RightBumper.wasJustReleased()){ if (g2RightBumper.wasJustReleased()) {
transfer.setTransferPower(1); transfer.setTransferPower(1);
transfer.transferOut(); transfer.transferOut();
} }
if (gamepad2.left_stick_y>0.5){ if (gamepad2.left_stick_y > 0.5) {
shooter.setManualPower(0); shooter.setManualPower(0);
} else if (gamepad2.left_stick_y<-0.5){ } else if (gamepad2.left_stick_y < -0.5) {
shooter.setManualPower(1); shooter.setManualPower(1);
} }
if (g2LeftBumper.wasJustPressed()){ if (g2LeftBumper.wasJustPressed()) {
g2LeftBumperStamp = getRuntime(); g2LeftBumperStamp = getRuntime();
notShooting = false; notShooting = false;
scoreAll = true; scoreAll = true;
@@ -365,7 +323,6 @@ public class TeleopV1 extends LinearOpMode {
if (scoreAll) { if (scoreAll) {
double time = getRuntime() - g2LeftBumperStamp; double time = getRuntime() - g2LeftBumperStamp;
shooter.setManualPower(1); shooter.setManualPower(1);
TELE.addData("greenImportant", green); TELE.addData("greenImportant", green);
@@ -377,7 +334,6 @@ public class TeleopV1 extends LinearOpMode {
if (square) { if (square) {
if (time < 0.3) { if (time < 0.3) {
ticker = 0; ticker = 0;
@@ -385,7 +341,6 @@ public class TeleopV1 extends LinearOpMode {
last = 0; last = 0;
second = 0; second = 0;
transfer.transferOut(); transfer.transferOut();
transfer.setTransferPower(1); transfer.setTransferPower(1);
} else if (time < 2) { } else if (time < 2) {
@@ -406,14 +361,12 @@ public class TeleopV1 extends LinearOpMode {
ticker++; ticker++;
} else if (time < 2.5) { } else if (time < 2.5) {
ticker = 0; ticker = 0;
second = last; second = last;
transfer.transferIn(); transfer.transferIn();
} else if (time < 4) { } else if (time < 4) {
transfer.transferOut(); transfer.transferOut();
@@ -433,11 +386,9 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0; ticker = 0;
transfer.transferIn(); transfer.transferIn();
} else if (time < 6) { } else if (time < 6) {
transfer.transferOut(); transfer.transferOut();
if (ticker == 0) { if (ticker == 0) {
@@ -458,7 +409,6 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0; ticker = 0;
scoreAll = false; scoreAll = false;
transfer.transferOut(); transfer.transferOut();
@@ -467,7 +417,6 @@ public class TeleopV1 extends LinearOpMode {
} }
} else if (tri) { } else if (tri) {
if (time < 0.3) { if (time < 0.3) {
ticker = 0; ticker = 0;
@@ -475,7 +424,6 @@ public class TeleopV1 extends LinearOpMode {
last = 0; last = 0;
second = 0; second = 0;
transfer.transferOut(); transfer.transferOut();
transfer.setTransferPower(1); transfer.setTransferPower(1);
} else if (time < 2) { } else if (time < 2) {
@@ -496,14 +444,12 @@ public class TeleopV1 extends LinearOpMode {
ticker++; ticker++;
} else if (time < 2.5) { } else if (time < 2.5) {
ticker = 0; ticker = 0;
second = last; second = last;
transfer.transferIn(); transfer.transferIn();
} else if (time < 4) { } else if (time < 4) {
transfer.transferOut(); transfer.transferOut();
@@ -523,11 +469,9 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0; ticker = 0;
transfer.transferIn(); transfer.transferIn();
} else if (time < 6) { } else if (time < 6) {
transfer.transferOut(); transfer.transferOut();
if (ticker == 0) { if (ticker == 0) {
@@ -548,15 +492,13 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0; ticker = 0;
scoreAll = false; scoreAll = false;
transfer.transferOut(); transfer.transferOut();
shooter.setManualPower(0); shooter.setManualPower(0);
} }
} else if (circle){ } else if (circle) {
if (time < 0.3) { if (time < 0.3) {
@@ -565,7 +507,6 @@ public class TeleopV1 extends LinearOpMode {
last = 0; last = 0;
second = 0; second = 0;
transfer.transferOut(); transfer.transferOut();
transfer.setTransferPower(1); transfer.setTransferPower(1);
} else if (time < 2) { } else if (time < 2) {
@@ -586,14 +527,12 @@ public class TeleopV1 extends LinearOpMode {
ticker++; ticker++;
} else if (time < 2.5) { } else if (time < 2.5) {
ticker = 0; ticker = 0;
second = last; second = last;
transfer.transferIn(); transfer.transferIn();
} else if (time < 4) { } else if (time < 4) {
transfer.transferOut(); transfer.transferOut();
@@ -613,11 +552,9 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0; ticker = 0;
transfer.transferIn(); transfer.transferIn();
} else if (time < 6) { } else if (time < 6) {
transfer.transferOut(); transfer.transferOut();
if (ticker == 0) { if (ticker == 0) {
@@ -638,15 +575,13 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0; ticker = 0;
scoreAll = false; scoreAll = false;
transfer.transferOut(); transfer.transferOut();
shooter.setManualPower(0); shooter.setManualPower(0);
} }
} else{ } else {
if (time < 0.3) { if (time < 0.3) {
@@ -668,7 +603,6 @@ public class TeleopV1 extends LinearOpMode {
all = true; all = true;
} }
transfer.transferOut(); transfer.transferOut();
transfer.setTransferPower(1); transfer.setTransferPower(1);
} else if (time < 2) { } else if (time < 2) {
@@ -693,7 +627,6 @@ public class TeleopV1 extends LinearOpMode {
ticker++; ticker++;
} else if (time < 2.5) { } else if (time < 2.5) {
ticker = 0; ticker = 0;
@@ -710,7 +643,6 @@ public class TeleopV1 extends LinearOpMode {
all = false; all = false;
} }
transfer.transferIn(); transfer.transferIn();
@@ -736,7 +668,6 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0; ticker = 0;
if (gamepad2.right_trigger > 0.5) { if (gamepad2.right_trigger > 0.5) {
green = false; green = false;
@@ -751,7 +682,6 @@ public class TeleopV1 extends LinearOpMode {
transfer.transferIn(); transfer.transferIn();
} else if (time < 6) { } else if (time < 6) {
transfer.transferOut(); transfer.transferOut();
if (ticker == 0) { if (ticker == 0) {
@@ -774,7 +704,6 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0; ticker = 0;
scoreAll = false; scoreAll = false;
transfer.transferOut(); transfer.transferOut();
@@ -782,33 +711,16 @@ public class TeleopV1 extends LinearOpMode {
} }
} }
} }
shooter.update(); shooter.update();
}
} }
public void intake() {
}
public void intake(){
g1RightBumper.readValue(); g1RightBumper.readValue();
@@ -818,84 +730,64 @@ public class TeleopV1 extends LinearOpMode {
g2Triangle.readValue(); g2Triangle.readValue();
if (g1RightBumper.wasJustPressed()){ if (g1RightBumper.wasJustPressed()) {
notShooting = true; notShooting = true;
if (getRuntime() - g1RightBumperStamp < 0.3) {
if (getRuntime() - g1RightBumperStamp < 0.3){
intake.reverse(); intake.reverse();
} else { } else {
intake.toggle(); intake.toggle();
} }
if (intake.getIntakeState()==1){ if (intake.getIntakeState() == 1) {
shooter.setManualPower(0); shooter.setManualPower(0);
} }
spindexer.intake(); spindexer.intake();
transfer.transferOut(); transfer.transferOut();
g1RightBumperStamp = getRuntime(); g1RightBumperStamp = getRuntime();
} }
if (intake.getIntakeState() == 1 && notShooting) {
if (intake.getIntakeState()==1 && notShooting) {
spindexer.intakeShake(getRuntime()); spindexer.intakeShake(getRuntime());
} else { } else {
if (g2Circle.wasJustPressed()){ if (g2Circle.wasJustPressed()) {
circle = true; circle = true;
tri = false; tri = false;
square = false; square = false;
} }
if (g2Triangle.wasJustPressed()){ if (g2Triangle.wasJustPressed()) {
circle = false; circle = false;
tri = true; tri = true;
square = false; square = false;
} }
if (g2Square.wasJustPressed()){ if (g2Square.wasJustPressed()) {
circle = false; circle = false;
tri = false; tri = false;
square = true; square = true;
} }
if (gamepad2.x){ if (gamepad2.x) {
circle = false; circle = false;
tri = false; tri = false;
square = false; square = false;
} }
} }
intake.update(); intake.update();
spindexer.update(); spindexer.update();
} }
} }

144
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.txt Normal file
View File

@@ -0,0 +1,144 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
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;
@TeleOp
@Config
public class ActiveColorSensorTest extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
@Override
public void runOpMode() throws InterruptedException{
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
int b1Purple = 1;
int b1Total = 1;
int b2Purple = 1;
int b2Total = 1;
int b3Purple = 1;
int b3Total = 1;
double totalStamp1 = 0.0;
double purpleStamp1 = 0.0;
double totalStamp2 = 0.0;
double purpleStamp2 = 0.0;
double totalStamp3 = 0.0;
double purpleStamp3 = 0.0;
String b1 = "none";
String b2 = "none";
String b3 = "none";
double position = 0.0;
double stamp = getRuntime();
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()){
if ((getRuntime() % 0.3) >0.15) {
position = spindexer_intakePos1 + 0.015;
} else {
position = spindexer_intakePos1 - 0.015;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1-position);
robot.intake.setPower(1);
// Reset the counters after 1 second of not reading a ball.
final double ColorCounterResetDelay = 1.0;
// Number of times the loop needs to run before deciding on a color.
final int ColorCounterTotalMinCount = 20;
// If the color sensor reads a color this percentage of time
// out of the total, declare the color.
// Usage: (Color Count)/(Total Count) > ColorCounterThreshold
final double ColorCounterThreshold = 0.65;
if (robot.pin1.getState()){
if (robot.pin0.getState()){
b1Purple ++;
}
b1Total++;
totalStamp1 = getRuntime();
}
if (getRuntime() - totalStamp1 > ColorCounterResetDelay) {
// Too Much time has passed without detecting ball
b1 = "none";
b1Total = 1;
b1Purple = 1;
}else if ((b1Total > ColorCounterTotalMinCount) && ((double) b1Purple / b1Total) >= ColorCounterThreshold){
// Enough Time has passed and we met the threshold
b1 = "Purple";
}else if (b1Total > ColorCounterTotalMinCount) {
// Enough Time passed WITHOUT meeting the threshold
b1 = "Green";
}
if (robot.pin3.getState()){
if (robot.pin2.getState()){
b2Purple ++;
}
b2Total++;
totalStamp2 = getRuntime();
}
if (getRuntime() - totalStamp2 > ColorCounterResetDelay) {
// Too Much time has passed without detecting ball
b2 = "none";
b2Total = 1;
b2Purple = 1;
}else if ((b2Total > ColorCounterTotalMinCount) && ((double) b2Purple / b2Total) >= ColorCounterThreshold){
// Enough Time has passed and we met the threshold
b2 = "Purple";
}else if (b2Total > ColorCounterTotalMinCount) {
// Enough Time passed WITHOUT meeting the threshold
b2 = "Green";
}
if (robot.pin5.getState()){
if (robot.pin4.getState()){
b3Purple ++;
}
b3Total++;
totalStamp3 = getRuntime();
}
if (getRuntime() - totalStamp3 > ColorCounterResetDelay) {
// Too Much time has passed without detecting ball
b3 = "none";
b3Total = 1;
b3Purple = 1;
}else if ((b3Total > ColorCounterTotalMinCount) && ((double) b3Purple / b3Total) >= ColorCounterThreshold){
// Enough Time has passed and we met the threshold
b3 = "Purple";
}else if (b3Total > ColorCounterTotalMinCount) {
// Enough Time passed WITHOUT meeting the threshold
b3 = "Green";
}
TELE.addData("Green1:", robot.pin1.getState());
TELE.addData("Purple1:", robot.pin0.getState());
TELE.addData("Green2:", robot.pin3.getState());
TELE.addData("Purple2:", robot.pin2.getState());
TELE.addData("Green3:", robot.pin5.getState());
TELE.addData("Purple3:", robot.pin4.getState());
TELE.addData("1", b1);
TELE.addData("2",b2);
TELE.addData("3",b3);
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.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.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();
}
}

63
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.txt Normal file
View File

@@ -0,0 +1,63 @@
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.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.utils.Robot;
@TeleOp
@Config
public class ColorSensorTest extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
// ----- COLOR 1 -----
double green1 = robot.color1.getNormalizedColors().green;
double blue1 = robot.color1.getNormalizedColors().blue;
double red1 = robot.color1.getNormalizedColors().red;
TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor());
TELE.addData("Color1 green", green1 / (green1 + blue1 + red1));
TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM));
// ----- COLOR 2 -----
double green2 = robot.color2.getNormalizedColors().green;
double blue2 = robot.color2.getNormalizedColors().blue;
double red2 = robot.color2.getNormalizedColors().red;
TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor());
TELE.addData("Color2 green", green2 / (green2 + blue2 + red2));
TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM));
// ----- COLOR 3 -----
double green3 = robot.color3.getNormalizedColors().green;
double blue3 = robot.color3.getNormalizedColors().blue;
double red3 = robot.color3.getNormalizedColors().red;
TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor());
TELE.addData("Color3 green", green3 / (green3 + blue3 + red3));
TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM));
TELE.update();
}
}
}

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

@@ -3,94 +3,108 @@ 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.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.utils.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;
// --- CONSTANTS YOU TUNE ---
public static double MAX_RPM = 4500; // your measured max RPM
public static double kP = 0.001; // small proportional gain (tune this)
public static double maxStep = 0.06; // prevents sudden jumps
//TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED
public static double transferPower = 0.0;
public static double hoodPos = 0.501;
public static double turretPos = 0.501;
Robot robot; Robot robot;
private double lastEncoderRevolutions = 0.0;
private double lastTimeStamp = 0.0;
public static double pow = 0.0;
public static double vel = 0.0;
public static int pos = 0;
public static double posPower = 0.0;
public static double p = 0.000003, i = 0, d = 0.000001;
public static String flyMode = "MANUAL";
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);
DcMotorEx leftShooter = robot.shooter1;
DcMotorEx rightShooter = robot.shooter2;
DcMotorEx 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()){ double kF = 1.0 / MAX_RPM; // baseline feedforward
shooter.setControllerCoefficients(p, i, d); double encoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
shooter.setTurretMode(turrMode); double velocity = -60 * (encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp);
shooter.setShooterMode(flyMode); TELE.addLine("Mode: 0 = Manual, 1 = Vel, 2 = Pos");
TELE.addLine("Parameter = pow, vel, or pos");
TELE.addData("leftShooterPower", leftShooter.getPower());
TELE.addData("rightShooterPower", rightShooter.getPower());
TELE.addData("shaftEncoderPos", encoderRevolutions);
TELE.addData("shaftEncoderVel", velocity);
shooter.setManualPower(pow); double velPID;
shooter.setVelocity(vel); if (mode == 0) {
rightShooter.setPower(parameter);
leftShooter.setPower(parameter);
} else if (mode == 1) {
shooter.setTargetPosition(pos); // --- FEEDFORWARD BASE POWER ---
double feed = kF * parameter; // Example: vel=2500 → feed=0.5
shooter.setTolerance(posTolerance); // --- PROPORTIONAL CORRECTION ---
double error = parameter - velocity;
double correction = kP * error;
shooter.setPosPower(posPower); // limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
if (servoPosition!=0.501) {shooter.sethoodPosition(servoPosition);} // --- FINAL MOTOR POWER ---
velPID = feed + correction;
// clamp to allowed range
velPID = Math.max(0, Math.min(1, velPID));
shooter.update(); rightShooter.setPower(velPID);
leftShooter.setPower(velPID);
}
lastTimeStamp = getRuntime();
lastEncoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
if (hoodPos != 0.501) {
robot.hood.setPosition(hoodPos);
}
if (turretPos!=0.501){
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(turretPos);
}
robot.transfer.setPower(transferPower);
TELE.update(); TELE.update();
} }
} }
} }

210
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.txt Normal file
View File

@@ -0,0 +1,210 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
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 com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.subsystems.Shooter;
@TeleOp
@Config
public class ShooterTest extends LinearOpMode {
Robot robot;
public static double pow = 0.0;
public static double vel = 0.0;
public static double ecpr = 1024.0; // CPR of the encoder
public static double hoodPos = 0.5;
public static double turretPos = 0.9;
public static String flyMode = "VEL";
public static boolean AutoTrack = false;
double initPos = 0.0;
double velo = 0.0;
double velo1 = 0.0;
double velo2 = 0.0;
double velo3 = 0.0;
double velo4 = 0.0;
double velo5 = 0.0;
double stamp1 = 0.0;
double initPos1 = 0.0;
double powPID = 0.0;
public static int maxVel = 4500;
public static boolean shoot = false;
public static int spindexPos = 1;
public static boolean intake = true;
public static int tolerance = 50;
double stamp = 0.0;
public static double kP = 0.001; // small proportional gain (tune this)
public static double maxStep = 0.06; // prevents sudden jumps
public static double distance = 50;
MultipleTelemetry TELE;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
Shooter shooter = new Shooter(robot, TELE);
robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
shooter.setTelemetryOn(true);
shooter.setShooterMode(flyMode);
initPos = shooter.getECPRPosition();
int ticker = 0;
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
ticker++;
if (AutoTrack){
hoodPos = hoodAnglePrediction(distance);
vel = velPrediction(distance);
}
shooter.setShooterMode(flyMode);
shooter.setManualPower(pow);
robot.hood.setPosition(hoodPos);
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1 - turretPos);
if (intake) {
robot.transfer.setPower(0);
robot.intake.setPower(0.75);
robot.spin1.setPosition(spindexer_intakePos1);
robot.spin2.setPosition(1 - spindexer_intakePos1);
} else {
robot.transfer.setPower(.75 + (powPID/4));
robot.intake.setPower(0);
if (spindexPos == 1) {
robot.spin1.setPosition(spindexer_outtakeBall1);
robot.spin2.setPosition(1 - spindexer_outtakeBall1);
} else if (spindexPos == 2) {
robot.spin1.setPosition(spindexer_outtakeBall2);
robot.spin2.setPosition(1 - spindexer_outtakeBall2);
} else if (spindexPos == 3) {
robot.spin1.setPosition(spindexer_outtakeBall3);
robot.spin2.setPosition(1 - spindexer_outtakeBall3);
}
}
double penguin = 0;
if (ticker % 8 ==0){
penguin = shooter.getECPRPosition();
stamp = getRuntime();
velo1 = -60 * ((penguin - initPos1) / (stamp - stamp1));
initPos1 = penguin;
stamp1 = stamp;
}
velo = velo1;
double feed = vel / maxVel; // Example: vel=2500 → feed=0.5
if (vel > 500){
feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
}
// --- PROPORTIONAL CORRECTION ---
double error = vel - velo1;
double correction = kP * error;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
powPID = feed + correction;
// clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID));
if (vel - velo > 1000){
powPID = 1;
} else if (velo - vel > 1000){
powPID = 0;
}
shooter.setVelocity(powPID);
if (shoot) {
robot.transferServo.setPosition(transferServo_in);
} else {
robot.transferServo.setPosition(transferServo_out);
}
shooter.update();
TELE.addData("Revolutions", shooter.getECPRPosition());
TELE.addData("hoodPos", shooter.gethoodPosition());
TELE.addData("turretPos", shooter.getTurretPosition());
TELE.addData("Power Fly 1", robot.shooter1.getPower());
TELE.addData("Power Fly 2", robot.shooter2.getPower());
TELE.addData("powPID", shooter.getpowPID());
TELE.addData("Velocity", velo);
TELE.update();
}
}
public double hoodAnglePrediction(double distance) {
double L = 0.298317;
double A = 1.02124;
double k = 0.0157892;
double n = 3.39375;
double dist = Math.sqrt(distance*distance+24*24);
return L + A * Math.exp(-Math.pow(k * dist, n));
}
public static double velPrediction(double distance) {
double x = Math.sqrt(distance*distance+24*24);
double A = -211149.992;
double B = -1.19943;
double C = 3720.15909;
return A * Math.pow(x, B) + C;
}
}

104
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AprilTagWebcam.java Normal file
View File

@@ -0,0 +1,104 @@
package org.firstinspires.ftc.teamcode.utils;
import android.util.Size;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
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.webcam);
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
}
}

92
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java Normal file
View File

@@ -0,0 +1,92 @@
package org.firstinspires.ftc.teamcode.utils;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.kP;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.maxStep;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
public class Flywheel {
Robot robot;
MultipleTelemetry TELE;
double initPos = 0.0;
double stamp = 0.0;
double stamp1 = 0.0;
double ticker = 0.0;
double stamp2 = 0.0;
double currentPos = 0.0;
boolean prevSteady = false;
double velo = 0.0;
double prevVelo = 0.0;
double targetVelocity = 0.0;
double powPID = 0.0;
boolean steady = false;
public Flywheel () {
//robot = new Robot(hardwareMap);
}
public double getVelo () {
return velo;
}
public boolean getSteady() {
return steady;
}
private double getTimeSeconds ()
{
return (double) System.currentTimeMillis()/1000.0;
}
public double manageFlywheel(int commandedVelocity, double shooter1CurPos) {
targetVelocity = (double) commandedVelocity;
ticker++;
if (ticker % 8 == 0) {
currentPos = shooter1CurPos / 2048;
stamp = getTimeSeconds(); //getRuntime();
velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
initPos = currentPos;
stamp1 = stamp;
}
// Flywheel control code here
if (targetVelocity - velo > 500) {
powPID = 1.0;
} else if (velo - targetVelocity > 500){
powPID = 0.0;
} else {
double feed = Math.log((668.39 / (targetVelocity + 591.96)) - 0.116) / -4.18;
// --- PROPORTIONAL CORRECTION ---
double error = targetVelocity - velo;
double correction = kP * error;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
powPID = feed + correction;
// clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID));
}
// really should be a running average of the last 5
if ((Math.abs(targetVelocity - velo) < 150.0) && (Math.abs(targetVelocity - prevVelo) < 150.0))
{
steady = true;
}
else
{
steady = false;
}
return powPID;
}
public void update()
{
};
};

53
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java Normal file
View File

@@ -0,0 +1,53 @@
package org.firstinspires.ftc.teamcode.utils;
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;
@TeleOp
@Config
public class PositionalServoProgrammer extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
public static double spindexPos = 0.501;
public static double turretPos = 0.501;
public static double transferPos = 0.501;
public static double hoodPos = 0.501;
public static double scalar = 1.112;
public static double restPos = 0.15;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()){
if (spindexPos != 0.501){
robot.spin1.setPosition(spindexPos);
robot.spin2.setPosition(1-spindexPos);
}
if (turretPos != 0.501){
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1-turretPos);
}
if (transferPos != 0.501){
robot.transferServo.setPosition(transferPos);
}
if (hoodPos != 0.501){
robot.hood.setPosition(hoodPos);
}
TELE.addData("spindexer", scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3));
TELE.addData("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3));
TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3));
TELE.addData("turret", scalar*((robot.turr1Pos.getVoltage() - restPos) / 3.3));
TELE.addData("spindexerA", robot.spin1Pos.getVoltage());
TELE.addData("hoodA", robot.hoodPos.getVoltage());
TELE.addData("transferServoA", robot.transferServoPos.getVoltage());
TELE.addData("turretA", robot.turr1Pos.getVoltage());
TELE.update();
}
}
}

58
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
View File

@@ -1,19 +1,16 @@
package org.firstinspires.ftc.teamcode.utils; package org.firstinspires.ftc.teamcode.utils;
import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorImplEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.DigitalChannel;
import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
import org.openftc.easyopencv.OpenCvWebcam;
public class Robot { public class Robot {
@@ -30,8 +27,6 @@ public class Robot {
public DcMotorEx transfer; public DcMotorEx transfer;
public DcMotorEx shooter1; public DcMotorEx shooter1;
public DcMotorEx shooter2; public DcMotorEx shooter2;
public Servo hood; public Servo hood;
@@ -61,27 +56,31 @@ public class Robot {
public AnalogInput analogInput2; public AnalogInput analogInput2;
public AprilTagProcessor aprilTagProcessor; public AnalogInput spin1Pos;
public AnalogInput spin2Pos;
public AnalogInput hoodPos;
public AnalogInput turr1Pos;
public AnalogInput turr2Pos;
public AnalogInput transferServoPos;
public AprilTagProcessor aprilTagProcessor;
public WebcamName webcam; public WebcamName webcam;
public DcMotorEx shooterEncoder; public DcMotorEx shooterEncoder;
public RevColorSensorV3 color1;
public RevColorSensorV3 color2;
public RevColorSensorV3 color3;
public Robot(HardwareMap hardwareMap) {
public Robot (HardwareMap hardwareMap) {
//Define components w/ hardware map //Define components w/ hardware map
@@ -106,16 +105,28 @@ public class Robot {
shooter1.setDirection(DcMotorSimple.Direction.REVERSE); shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
shooterEncoder = shooter1;
hood = hardwareMap.get(Servo.class, "hood"); hood = hardwareMap.get(Servo.class, "hood");
hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");
turr1 = hardwareMap.get(Servo.class, "t1"); turr1 = hardwareMap.get(Servo.class, "t1");
turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos");
turr2 = hardwareMap.get(Servo.class, "t2"); turr2 = hardwareMap.get(Servo.class, "t2");
turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos");
spin1 = hardwareMap.get(Servo.class, "spin1"); spin1 = hardwareMap.get(Servo.class, "spin1");
spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
spin2 = hardwareMap.get(Servo.class, "spin2"); spin2 = hardwareMap.get(Servo.class, "spin2");
spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
pin0 = hardwareMap.get(DigitalChannel.class, "pin0"); pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
pin1 = hardwareMap.get(DigitalChannel.class, "pin1"); pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
@@ -128,27 +139,26 @@ public class Robot {
pin5 = hardwareMap.get(DigitalChannel.class, "pin5"); pin5 = hardwareMap.get(DigitalChannel.class, "pin5");
analogInput = hardwareMap.get(AnalogInput.class, "analog"); analogInput = hardwareMap.get(AnalogInput.class, "analog");
analogInput2 = hardwareMap.get(AnalogInput.class, "analog2"); analogInput2 = hardwareMap.get(AnalogInput.class, "analog2");
transfer = hardwareMap.get(DcMotorEx.class, "transfer"); transfer = hardwareMap.get(DcMotorEx.class, "transfer");
transferServo = hardwareMap.get(Servo.class, "transferServo"); transferServo = hardwareMap.get(Servo.class, "transferServo");
transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
transfer.setDirection(DcMotorSimple.Direction.REVERSE); transfer.setDirection(DcMotorSimple.Direction.REVERSE);
aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults(); aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
} }
} }