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

latest push

Browse Source
This commit is contained in:
DanTheMan-byte
2025-11-24 17:56:14 -06:00
parent 77b42acdda
commit 582ea86ac5
3 changed files with 44 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,7 +2,6 @@ package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.tests.ShooterTest.*;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.controller.PIDController;
@@ -44,6 +43,8 @@ public class Shooter implements Subsystem {
private int targetPosition = 0;
public double powPID = 1.0;
private double p = 0.0003, i = 0, d = 0.00001;
private PIDFController controller;
@@ -90,8 +91,6 @@ public class Shooter implements Subsystem {
public void setTurretPosition(double pos) { turretPos = pos; }
public double getVelocity(double vel) {
return vel;
}
@@ -224,6 +223,10 @@ public class Shooter implements Subsystem {
turret2.setPosition(1 - pos);
}
public double getpowPID() {
return powPID;
}
@Override
public void update() {
@@ -234,10 +237,7 @@ public class Shooter implements Subsystem {
fly1.setPower(manualPower);
fly2.setPower(manualPower);
} else if (Objects.equals(shooterMode, "VEL")) {
fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
double powPID = controller.calculate(-getVelocity(velo), velocity);
powPID = velocity;
fly1.setPower(powPID);
fly2.setPower(powPID);
@@ -245,7 +245,7 @@ public class Shooter implements Subsystem {
}
if (Objects.equals(turretMode, "MANUAL")) {
// hoodServo.setPosition(hoodPos);
hoodServo.setPosition(hoodPos);
moveTurret(turretPos);

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

@@ -19,7 +19,7 @@ public class ShooterTest extends LinearOpMode {
public static double vel = 0.0;
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 pos = 0.0;
public static double posPower = 0.0;
public static double posi = 0.5;
@@ -38,10 +38,20 @@ public class ShooterTest extends LinearOpMode {
public double initPos = 0.0;
public static double time = 1.0;
public static double time = 0.1;
public double velo = 0.0;
public double velo1 = 0.0;
public double stamp1 = 0.0;
public double initPos1 = 0.0;
double powPID = 0.0;
public static int tolerance = 300;
MultipleTelemetry TELE;
public boolean wait(double time) {
@@ -81,9 +91,7 @@ public class ShooterTest extends LinearOpMode {
shooter.setManualPower(pow);
shooter.setVelocity(vel);
shooter.setTargetPosition(pos);
shooter.sethoodPosition(pos);
shooter.setTurretPosition(posi);
@@ -93,22 +101,33 @@ public class ShooterTest extends LinearOpMode {
if (servoPosition != 0.501) { shooter.sethoodPosition(servoPosition); }
if (wait(time)){
velo = 60*((shooter.getECPRPosition() - initPos) / time);
stamp = getRuntime();
initPos = shooter.getECPRPosition();
velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1));
stamp1 = getRuntime();
initPos1 = shooter.getECPRPosition();
if (Math.abs(vel - velo1) > 1500){
if (vel - velo1 > 0){
powPID = 0.75;
} else if (vel - velo1 < 0){
powPID = 0.25;
}
} else if (vel - tolerance > velo1) {
powPID = powPID + 0.001;
} else if (vel + tolerance < velo1) {
powPID = powPID - 0.001;
}
shooter.setVelocity(powPID);
shooter.update();
TELE.addData("ECPR Revolutions", shooter.getECPRPosition());
TELE.addData("MCPR Revolutions", shooter.getMCPRPosition());
TELE.addData("Velocity", shooter.getVelocity(velo));
TELE.addData("Velocity", velo);
TELE.addData("hoodPos", shooter.gethoodPosition());
TELE.addData("turretPos", shooter.getTurretPosition());
TELE.addData("Power Fly 1", robot.shooter1.getPower());
TELE.addData("Power Fly 2", robot.shooter2.getPower());
TELE.addData("powPID", shooter.getpowPID());
TELE.addData("Ins Velocity", velo1);
TELE.update();
}

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

@@ -11,7 +11,6 @@ import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
@Config
@Autonomous
public class ConfigureColorRangefinder extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
@@ -24,10 +23,9 @@ public class ConfigureColorRangefinder extends LinearOpMode {
neither --> no object
*/
crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 60 / 360.0 * 255, 180 / 360.0 * 255); // green
crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 20mm or closer requirement
crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 20); // 20mm or closer requirement
crf.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // purple
crf.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 2, 80); // purple
crf.setLedBrightness(1);
@@ -86,6 +84,7 @@ class ColorRangefinder {
public void setPin1Value(double lowerBound, double higherBound) {
setDigital(PinNum.PIN1, DigitalMode.VALUE, lowerBound, higherBound);
}
/**
* Sets the maximum distance (in millimeters) within which an object must be located for Pin 0's thresholds to trigger.
* This is most useful when we want to know if an object is both close and the correct color.
@@ -108,7 +107,6 @@ class ColorRangefinder {
* the color and look for blue.
*/
/**
* Invert the hue value before thresholding it, meaning that the colors become their opposite.
* This is useful if we want to threshold red; instead of having two thresholds we would invert
@@ -162,6 +160,7 @@ class ColorRangefinder {
/**
* Read distance via I2C
*
* @return distance in millimeters
*/
public double readDistance() {
@@ -180,7 +179,8 @@ class ColorRangefinder {
if (lowerBound == higherBound) {
lo = (int) lowerBound;
hi = (int) higherBound;
} else if (digitalMode.value <= DigitalMode.VALUE.value) { // HSV/HUE/SATURATION/VALUE color range
} else if (digitalMode.value <=
DigitalMode.VALUE.value) { // HSV/HUE/SATURATION/VALUE color range
lo = (int) Math.round(lowerBound / 255.0 * 65535);
hi = (int) Math.round(higherBound / 255.0 * 65535);
} else { // distance in mm