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

pid is good, fix color

Browse Source
This commit is contained in:
DanTheMan-byte
2025-11-29 18:14:03 -06:00
parent 00966f98ba
commit a869d4b7a0
7 changed files with 197 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java
View File

@@ -13,7 +13,6 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.ParallelAction;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@@ -37,6 +36,12 @@ public class redDaniel extends LinearOpMode {
AprilTag aprilTag;
int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
// TODO: change this velocity PID
public Action initShooter(int velocity){
return new Action(){
double velo = 0.0;
@@ -67,6 +72,10 @@ public class redDaniel extends LinearOpMode {
};
}
public void Obelisk (){
// TODO: write the code to detect order
}
public Action Shoot(double spindexer){
return new Action() {
boolean transfer = false;
@@ -87,6 +96,93 @@ public class redDaniel extends LinearOpMode {
};
}
public Action intake (){
return new Action() {
double position = 0.0;
final double intakeTime = 4.0; // TODO: change this so it serves as a backup
final double stamp = getRuntime();
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if ((getRuntime() % 0.3) >0.15) {
position = spindexer_intakePos1 + 0.02;
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1-position);
robot.intake.setPower(1);
return !(robot.pin1.getState() && robot.pin3.getState() && robot.pin5.getState()) || getRuntime() - stamp > intakeTime;
}
};
}
public Action ColorDetect (){
return new Action() {
int t1 = 0;
int t2 = 0;
int t3 = 0;
int tP1 = 0;
int tP2 = 0;
int tP3 = 0;
final double stamp = getRuntime();
final double detectTime = 3.0;
double position = 0.0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if ((getRuntime() % 0.3) >0.15) {
position = spindexer_intakePos1 + 0.02;
} else {
position = spindexer_intakePos1 - 0.02;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1-position);
if (robot.pin1.getState()) {
t1 += 1;
if (robot.pin0.getState()){
tP1 += 1;
}
}
if (robot.pin3.getState()) {
t2 += 1;
if (robot.pin0.getState()){
tP2 += 1;
}
}
if (robot.pin5.getState()) {
t3 += 1;
if (robot.pin0.getState()){
tP3 += 1;
}
}
if (t1 > 20){
if (tP1 > 20){
b1 = 2;
} else {
b1 = 1;
}
}
if (t2 > 20){
if (tP2 > 20){
b2 = 2;
} else {
b2 = 1;
}
}
if (t3 > 20){
if (tP3 > 20){
b3 = 2;
} else {
b3 = 1;
}
}
return !(b1 + b2 + b3 >= 5) || (getRuntime() - stamp < detectTime);
}
};
}
@Override
public void runOpMode() throws InterruptedException {

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
View File

@@ -6,7 +6,11 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class ServoPositions {
public static double spindexer_intakePos = 0.665;
public static double spindexer_intakePos1 = 0.665;
public static double spindexer_intakePos3 = 0.29;
public static double spindexer_intakePos2 = 0.99;
public static double spindexer_outtakeBall3 = 0.845;

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java
View File

@@ -9,8 +9,6 @@ import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.utils.Robot;
import java.util.ArrayList;
public class Spindexer implements Subsystem{
private Servo s1;
@@ -118,14 +116,14 @@ public class Spindexer implements Subsystem{
}
public void intake () {
position = spindexer_intakePos;
position = spindexer_intakePos1;
}
public void intakeShake(double runtime) {
if ((runtime % 0.25) >0.125) {
position = spindexer_intakePos + 0.04;
position = spindexer_intakePos1 + 0.04;
} else {
position = spindexer_intakePos - 0.04;
position = spindexer_intakePos1 - 0.04;
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java
View File

@@ -49,9 +49,9 @@ public class ActiveColorSensorTest extends LinearOpMode {
while (opModeIsActive()){
if ((getRuntime() % 0.3) >0.15) {
position = spindexer_intakePos + 0.02;
position = spindexer_intakePos1 + 0.015;
} else {
position = spindexer_intakePos - 0.02;
position = spindexer_intakePos1 - 0.015;
}
robot.spin1.setPosition(position);
robot.spin2.setPosition(1-position);

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java
View File

@@ -29,6 +29,8 @@ public class ColorSensorTest extends LinearOpMode{
TELE.addData("Purple2:", robot.pin2.getState());
TELE.addData("Green3:", robot.pin5.getState());
TELE.addData("Purple3:", robot.pin4.getState());
TELE.addData("Hello Keshav (analog)", robot.analogInput.getVoltage());
TELE.addData("Hello again (analog2)", robot.analogInput2.getVoltage());
TELE.update();
}

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

@@ -30,7 +30,12 @@ public class ShooterTest extends LinearOpMode {
double initPos = 0.0;
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 stamp1 = 0.0;
@@ -38,7 +43,7 @@ public class ShooterTest extends LinearOpMode {
double powPID = 0.0;
public static int maxVel = 4000;
public static int maxVel = 4500;
public static boolean shoot = false;
@@ -48,7 +53,9 @@ public class ShooterTest extends LinearOpMode {
public static int tolerance = 50;
public static double kP = 0.0005; // small proportional gain (tune this)
double stamp = 0.0;
public static double kP = 0.001; // small proportional gain (tune this)
public static double maxStep = 0.06; // prevents sudden jumps
public static double distance = 50;
@@ -72,12 +79,16 @@ public class ShooterTest extends LinearOpMode {
initPos = shooter.getECPRPosition();
int ticker = 0;
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
ticker++;
if (AutoTrack){
hoodPos = hoodAnglePrediction(distance);
vel = velPrediction(distance);
@@ -85,69 +96,88 @@ public class ShooterTest extends LinearOpMode {
shooter.setShooterMode(flyMode);
shooter.setShooterMode(flyMode);
shooter.setManualPower(pow);
shooter.setManualPower(pow);
robot.hood.setPosition(hoodPos);
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1 - turretPos);
if (intake) {
robot.transfer.setPower(0);
robot.intake.setPower(0.75);
robot.spin1.setPosition(spindexer_intakePos);
robot.spin2.setPosition(1 - spindexer_intakePos);
} else {
robot.transfer.setPower(1);
robot.intake.setPower(0.75 + (powPID/4));
if (spindexPos == 1) {
robot.spin1.setPosition(spindexer_outtakeBall1);
robot.spin2.setPosition(1 - spindexer_outtakeBall1);
} else if (spindexPos == 2) {
robot.spin1.setPosition(spindexer_outtakeBall2);
robot.spin2.setPosition(1 - spindexer_outtakeBall2);
} else if (spindexPos == 3) {
robot.spin1.setPosition(spindexer_outtakeBall3);
robot.spin2.setPosition(1 - spindexer_outtakeBall3);
}
robot.hood.setPosition(hoodPos);
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1 - turretPos);
if (intake) {
robot.transfer.setPower(0);
robot.intake.setPower(0.75);
robot.spin1.setPosition(spindexer_intakePos1);
robot.spin2.setPosition(1 - spindexer_intakePos1);
} else {
robot.transfer.setPower(.75 + (powPID/4));
robot.intake.setPower(0);
if (spindexPos == 1) {
robot.spin1.setPosition(spindexer_outtakeBall1);
robot.spin2.setPosition(1 - spindexer_outtakeBall1);
} else if (spindexPos == 2) {
robot.spin1.setPosition(spindexer_outtakeBall2);
robot.spin2.setPosition(1 - spindexer_outtakeBall2);
} else if (spindexPos == 3) {
robot.spin1.setPosition(spindexer_outtakeBall3);
robot.spin2.setPosition(1 - spindexer_outtakeBall3);
}
}
velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1));
stamp1 = getRuntime();
initPos1 = shooter.getECPRPosition();
double penguin = 0;
if (ticker % 8 ==0){
penguin = shooter.getECPRPosition();
stamp = getRuntime();
velo1 = -60 * ((penguin - initPos1) / (stamp - stamp1));
initPos1 = penguin;
stamp1 = stamp;
}
double feed = vel / maxVel; // Example: vel=2500 → feed=0.5
// --- PROPORTIONAL CORRECTION ---
double error = vel - velo1;
double correction = kP * error;
velo = velo1;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
double feed = vel / maxVel; // Example: vel=2500 → feed=0.5
// --- FINAL MOTOR POWER ---
powPID = feed + correction;
if (vel > 500){
feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
}
// clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID));
shooter.setVelocity(powPID);
// --- PROPORTIONAL CORRECTION ---
double error = vel - velo1;
double correction = kP * error;
if (shoot) {
robot.transferServo.setPosition(transferServo_in);
} else {
robot.transferServo.setPosition(transferServo_out);
}
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
shooter.update();
// --- FINAL MOTOR POWER ---
powPID = feed + correction;
TELE.addData("Revolutions", shooter.getECPRPosition());
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("Velocity", velo1);
TELE.update();
// clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID));
if (vel - velo > 1000){
powPID = 1;
} else if (velo - vel > 1000){
powPID = 0;
}
shooter.setVelocity(powPID);
if (shoot) {
robot.transferServo.setPosition(transferServo_in);
} else {
robot.transferServo.setPosition(transferServo_out);
}
shooter.update();
TELE.addData("Revolutions", shooter.getECPRPosition());
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("Velocity", velo);
TELE.update();
}

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

@@ -22,10 +22,10 @@ public class ConfigureColorRangefinder extends LinearOpMode {
/* Using this example configuration, you can detect both artifact colors based on which pin is reading true:
pin0 --> purple
pin1 --> green */
crf.setPin1Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // green
crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple
crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255);
crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement
crf.setPin0Analog(ColorRangefinder.AnalogMode.GREEN); // green
// crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple
// crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255);
// crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement
crf.setLedBrightness(LED_Brightness);
}