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

turret

Browse Source
This commit is contained in:
Keshav Anand
2026-01-23 21:24:38 -06:00
parent 3591e20001
commit 8a4bfecbf8
5 changed files with 61 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,6 +1,7 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
@@ -55,7 +56,7 @@ public class ShooterTest extends LinearOpMode {
rightShooter.setPower(parameter);
leftShooter.setPower(parameter);
} else if (mode == 1) {
flywheel.setPIDF(P,I,D,F);
flywheel.setPIDF(P, I, D, F);
flywheel.manageFlywheel((int) Velocity);
}
@@ -63,7 +64,6 @@ public class ShooterTest extends LinearOpMode {
robot.hood.setPosition(hoodPos);
}
robot.transfer.setPower(transferPower);
if (shoot) {
robot.transferServo.setPosition(transferServo_in);

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java
View File

@@ -16,6 +16,8 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
@Autonomous
@Config
public class TurretTest extends LinearOpMode {
public static boolean zeroTurr = false;
@Override
public void runOpMode() throws InterruptedException {
@@ -41,6 +43,12 @@ public class TurretTest extends LinearOpMode {
webcam.update();
webcam.displayAllTelemetry();
TELE.addData("tpos", turret.getTurrPos());
if(zeroTurr){
turret.zeroTurretEncoder();
}
TELE.update();

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

@@ -8,6 +8,8 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
@TeleOp
@Config
public class PositionalServoProgrammer extends LinearOpMode {
@@ -25,11 +27,19 @@ public class PositionalServoProgrammer extends LinearOpMode {
public static double hoodPos = 0.501;
public static int mode = 0; //0 for positional, 1 for power
Turret turret;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
servo = new Servos(hardwareMap);
AprilTagWebcam cam = new AprilTagWebcam();
cam.init(robot, TELE);
turret = new Turret(robot, TELE,cam );
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()){
@@ -66,12 +76,13 @@ public class PositionalServoProgrammer extends LinearOpMode {
//TODO: @KeshavAnandCode do the above please
TELE.addData("spindexer pos", servo.getSpinPos());
TELE.addData("turret pos", servo.getTurrPos());
TELE.addData("turret pos", robot.turr1.getPosition());
TELE.addData("spindexer voltage 1", robot.spin1Pos.getVoltage());
TELE.addData("spindexer voltage 2", robot.spin2Pos.getVoltage());
TELE.addData("hood pos", robot.hood.getPosition());
TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage());
TELE.addData("spindexer pow", robot.spin1.getPower());
TELE.addData("tpos ", turret.getTurrPos() );
TELE.update();
}
}

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

@@ -79,10 +79,10 @@ public class Robot {
shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F);
shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
shooter1.setVelocity(1400);
shooter1.setVelocity(0);
shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
shooter2.setVelocity(1400);
shooter2.setVelocity(0);
hood = hardwareMap.get(Servo.class, "hood");

47
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
View File

@@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
@@ -12,13 +13,15 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
public class Turret {
public static double turretTolerance = 0.02;
public static double turrPosScalar = 1;
public static double turrPosScalar = 0.00011264432;
public static double turret180Range = 0.4;
public static double turrDefault = 0.4;
public static double cameraBearingEqual = 1;
public static double errorLearningRate = 0.15;
public static double turrMin = 0.2;
public static double turrMax = 0.8;
public static double mult = 0.0;
private boolean lockOffset = false;
public static double deltaAngleThreshold = 0.02;
public static double angleMultiplier = 0.0;
Robot robot;
@@ -29,7 +32,7 @@ public class Turret {
private double offset = 0.0;
private double bearing = 0.0;
public static double clampTolerance = 0.03;
public Turret(Robot rob, MultipleTelemetry tele, AprilTagWebcam cam) {
this.TELE = tele;
@@ -38,13 +41,18 @@ public class Turret {
}
public double getTurrPos() {
return turrPosScalar * (robot.intake.getCurrentPosition());
return turrPosScalar * (robot.intake.getCurrentPosition()) + turrDefault;
}
public void manualSetTurret(double pos){
public void zeroTurretEncoder() {
robot.intake.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
public void manualSetTurret(double pos) {
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1-pos);
robot.turr2.setPosition(1 - pos);
}
public boolean turretEqual(double pos) {
@@ -89,7 +97,13 @@ public class Turret {
return obeliskID;
}
public void zeroOffset() {
offset = 0.0;
}
public void lockOffset(boolean lock) {
lockOffset = lock;
}
/*
Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
@@ -120,10 +134,7 @@ public class Turret {
//
double tagBearingDeg = getBearing(); // + = target is to the left
if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual) {
// Slowly learn turret offset (persistent calibration)
offset -= tagBearingDeg * errorLearningRate;
}
turretAngleDeg += offset;
@@ -131,8 +142,22 @@ public class Turret {
double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
// Clamp to servo range
turretPos = Math.max(turrMin, Math.min(turretPos, turrMax));
double currentEncoderPos = this.getTurrPos();
if (!turretEqual(turretPos)) {
double diff = turretPos - currentEncoderPos;
turretPos = turretPos + diff * mult;
}
if (currentEncoderPos < (turrMin + clampTolerance) || currentEncoderPos > (turrMax - clampTolerance)) {
// Clamp to servo range
turretPos = Math.max(turrMin, Math.min(turretPos, turrMax));
} else {
if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual && !lockOffset) {
// Slowly learn turret offset (persistent calibration)
offset -= tagBearingDeg * errorLearningRate;
}
}
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1.0 - turretPos);