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

intake test was tested with good results in both modes

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-10 21:06:57 -06:00
parent 82c3c83262
commit d81a189ef9
4 changed files with 48 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java
View File

@@ -35,7 +35,7 @@ public class IntakeTest extends LinearOpMode {
List<Boolean> s2 = new ArrayList<>();
List<Boolean> s3 = new ArrayList<>();
public static int mode = 0; // 0 for teleop, 1 for auto
public static double manualPow = 1.0;
public static double manualPow = 0.15;
double stamp = 0;
int ticker = 0;
boolean steadySpin = false;
@@ -43,6 +43,9 @@ public class IntakeTest extends LinearOpMode {
boolean intake = true;
double spindexerPos = spindexer_intakePos1;
boolean wasMoving = false;
double currentPos = 0.0;
double initPos = 0.0;
boolean reverse = false;
@Override
public void runOpMode() throws InterruptedException {
List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
@@ -58,32 +61,44 @@ public class IntakeTest extends LinearOpMode {
if (isStopRequested()) return;
while (opModeIsActive()) {
//TODO: test tele intake with new spindexer
if (mode == 0) {
if (gamepad1.cross) {
ticker = 0;
robot.spin1.setPower(manualPow);
robot.spin2.setPower(-manualPow);
if (gamepad1.right_bumper) {
ticker++;
if (ticker % 16 == 0){
currentPos = servo.getSpinPos();
if (Math.abs(currentPos - initPos) == 0.0){
reverse = !reverse;
}
initPos = currentPos;
}
if (reverse){
robot.spin1.setPower(manualPow);
robot.spin2.setPower(-manualPow);
} else {
robot.spin1.setPower(-manualPow);
robot.spin2.setPower(manualPow);
}
robot.intake.setPower(1);
stamp = getRuntime();
TELE.addData("Reverse?", reverse);
TELE.update();
} else {
robot.spin1.setPower(0);
robot.spin2.setPower(0);
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if (getRuntime() - stamp < 0.5) {
robot.intake.setPower(-1);
} else {
robot.intake.setPower(0);
}
ticker = 0;
}
//TODO: test this monstrosity
} else if (mode == 1) {
if (gamepad1.cross && intake){
if (gamepad1.right_bumper && intake){
robot.intake.setPower(1);
} else if (gamepad1.circle){
} else if (gamepad1.left_bumper){
robot.intake.setPower(-1);
} else {
robot.intake.setPower(0);
@@ -123,6 +138,21 @@ public class IntakeTest extends LinearOpMode {
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
}
double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed
double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
double rx = gamepad1.left_stick_x;
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
robot.frontLeft.setPower(frontLeftPower);
robot.backLeft.setPower(backLeftPower);
robot.frontRight.setPower(frontRightPower);
robot.backRight.setPower(backRightPower);
TELE.addData("Manual Power", manualPow);
TELE.addData("PID Power", powPID);
TELE.addData("B1", ballIn(1));
@@ -143,15 +173,15 @@ public class IntakeTest extends LinearOpMode {
TELE.addData("Color 3 Distance", s3D);
TELE.update();
if (s1D < 40) {
if (s1D < 43) {
s1T.add(getRuntime());
}
if (s2D < 40) {
if (s2D < 60) {
s2T.add(getRuntime());
}
if (s3D < 30) {
if (s3D < 33) {
s3T.add(getRuntime());
}
}

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java
View File

@@ -45,6 +45,7 @@ public class LimelightTest extends LinearOpMode {
for (LLResultTypes.FiducialResult fiducial : fiducials) {
int id = fiducial.getFiducialId();
TELE.addData("ID", id);
TELE.update();
}
}

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

@@ -20,68 +20,39 @@ public class Robot {
public DcMotorEx frontLeft;
public DcMotorEx frontRight;
public DcMotorEx backLeft;
public DcMotorEx backRight;
public DcMotorEx intake;
public DcMotorEx transfer;
public DcMotorEx shooter1;
public DcMotorEx shooter2;
public Servo hood;
public Servo transferServo;
public Servo rejecter;
public CRServo turr1;
public CRServo turr2;
public CRServo spin1;
public CRServo spin2;
public DigitalChannel pin0;
public DigitalChannel pin1;
public DigitalChannel pin2;
public DigitalChannel pin3;
public DigitalChannel pin4;
public DigitalChannel pin5;
public AnalogInput analogInput;
public AnalogInput analogInput2;
public AnalogInput spin1Pos;
public AnalogInput spin2Pos;
public AnalogInput hoodPos;
public AnalogInput turr1Pos;
public AnalogInput turr2Pos;
public AnalogInput transferServoPos;
public AprilTagProcessor aprilTagProcessor;
public WebcamName webcam;
public DcMotorEx shooterEncoder;
public RevColorSensorV3 color1;
public RevColorSensorV3 color2;
public RevColorSensorV3 color3;
public Limelight3A limelight;
public Robot(HardwareMap hardwareMap) {

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
View File

@@ -41,7 +41,7 @@ public class Servos {
}
public boolean spinEqual(double pos) {
return Math.abs(pos - this.getSpinPos()) < 0.01;
return Math.abs(pos - this.getSpinPos()) < 0.02;
}
public double getTurrPos() {