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

11/26 edits

Browse Source
This commit is contained in:
DanTheMan-byte
2025-11-26 19:03:43 -06:00
parent d639530fa9
commit 84c9b08205
8 changed files with 252 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

203
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java Normal file
View File

@@ -0,0 +1,203 @@
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 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.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
@Autonomous
public class redDaniel extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
AprilTag aprilTag;
public Action initShooter(int velocity){
return new Action(){
double velo = 0.0;
double initPos = 0.0;
double stamp = 0.0;
double powPID = 0.0;
final int maxVel = 4500;
double ticker = 0.0;
public boolean run(@NonNull TelemetryPacket telemetryPacket){
velo = -60 * ((((double) robot.shooter1.getCurrentPosition() / 2048) - initPos) / (getRuntime() - stamp));
stamp = getRuntime();
initPos = (double) robot.shooter1.getCurrentPosition() / 2048;
if (Math.abs(velocity - velo) > 3 * velTolerance) {
powPID = (double) velocity / maxVel;
ticker = getRuntime();
} else if (velocity - velTolerance > velo) {
powPID = powPID + 0.0001;
ticker = getRuntime();
} else if (velocity + velTolerance < velo) {
powPID = powPID - 0.0001;
ticker = getRuntime();
}
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
robot.transfer.setPower((powPID / 2) + 0.5);
return getRuntime() - ticker < 0.5;
}
};
}
public Action Shoot(double spindexer){
return new Action() {
boolean transfer = false;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
robot.spin1.setPosition(spindexer);
robot.spin2.setPosition(1-spindexer);
if (robot.spin1Pos.getVoltage() / 3.3 == spindexer){
robot.transferServo.setPosition(transferServo_in);
transfer = true;
}
if (robot.transferServoPos.getVoltage() / 3.3 == transferServo_in && transfer){
robot.transferServo.setPosition(transferServo_out);
return false;
}
return true;
}
};
}
@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);
TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(x1, y1), h1);
TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.turnTo(Math.toRadians(h2))
.strafeToLinearHeading(new Vector2d(x2, y2), h2);
TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(x2, y2, h2))
.strafeToLinearHeading(new Vector2d(x1, y1), h1);
TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x2_b, y2_b), h2_b)
.strafeToLinearHeading(new Vector2d(x3, y3), h3);
TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(x3, y3, h3))
.strafeToLinearHeading(new Vector2d(x1, y1), h1);
TrajectoryActionBuilder park = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x1, y1 + 30), h1);
while (opModeInInit()) {
if (gamepad2.dpadUpWasPressed()) {
hoodDefault -= 0.01;
}
if (gamepad2.dpadDownWasPressed()) {
hoodDefault += 0.01;
}
robot.hood.setPosition(hoodDefault);
robot.turr1.setPosition(turret_red);
robot.turr2.setPosition(1 - turret_red);
robot.transferServo.setPosition(transferServo_out);
aprilTag.initTelemetry();
aprilTag.update();
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
if (opModeIsActive()) {
robot.hood.setPosition(hoodDefault);
Actions.runBlocking(
new ParallelAction(
shoot0.build()
)
);
Actions.runBlocking(
pickup1.build()
);
Actions.runBlocking(
shoot1.build()
);
Actions.runBlocking(
pickup2.build()
);
Actions.runBlocking(
shoot2.build()
);
Actions.runBlocking(
park.build()
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addLine("finished");
TELE.update();
sleep(2000);
}
}
}

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

@@ -15,12 +15,12 @@ public class Poses {
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(135);
public static double x3 = 34, y3 = 58, h3 = Math.toRadians(140);
public static Pose2d teleStart = new Pose2d(x1,-10,0);

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

@@ -15,12 +15,16 @@ public class ServoPositions {
public static double transferServo_out = 0.13;
public static double transferServo_in = 0.4;
public static double transferServo_in = 0.36;
public static double hoodDefault = 0.35;
public static double turret_red = 0.33;
public static double turret_blue = 0.3;
public static double hoodHigh = 0.21;
public static double hoodLow = 1.0;
public static double turret_red = 0.43;
public static double turret_blue = 0.4;
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java
View File

@@ -7,4 +7,6 @@ public class ShooterVars {
public static double turret_GearRatio = 0.9974;
public static double turret_Range = 355;
public static int velTolerance = 500;
}

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

@@ -83,7 +83,7 @@ public class ShooterTest extends LinearOpMode {
robot.hood.setPosition(hoodPos);
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(turretPos);
robot.turr2.setPosition(1-turretPos);
velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1));
stamp1 = getRuntime();
@@ -91,9 +91,9 @@ public class ShooterTest extends LinearOpMode {
if (Math.abs(vel - velo1) > 3 * tolerance) {
powPID = vel / maxVel;
} else if (vel - tolerance > velo1) {
powPID = powPID + 0.001;
powPID = powPID + 0.0001;
} else if (vel + tolerance < velo1) {
powPID = powPID - 0.001;
powPID = powPID - 0.0001;
}
shooter.setVelocity(powPID);
robot.transfer.setPower((powPID / 2) + 0.5);

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java
View File

@@ -11,7 +11,7 @@ public class ConfigureColorRangefinder extends LinearOpMode {
public static int LED_Brightness = 50;
public static int lowerGreen = 100;
public static int lowerGreen = 120;
public static int higherGreen = 150;

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java
View File

@@ -1,6 +1,8 @@
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;
@@ -8,6 +10,7 @@ import com.qualcomm.robotcore.eventloop.opmode.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;
@@ -15,6 +18,7 @@ public class PositionalServoProgrammer extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()){
@@ -32,6 +36,11 @@ public class PositionalServoProgrammer extends LinearOpMode {
if (hoodPos != 0.501){
robot.hood.setPosition(hoodPos);
}
TELE.addData("spindexer", 1.111111111*((robot.spin1Pos.getVoltage() / 3.3) - 0.05));
TELE.addData("hood", 1.111111111*((robot.hoodPos.getVoltage() / 3.3) - 0.05));
TELE.addData("transferServo", 1.111111111*((robot.transferServoPos.getVoltage() / 3.3) - 0.05));
TELE.addData("turret", 1.111111111*((robot.turr1Pos.getVoltage() / 3.3) - 0.05));
TELE.update();
}
}
}

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

@@ -59,6 +59,18 @@ public class Robot {
public AnalogInput analogInput2;
public AnalogInput spin1Pos;
public AnalogInput spin2Pos;
public AnalogInput hoodPos;
public AnalogInput turr1Pos;
public AnalogInput turr2Pos;
public AnalogInput transferServoPos;
public AprilTagProcessor aprilTagProcessor;
public WebcamName webcam;
@@ -92,14 +104,24 @@ public class Robot {
hood = hardwareMap.get(Servo.class, "hood");
hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");
turr1 = hardwareMap.get(Servo.class, "t1");
turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos");
turr2 = hardwareMap.get(Servo.class, "t2");
turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos");
spin1 = hardwareMap.get(Servo.class, "spin1");
spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
spin2 = hardwareMap.get(Servo.class, "spin2");
spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
@@ -120,6 +142,8 @@ public class Robot {
transferServo = hardwareMap.get(Servo.class, "transferServo");
transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
transfer.setDirection(DcMotorSimple.Direction.REVERSE);
aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();