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

velocity math: to test

Browse Source
This commit is contained in:
DanTheMan-byte
2025-11-18 22:47:58 -06:00
parent e5ba0947e3
commit 8d75b245e3
4 changed files with 88 additions and 199 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
View File

@@ -2,9 +2,12 @@ package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.tests.ShooterTest.*;
import static java.lang.Runtime.getRuntime;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.controller.PIDController;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.PIDCoefficients;
@@ -42,7 +45,7 @@ public class Shooter implements Subsystem {
private double p = 0.0003, i = 0, d = 0.00001;
private PIDController controller;
private PIDFController controller;
private double pow = 0.0;
private String shooterMode = "AUTO";
@@ -62,9 +65,9 @@ public class Shooter implements Subsystem {
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
controller = new PIDController(p, i, d);
controller = new PIDFController(p, i, d, f);
controller.setPID(p, i, d);
controller.setPIDF(p, i, d, f);
this.turret1 = robot.turr1;
@@ -83,11 +86,9 @@ public class Shooter implements Subsystem {
telemetry.addData("ECPR Revolutions", getECPRPosition());
telemetry.addData("MCPR Revolutions", getMCPRPosition());
telemetry.addData("TargetPosition", targetPosition);
telemetry.addData("Velocity", getVelocity(mcpr) * 60);
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("Power Fly 1", fly1.getPower());
telemetry.addData("Power Fly 2", fly2.getPower());
telemetry.addData("Pow", pow);
@@ -106,8 +107,8 @@ public class Shooter implements Subsystem {
public void setTurretPosition(double pos) { turretPos = pos; }
public double getVelocity(double cpr) {
return 60 * (fly1.getVelocity() / (2 * cpr));
public double getVelocity(double initPos) {
return (getECPRPosition() - initPos);
}
public void setVelocity(double vel) { velocity = vel; }
@@ -122,11 +123,12 @@ public class Shooter implements Subsystem {
controller.setTolerance(tolerance);
}
public void setControllerCoefficients(double kp, double ki, double kd) {
public void setControllerCoefficients(double kp, double ki, double kd, double kf) {
p = kp;
i = ki;
d = kd;
controller.setPID(p, i, d);
f = kf;
controller.setPIDF(p, i, d, f);
}
@@ -143,11 +145,11 @@ public class Shooter implements Subsystem {
public String getTurretMode() { return turretMode; }
public double getECPRPosition() {
return (fly1.getCurrentPosition() * 720 / ecpr);
return fly1.getCurrentPosition() / (2 * ecpr);
}
public double getMCPRPosition() {
return (fly1.getCurrentPosition() * 720) / mcpr;
return fly1.getCurrentPosition() / (2 * mcpr);
}
public void setShooterMode(String mode) { shooterMode = mode; }
@@ -250,7 +252,7 @@ public class Shooter implements Subsystem {
fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
double powPID = controller.calculate(getVelocity(mcpr) * ecpr, velocity);
double powPID = controller.calculate(fly1.getVelocity(), velocity);
fly1.setPower(powPID);
fly2.setPower(powPID);

176
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java
View File

@@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Poses.*;
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;
@@ -23,13 +22,11 @@ import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
import org.firstinspires.ftc.teamcode.subsystems.Transfer;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
@TeleOp
public class TeleopV1 extends LinearOpMode {
Robot robot;
Drivetrain drivetrain;
@@ -64,7 +61,6 @@ public class TeleopV1 extends LinearOpMode {
ToggleButtonReader g2Square;
ToggleButtonReader g2Triangle;
ToggleButtonReader g2RightBumper;
@@ -86,7 +82,6 @@ public class TeleopV1 extends LinearOpMode {
public double g1LeftBumperStamp = 0.0;
public double g2LeftBumperStamp = 0.0;
public static int spindexerPos = 0;
@@ -97,7 +92,7 @@ public class TeleopV1 extends LinearOpMode {
public boolean scoreAll = false;
MecanumDrive drive ;
MecanumDrive drive;
public boolean autotrack = false;
@@ -118,17 +113,11 @@ public class TeleopV1 extends LinearOpMode {
public boolean tri = false;
@Override
public void runOpMode() throws InterruptedException {
drive = new MecanumDrive(hardwareMap, teleStart);
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(
@@ -164,7 +153,6 @@ public class TeleopV1 extends LinearOpMode {
g2, GamepadKeys.Button.RIGHT_BUMPER
);
g2LeftBumper = new ToggleButtonReader(
g2, GamepadKeys.Button.LEFT_BUMPER
);
@@ -173,7 +161,6 @@ public class TeleopV1 extends LinearOpMode {
g2, GamepadKeys.Button.DPAD_UP
);
g2DpadDown = new ToggleButtonReader(
g2, GamepadKeys.Button.DPAD_DOWN
);
@@ -182,14 +169,10 @@ public class TeleopV1 extends LinearOpMode {
g2, GamepadKeys.Button.DPAD_LEFT
);
g2DpadRight = new ToggleButtonReader(
g2, GamepadKeys.Button.DPAD_RIGHT
);
drivetrain = new Drivetrain(robot, TELE, g1);
drivetrain.setMode("Default");
@@ -202,7 +185,6 @@ public class TeleopV1 extends LinearOpMode {
transfer = new Transfer(robot);
spindexer = new Spindexer(robot, TELE);
spindexer.setTelemetryOn(true);
@@ -213,17 +195,13 @@ public class TeleopV1 extends LinearOpMode {
robot.rejecter.setPosition(rIn);
waitForStart();
if (isStopRequested()) return;
drive = new MecanumDrive(hardwareMap, teleStart);
while(opModeIsActive()){
while (opModeIsActive()) {
drive.updatePoseEstimate();
@@ -231,29 +209,25 @@ public class TeleopV1 extends LinearOpMode {
TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
TELE.addData("off", offset);
robot.transferServo.setPosition(transferServoPos);
robot.hood.setPosition(pos);
g1LeftBumper.readValue();
if (g1LeftBumper.wasJustPressed()){
if (g1LeftBumper.wasJustPressed()) {
g2LeftBumperStamp = getRuntime();
spindexer.intakeShake(getRuntime());
leftBumper = true;
}
if (leftBumper){
if (leftBumper) {
double time = getRuntime() - g2LeftBumperStamp;
if (time < 1.0){
if (time < 1.0) {
robot.rejecter.setPosition(rOut);
} else {
robot.rejecter.setPosition(rIn);
@@ -261,9 +235,6 @@ public class TeleopV1 extends LinearOpMode {
}
intake();
drivetrain.update();
@@ -280,63 +251,49 @@ public class TeleopV1 extends LinearOpMode {
g2DpadUp.readValue();
if (!scoreAll){
if (!scoreAll) {
spindexer.checkForBalls();
}
if(g2DpadUp.wasJustPressed()){
pos -=0.02;
if (g2DpadUp.wasJustPressed()) {
pos -= 0.02;
}
if(g2DpadDown.wasJustPressed()){
pos +=0.02;
if (g2DpadDown.wasJustPressed()) {
pos += 0.02;
}
g2DpadLeft.readValue();
g2DpadRight.readValue();
if(g2DpadLeft.wasJustPressed()){
offset -=0.02;
if (g2DpadLeft.wasJustPressed()) {
offset -= 0.02;
}
if(g2DpadRight.wasJustPressed()){
offset +=0.02;
if (g2DpadRight.wasJustPressed()) {
offset += 0.02;
}
TELE.addData("hood", pos);
if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) {
shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset);
} else {
autotrack = false;
shooter.moveTurret(0.3+offset);
shooter.moveTurret(0.3 + offset);
}
if (gamepad2.right_stick_button){
if (gamepad2.right_stick_button) {
autotrack = true;
}
if (g2RightBumper.wasJustPressed()){
if (g2RightBumper.wasJustPressed()) {
transfer.setTransferPower(1);
transfer.transferIn();
shooter.setManualPower(1);
@@ -345,19 +302,19 @@ public class TeleopV1 extends LinearOpMode {
}
if (g2RightBumper.wasJustReleased()){
if (g2RightBumper.wasJustReleased()) {
transfer.setTransferPower(1);
transfer.transferOut();
}
if (gamepad2.left_stick_y>0.5){
if (gamepad2.left_stick_y > 0.5) {
shooter.setManualPower(0);
} else if (gamepad2.left_stick_y<-0.5){
} else if (gamepad2.left_stick_y < -0.5) {
shooter.setManualPower(1);
}
if (g2LeftBumper.wasJustPressed()){
if (g2LeftBumper.wasJustPressed()) {
g2LeftBumperStamp = getRuntime();
notShooting = false;
scoreAll = true;
@@ -366,7 +323,6 @@ public class TeleopV1 extends LinearOpMode {
if (scoreAll) {
double time = getRuntime() - g2LeftBumperStamp;
shooter.setManualPower(1);
TELE.addData("greenImportant", green);
@@ -378,7 +334,6 @@ public class TeleopV1 extends LinearOpMode {
if (square) {
if (time < 0.3) {
ticker = 0;
@@ -386,7 +341,6 @@ public class TeleopV1 extends LinearOpMode {
last = 0;
second = 0;
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 2) {
@@ -407,14 +361,12 @@ public class TeleopV1 extends LinearOpMode {
ticker++;
} else if (time < 2.5) {
ticker = 0;
second = last;
transfer.transferIn();
} else if (time < 4) {
transfer.transferOut();
@@ -434,11 +386,9 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0;
transfer.transferIn();
} else if (time < 6) {
transfer.transferOut();
if (ticker == 0) {
@@ -459,7 +409,6 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0;
scoreAll = false;
transfer.transferOut();
@@ -468,7 +417,6 @@ public class TeleopV1 extends LinearOpMode {
}
} else if (tri) {
if (time < 0.3) {
ticker = 0;
@@ -476,7 +424,6 @@ public class TeleopV1 extends LinearOpMode {
last = 0;
second = 0;
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 2) {
@@ -497,14 +444,12 @@ public class TeleopV1 extends LinearOpMode {
ticker++;
} else if (time < 2.5) {
ticker = 0;
second = last;
transfer.transferIn();
} else if (time < 4) {
transfer.transferOut();
@@ -524,11 +469,9 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0;
transfer.transferIn();
} else if (time < 6) {
transfer.transferOut();
if (ticker == 0) {
@@ -549,15 +492,13 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0;
scoreAll = false;
transfer.transferOut();
shooter.setManualPower(0);
}
} else if (circle){
} else if (circle) {
if (time < 0.3) {
@@ -566,7 +507,6 @@ public class TeleopV1 extends LinearOpMode {
last = 0;
second = 0;
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 2) {
@@ -587,14 +527,12 @@ public class TeleopV1 extends LinearOpMode {
ticker++;
} else if (time < 2.5) {
ticker = 0;
second = last;
transfer.transferIn();
} else if (time < 4) {
transfer.transferOut();
@@ -614,11 +552,9 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0;
transfer.transferIn();
} else if (time < 6) {
transfer.transferOut();
if (ticker == 0) {
@@ -639,15 +575,13 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0;
scoreAll = false;
transfer.transferOut();
shooter.setManualPower(0);
}
} else{
} else {
if (time < 0.3) {
@@ -669,7 +603,6 @@ public class TeleopV1 extends LinearOpMode {
all = true;
}
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 2) {
@@ -694,7 +627,6 @@ public class TeleopV1 extends LinearOpMode {
ticker++;
} else if (time < 2.5) {
ticker = 0;
@@ -711,7 +643,6 @@ public class TeleopV1 extends LinearOpMode {
all = false;
}
transfer.transferIn();
@@ -737,7 +668,6 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0;
if (gamepad2.right_trigger > 0.5) {
green = false;
@@ -752,7 +682,6 @@ public class TeleopV1 extends LinearOpMode {
transfer.transferIn();
} else if (time < 6) {
transfer.transferOut();
if (ticker == 0) {
@@ -775,7 +704,6 @@ public class TeleopV1 extends LinearOpMode {
ticker = 0;
scoreAll = false;
transfer.transferOut();
@@ -783,33 +711,16 @@ public class TeleopV1 extends LinearOpMode {
}
}
}
shooter.update();
}
}
}
public void intake(){
public void intake() {
g1RightBumper.readValue();
@@ -819,84 +730,63 @@ public class TeleopV1 extends LinearOpMode {
g2Triangle.readValue();
if (g1RightBumper.wasJustPressed()){
if (g1RightBumper.wasJustPressed()) {
notShooting = true;
if (getRuntime() - g1RightBumperStamp < 0.3){
if (getRuntime() - g1RightBumperStamp < 0.3) {
intake.reverse();
} else {
intake.toggle();
}
if (intake.getIntakeState()==1){
if (intake.getIntakeState() == 1) {
shooter.setManualPower(0);
}
spindexer.intake();
transfer.transferOut();
g1RightBumperStamp = getRuntime();
}
if (intake.getIntakeState()==1 && notShooting) {
if (intake.getIntakeState() == 1 && notShooting) {
spindexer.intakeShake(getRuntime());
} else {
if (g2Circle.wasJustPressed()){
if (g2Circle.wasJustPressed()) {
circle = true;
tri = false;
square = false;
}
if (g2Triangle.wasJustPressed()){
if (g2Triangle.wasJustPressed()) {
circle = false;
tri = true;
square = false;
}
if (g2Square.wasJustPressed()){
if (g2Square.wasJustPressed()) {
circle = false;
tri = false;
square = true;
}
if (gamepad2.x){
if (gamepad2.x) {
circle = false;
tri = false;
square = false;
}
}
intake.update();
spindexer.update();
}
}

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

@@ -1,8 +1,14 @@
package org.firstinspires.ftc.teamcode.tests;
import android.app.Notification;
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.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -20,12 +26,14 @@ public class ShooterTest extends LinearOpMode {
public static double pow = 0.0;
public static double vel = 0.0;
public static double mcpr = 28; // CPR of the motor
public static double mcpr = 28.0; // CPR of the motor
public static double ecpr = 1024.0; // CPR of the encoder
public static int pos = 0;
public static double posPower = 0.0;
public static double p = 0.001, i = 0, d = 0;
public static double posi = 0.5;
public static double p = 0.001, i = 0, d = 0, f = 0;
public static String flyMode = "MANUAL";
@@ -35,8 +43,18 @@ public class ShooterTest extends LinearOpMode {
public static double servoPosition = 0.501;
public double stamp = 0.0;
public double initPos = 0.0;
public static double time = 1.0;
MultipleTelemetry TELE;
public boolean wait(double time) {
return (getRuntime() - stamp) > time;
}
@Override
public void runOpMode() throws InterruptedException {
@@ -52,7 +70,7 @@ public class ShooterTest extends LinearOpMode {
shooter.setShooterMode(flyMode);
shooter.setControllerCoefficients(p, i, d);
shooter.setControllerCoefficients(p, i, d, f);
waitForStart();
@@ -60,7 +78,7 @@ public class ShooterTest extends LinearOpMode {
while (opModeIsActive()) {
shooter.setControllerCoefficients(p, i, d);
shooter.setControllerCoefficients(p, i, d, f);
shooter.setTurretMode(turrMode);
@@ -72,6 +90,8 @@ public class ShooterTest extends LinearOpMode {
shooter.setTargetPosition(pos);
shooter.setTurretPosition(posi);
shooter.setTolerance(posTolerance);
shooter.setPosPower(posPower);
@@ -80,6 +100,11 @@ public class ShooterTest extends LinearOpMode {
shooter.update();
if (wait(time)){
telemetry.addData("Velocity", shooter.getVelocity(initPos));
stamp = getRuntime();
initPos = shooter.getECPRPosition();
}
TELE.update();
}

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

@@ -30,8 +30,6 @@ public class Robot {
public DcMotorEx transfer;
public DcMotorEx shooter1;
public DcMotorEx shooter2;
public Servo hood;
@@ -63,25 +61,11 @@ public class Robot {
public AprilTagProcessor aprilTagProcessor;
public WebcamName webcam;
public DcMotorEx shooterEncoder;
public Robot (HardwareMap hardwareMap) {
public Robot(HardwareMap hardwareMap) {
//Define components w/ hardware map
@@ -106,10 +90,6 @@ public class Robot {
shooter2.setDirection(DcMotorSimple.Direction.REVERSE);
hood = hardwareMap.get(Servo.class, "hood");
turr1 = hardwareMap.get(Servo.class, "t1");
@@ -132,11 +112,8 @@ public class Robot {
pin5 = hardwareMap.get(DigitalChannel.class, "pin5");
analogInput = hardwareMap.get(AnalogInput.class, "analog");
analogInput2 = hardwareMap.get(AnalogInput.class, "analog2");
transfer = hardwareMap.get(DcMotorEx.class, "transfer");
@@ -147,12 +124,7 @@ public class Robot {
aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
}
}