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

added continous servos and smoother velocity PID

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-02 00:17:28 -06:00
parent 05412940e8
commit 0bf392f81f
7 changed files with 152 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java
View File

@@ -5,7 +5,7 @@ import static org.firstinspires.ftc.teamcode.constants.Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
import androidx.annotation.NonNull;

93
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java
View File

@@ -3,7 +3,7 @@ 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 static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
import androidx.annotation.NonNull;
@@ -42,7 +42,6 @@ public class Red_V2 extends LinearOpMode {
Flywheel flywheel;
double velo = 0.0;
double targetVelocity = 0.0;
public static double intake1Time = 2.9;
public static double intake2Time = 2.9;
@@ -64,7 +63,6 @@ public class Red_V2 extends LinearOpMode {
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 &&
@@ -73,26 +71,23 @@ public class Red_V2 extends LinearOpMode {
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());
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
@@ -121,7 +116,7 @@ public class Red_V2 extends LinearOpMode {
double stamp = 0.0;
boolean steady = false;
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
steady = flywheel.getSteady();
robot.shooter1.setPower(powPID);
@@ -186,46 +181,12 @@ public class Red_V2 extends LinearOpMode {
};
}
public Action spindex (double spindexer, double vel){
public Action spindex (double spindexer, int 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());
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
@@ -244,51 +205,18 @@ public class Red_V2 extends LinearOpMode {
};
}
public Action Shoot(double vel) {
public Action Shoot(int 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());
powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
@@ -297,7 +225,6 @@ public class Red_V2 extends LinearOpMode {
teleStart = drive.localizer.getPose();
if (ticker == 1) {
transferStamp = getRuntime();
ticker++;
@@ -560,7 +487,7 @@ public class Red_V2 extends LinearOpMode {
teleStart = drive.localizer.getPose();
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
@@ -596,7 +523,7 @@ public class Red_V2 extends LinearOpMode {
teleStart = drive.localizer.getPose();
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
@@ -626,7 +553,7 @@ public class Red_V2 extends LinearOpMode {
)
);
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);

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

@@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
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 static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;

76
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java Normal file
View File

@@ -0,0 +1,76 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.controller.PIDController;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import org.firstinspires.ftc.teamcode.utils.Robot;
@TeleOp
@Config
public class PIDServoTest extends LinearOpMode {
public static double p = 0.0003, i = 0, d = 0.00001;
public static double target = 0.5;
public static int mode = 0; //0 is for turret, 1 is for spindexer
public static double scalar = 1.112;
public static double restPos = 0.15;
Robot robot;
double pos = 0.0;
@Override
public void runOpMode() throws InterruptedException {
PIDController controller = new PIDController(p, i, d);
controller.setTolerance(0);
robot = new Robot(hardwareMap);
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
controller.setPID(p, i, d);
if (mode == 0) {
pos = scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3);
double pid = controller.calculate(pos, target);
robot.turr1.setPosition(pid);
robot.turr2.setPosition(-pid);
} else if (mode == 1) {
pos = scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3);
double pid = controller.calculate(pos, target);
robot.spin1.setPosition(pid);
robot.spin2.setPosition(-pid);
}
telemetry.addData("pos", pos);
telemetry.addData("Turret Voltage", robot.turr1Pos.getVoltage());
telemetry.addData("Spindex Voltage", robot.spin1Pos.getVoltage());
telemetry.addData("target", target);
telemetry.addData("Mode", mode);
telemetry.update();
}
}
}

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

@@ -1,7 +1,5 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -9,6 +7,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.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
@@ -18,7 +17,6 @@ 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
//TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED
public static double transferPower = 0.0;
@@ -26,8 +24,8 @@ public class ShooterTest extends LinearOpMode {
public static double turretPos = 0.501;
Robot robot;
private double lastEncoderRevolutions = 0.0;
private double lastTimeStamp = 0.0;
Flywheel flywheel;
@Override
public void runOpMode() throws InterruptedException {
@@ -35,7 +33,6 @@ public class ShooterTest extends LinearOpMode {
robot = new Robot(hardwareMap);
DcMotorEx leftShooter = robot.shooter1;
DcMotorEx rightShooter = robot.shooter2;
DcMotorEx encoder = robot.shooter1;
MultipleTelemetry TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
@@ -47,50 +44,19 @@ public class ShooterTest extends LinearOpMode {
while (opModeIsActive()) {
double kF = 1.0 / MAX_RPM; // baseline feedforward
double encoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
double velocity = -60 * (encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp);
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);
double velPID;
if (mode == 0) {
rightShooter.setPower(parameter);
leftShooter.setPower(parameter);
} else if (mode == 1) {
// --- FEEDFORWARD BASE POWER ---
double feed = kF * parameter; // Example: vel=2500 → feed=0.5
// --- PROPORTIONAL CORRECTION ---
double error = parameter - velocity;
double correction = kP * error;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
velPID = feed + correction;
// clamp to allowed range
velPID = Math.max(0, Math.min(1, velPID));
rightShooter.setPower(velPID);
leftShooter.setPower(velPID);
double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition());
TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("PIDPower", powPID);
TELE.addData("Power", robot.shooter1.getPower());
TELE.addData("Steady?", flywheel.getSteady());
}
lastTimeStamp = getRuntime();
lastEncoderRevolutions = (double) encoder.getCurrentPosition() / 2048;
if (hoodPos != 0.501) {
robot.hood.setPosition(hoodPos);
}

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

@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.utils;
import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -16,8 +18,6 @@ public class PositionalServoProgrammer extends LinearOpMode {
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);
@@ -43,10 +43,10 @@ public class PositionalServoProgrammer extends LinearOpMode {
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.addData("spindexer voltage", robot.spin1Pos.getVoltage());
TELE.addData("hood voltage", robot.hoodPos.getVoltage());
TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage());
TELE.addData("turret voltage", robot.turr1Pos.getVoltage());
TELE.update();
}
}

50
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java Normal file
View File

@@ -0,0 +1,50 @@
package org.firstinspires.ftc.teamcode.utils;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.controller.PIDController;
public class Servos {
PIDController spinPID;
PIDController turretPID;
//PID constants
public static double spinP = 0.0, spinI = 0.0, spinD = 0.0;
public static double turrP = 0.0, turrI = 0.0, turrD = 0.0;
public static double spin_scalar = 0.15;
public static double spin_restPos = 1.112;
public static double turret_scalar = 0.15;
public static double turret_restPos = 1.112;
public void initServos() {
spinPID = new PIDController(spinP, spinI, spinD);
turretPID = new PIDController(turrP, turrI, turrD);
}
// In the code below, encoder = robot.servo.getVoltage()
public double getSpinPos(double encoder){
return spin_scalar * ((encoder - spin_restPos) / 3.3);
}
public double setSpinPos(double pos, double encoder){
spinPID.setPID(spinP, spinI, spinD);
return spinPID.calculate(this.getSpinPos(encoder), pos);
}
public boolean spinEqual(double pos, double encoder){
return Math.abs(pos - this.getSpinPos(encoder)) < 0.01;
}
public double getTurrPos(double encoder){
return turret_scalar * ((encoder - turret_restPos) / 3.3);
}
public double setTurrPos(double pos, double encoder){
turretPID.setPID(turrP, turrI, turrD);
return spinPID.calculate(this.getTurrPos(encoder), pos);
}
public boolean turretEqual(double pos, double encoder){
return Math.abs(pos - this.getTurrPos(encoder)) < 0.01;
}
}