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

spindex is built, needs improvement

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-06 22:50:37 -06:00
parent 7f3ca719fa
commit 16d9a13376
5 changed files with 27 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,16 +5,16 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class ServoPositions {
public static double spindexer_intakePos1 = 0.665;
public static double spindexer_intakePos1 = 0.34;
public static double spindexer_intakePos3 = 0.29;
public static double spindexer_intakePos3 = 0.5;
public static double spindexer_intakePos2 = 0.99;
public static double spindexer_intakePos2 = 0.66;
public static double spindexer_outtakeBall3 = 0.845;
public static double spindexer_outtakeBall3 = 0.42;
public static double spindexer_outtakeBall2 = 0.48;
public static double spindexer_outtakeBall1 = 0.1;
public static double spindexer_outtakeBall2 = 0.74;
public static double spindexer_outtakeBall1 = 0.58;
public static double transferServo_out = 0.15;

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

@@ -74,7 +74,7 @@ public class IntakeTest extends LinearOpMode {
colorDetect();
spindexer();
if (b1 && steadySpin && getRuntime() - stamp > 0.5){
if (b1 && steadySpin && getRuntime() - stamp > 1.0){
if (!b2){
if (servo.spinEqual(spindexer_intakePos1)){
spindexerPos = spindexer_intakePos2;
@@ -94,8 +94,6 @@ public class IntakeTest extends LinearOpMode {
}
}
powPID = servo.setSpinPos(spindexerPos);
} else if (mode == 2){ // switch to this mode before switching modes
powPID = 0;
spindexerPos = spindexer_intakePos1;
@@ -150,18 +148,15 @@ public class IntakeTest extends LinearOpMode {
public void spindexer(){
if (!servo.spinEqual(spindexerPos)){
powPID = servo.setSpinPos(spindexerPos);
robot.spin1.setPower(powPID);
robot.spin2.setPower(-powPID);
steadySpin = false;
ticker = 0;
stamp = getRuntime();
} else{
robot.spin1.setPower(0);
robot.spin2.setPower(0);
steadySpin = true;
if (ticker == 0){
stamp = getRuntime();
}
ticker++;
}
}
}

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

@@ -3,28 +3,28 @@ package org.firstinspires.ftc.teamcode.tests;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.utils.Robot;
//TODO: fix to get the apriltag that it is reading
public class LimelightTest extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
public static int pipeline = 0; //0 is for test; 1, 2, and 3 are for obelisk; 4 is for blue track; 5 is for red track
public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track
int obeliskPipe = 1;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
Limelight3A limelight = hardwareMap.get(Limelight3A.class, "Limelight");
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
robot.limelight.pipelineSwitch(pipeline);
limelight.pipelineSwitch(pipeline);
waitForStart();
if (isStopRequested()) return;
robot.limelight.start();
limelight.start();
while (opModeIsActive()){
if (mode == 0){
robot.limelight.pipelineSwitch(pipeline);
LLResult result = robot.limelight.getLatestResult();
limelight.pipelineSwitch(pipeline);
LLResult result = limelight.getLatestResult();
if (result != null) {
if (result.isValid()) {
TELE.addData("tx", result.getTx());
@@ -33,8 +33,8 @@ public class LimelightTest extends LinearOpMode {
}
}
} else if (mode == 1){
robot.limelight.pipelineSwitch(obeliskPipe);
LLResult result = robot.limelight.getLatestResult();
limelight.pipelineSwitch(obeliskPipe);
LLResult result = limelight.getLatestResult();
if (result != null && result.isValid()){
TELE.addData("ID", obeliskPipe+20);
TELE.update();
@@ -46,8 +46,8 @@ public class LimelightTest extends LinearOpMode {
}
}
} else if (mode == 2){
robot.limelight.pipelineSwitch(4);
LLResult result = robot.limelight.getLatestResult();
limelight.pipelineSwitch(4);
LLResult result = limelight.getLatestResult();
if (result != null) {
if (result.isValid()) {
TELE.addData("tx", result.getTx());
@@ -56,8 +56,8 @@ public class LimelightTest extends LinearOpMode {
}
}
} else if (mode == 3){
robot.limelight.pipelineSwitch(5);
LLResult result = robot.limelight.getLatestResult();
limelight.pipelineSwitch(5);
LLResult result = limelight.getLatestResult();
if (result != null) {
if (result.isValid()) {
TELE.addData("tx", result.getTx());
@@ -66,7 +66,7 @@ public class LimelightTest extends LinearOpMode {
}
}
} else {
robot.limelight.pipelineSwitch(0);
limelight.pipelineSwitch(0);
}
}
}

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

@@ -133,6 +133,9 @@ public class Robot {
spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
spin1.setDirection(DcMotorSimple.Direction.REVERSE);
spin2.setDirection(DcMotorSimple.Direction.REVERSE);
pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
@@ -166,7 +169,5 @@ public class Robot {
color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
limelight = hardwareMap.get(Limelight3A.class,"Limelight");
}
}

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

@@ -16,10 +16,10 @@ public class Servos {
//PID constants
// TODO: get PIDF constants
public static double spinP = 2.85, spinI = 0.015, spinD = 0.09, spinF = 0.03;
public static double spinP = 1.8, spinI = 0, spinD = 0.03, spinF = 0.03;
public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF = 0.0;
public static double spin_scalar = 1.011;
public static double spin_scalar = 1.0086;
public static double spin_restPos = 0.0;
public static double turret_scalar = 1.009;
public static double turret_restPos = 0.0;