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

Switched to built in FTC PIDF Controls.

Browse Source
This commit is contained in:
Matt9150
2026-01-18 11:19:54 -06:00
parent d42af20447
commit 59796154bd
6 changed files with 82 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java
View File

@@ -76,7 +76,7 @@ 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.FlywheelV2;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
@@ -93,7 +93,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
FlywheelV2 flywheel;
Flywheel flywheel;
Servos servo;
double velo = 0.0;
boolean gpp = false;
@@ -109,10 +109,8 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
public Action initShooter(int vel) {
return new Action() {
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
TELE.addData("Velocity", velo);
TELE.update();
@@ -180,10 +178,8 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
spinPID = servo.setSpinPos(spindexer);
robot.spin1.setPower(spinPID);
@@ -224,10 +220,8 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
TELE.addLine("shooting");
TELE.update();
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
drive.updatePoseEstimate();
@@ -376,10 +370,8 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
}
ticker++;
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
flywheel.manageFlywheel(vel);
velo = flywheel.getVelo();
double s1D = robot.color1.getDistance(DistanceUnit.MM);
double s2D = robot.color2.getDistance(DistanceUnit.MM);
@@ -454,7 +446,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode {
robot = new Robot(hardwareMap);
flywheel = new FlywheelV2();
flywheel = new Flywheel(hardwareMap);
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
View File

@@ -129,7 +129,7 @@ public class TeleopV2 extends LinearOpMode {
telemetry, FtcDashboard.getInstance().getTelemetry()
);
servo = new Servos(hardwareMap);
flywheel = new Flywheel();
flywheel = new Flywheel(hardwareMap);
drive = new MecanumDrive(hardwareMap, teleStart);
@@ -282,10 +282,7 @@ public class TeleopV2 extends LinearOpMode {
//SHOOTER:
double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition());
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
double powPID = flywheel.manageFlywheel((int) vel);
robot.transfer.setPower(1);

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -29,7 +29,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.FlywheelV2;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
@@ -68,7 +68,7 @@ public class TeleopV3 extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
Servos servo;
FlywheelV2 flywheel;
Flywheel flywheel;
MecanumDrive drive;
double autoHoodOffset = 0.0;
@@ -140,7 +140,7 @@ public class TeleopV3 extends LinearOpMode {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
servo = new Servos(hardwareMap);
flywheel = new FlywheelV2();
flywheel = new Flywheel(hardwareMap);
drive = new MecanumDrive(hardwareMap, teleStart);
PIDFController tController = new PIDFController(tp, ti, td, tf);
@@ -400,10 +400,7 @@ public class TeleopV3 extends LinearOpMode {
//SHOOTER:
double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
flywheel.manageFlywheel(vel);
//VELOCITY AUTOMATIC
@@ -784,8 +781,7 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("distanceToGoal", distanceToGoal);
TELE.addData("hood", robot.hood.getPosition());
TELE.addData("targetVel", vel);
TELE.addData("Velocity", flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()));
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("shootOrder", shootOrder);
TELE.addData("oddColor", oddBallColor);

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

@@ -9,7 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import org.firstinspires.ftc.teamcode.utils.FlywheelV2;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
@@ -21,12 +21,17 @@ public class ShooterTest extends LinearOpMode {
// --- CONSTANTS YOU TUNE ---
//TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED
public static double Velocity = 0.0;
public static double P = 40.0;
public static double I = 0.3;
public static double D = 7.0;
public static double F = 10.0;
public static double transferPower = 1.0;
public static double hoodPos = 0.501;
public static double turretPos = 0.501;
public static boolean shoot = false;
Robot robot;
FlywheelV2 flywheel;
Flywheel flywheel;
@Override
public void runOpMode() throws InterruptedException {
@@ -34,7 +39,7 @@ public class ShooterTest extends LinearOpMode {
robot = new Robot(hardwareMap);
DcMotorEx leftShooter = robot.shooter1;
DcMotorEx rightShooter = robot.shooter2;
flywheel = new FlywheelV2();
flywheel = new Flywheel(hardwareMap);
MultipleTelemetry TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
@@ -50,10 +55,8 @@ public class ShooterTest extends LinearOpMode {
rightShooter.setPower(parameter);
leftShooter.setPower(parameter);
} else if (mode == 1) {
double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
rightShooter.setPower(powPID);
leftShooter.setPower(powPID);
TELE.addData("PIDPower", powPID);
flywheel.setPIDF(P,I,D,F);
flywheel.manageFlywheel((int) Velocity);
}
if (hoodPos != 0.501) {
@@ -67,7 +70,7 @@ public class ShooterTest extends LinearOpMode {
} else {
robot.transferServo.setPosition(transferServo_out);
}
TELE.addData("Velocity", flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()));
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Velocity 1", flywheel.getVelo1());
TELE.addData("Velocity 2", flywheel.getVelo2());
TELE.addData("Power", robot.shooter1.getPower());

80
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java
View File

@@ -2,6 +2,9 @@ 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.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -17,72 +20,65 @@ public class Flywheel {
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 targetVelocity = 0.0;
double powPID = 0.0;
boolean steady = false;
public Flywheel () {
//robot = new Robot(hardwareMap);
public Flywheel (HardwareMap hardwareMap) {
robot = new Robot(hardwareMap);
}
public double getVelo () {
return velo;
}
public double getVelo1 () {
return velo1;
}
public double getVelo2 () {
return velo2;
}
public boolean getSteady() {
return steady;
}
// Set the robot PIDF for the next cycle.
public void setPIDF(double p, double i, double d, double f) {
robot.shooterPIDF.p = p;
robot.shooterPIDF.i = i;
robot.shooterPIDF.d = d;
robot.shooterPIDF.f = f;
}
private double getTimeSeconds ()
{
return (double) System.currentTimeMillis()/1000.0;
}
// Convert from RPM to Ticks per Second
private double RPM_to_TPS (double RPM) { return (RPM*28.0)/60.0;}
public double manageFlywheel(int commandedVelocity, double shooter1CurPos) {
// Convert from Ticks per Second to RPM
private double TPS_to_RPM (double TPS) { return (TPS*60.0)/28.0;}
public double manageFlywheel(double commandedVelocity) {
targetVelocity = commandedVelocity;
ticker++;
if (ticker % 2 == 0) {
velo5 = velo4;
velo4 = velo3;
velo3 = velo2;
velo2 = velo1;
// Turn PIDF for Target Velocities
//robot.shooterPIDF.p = P;
//robot.shooterPIDF.i = I;
//robot.shooterPIDF.d = D;
//robot.shooterPIDF.f = F;
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, robot.shooterPIDF);
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, robot.shooterPIDF);
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
currentPos = shooter1CurPos / 2048;
stamp = getTimeSeconds(); //getRuntime();
velo1 = -60 * ((currentPos - initPos) / (stamp - stamp1));
initPos = currentPos;
stamp1 = stamp;
velo = (velo1 + velo2 + velo3 + velo4 + velo5) / 5;
}
// 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));
}
// Record Current Velocity
velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
velo2 = TPS_to_RPM(robot.shooter1.getVelocity());
velo = Math.max(velo1,velo2);
// really should be a running average of the last 5
steady = (Math.abs(targetVelocity - velo) < 100.0);
steady = (Math.abs(targetVelocity - velo) < 200.0);
return powPID;
}

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

@@ -8,6 +8,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
@@ -23,6 +24,13 @@ public class Robot {
public DcMotorEx backRight;
public DcMotorEx intake;
public DcMotorEx transfer;
public PIDFCoefficients shooterPIDF;
public double shooterPIDF_P = 10.0;
public double shooterPIDF_I = 0.6;
public double shooterPIDF_D = 5.0;
public double shooterPIDF_F = 10.0;
public double[] shooterPIDF_StepSizes = {10.0,1.0,0.001, 0.0001};
public DcMotorEx shooter1;
public DcMotorEx shooter2;
public Servo hood;
@@ -69,8 +77,11 @@ public class Robot {
shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
//TODO: figure out which shooter motor is reversed using ShooterTest and change it in code @KeshavAnandCode
shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
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);
shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
hood = hardwareMap.get(Servo.class, "hood");