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 @Config
public class ServoPositions { 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_outtakeBall2 = 0.74;
public static double spindexer_outtakeBall1 = 0.1; public static double spindexer_outtakeBall1 = 0.58;
public static double transferServo_out = 0.15; 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(); colorDetect();
spindexer(); spindexer();
if (b1 && steadySpin && getRuntime() - stamp > 0.5){ if (b1 && steadySpin && getRuntime() - stamp > 1.0){
if (!b2){ if (!b2){
if (servo.spinEqual(spindexer_intakePos1)){ if (servo.spinEqual(spindexer_intakePos1)){
spindexerPos = spindexer_intakePos2; 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 } else if (mode == 2){ // switch to this mode before switching modes
powPID = 0; powPID = 0;
spindexerPos = spindexer_intakePos1; spindexerPos = spindexer_intakePos1;
@@ -150,18 +148,15 @@ public class IntakeTest extends LinearOpMode {
public void spindexer(){ public void spindexer(){
if (!servo.spinEqual(spindexerPos)){ if (!servo.spinEqual(spindexerPos)){
powPID = servo.setSpinPos(spindexerPos);
robot.spin1.setPower(powPID); robot.spin1.setPower(powPID);
robot.spin2.setPower(-powPID); robot.spin2.setPower(-powPID);
steadySpin = false; steadySpin = false;
ticker = 0; stamp = getRuntime();
} else{ } else{
robot.spin1.setPower(0); robot.spin1.setPower(0);
robot.spin2.setPower(0); robot.spin2.setPower(0);
steadySpin = true; 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.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;
//TODO: fix to get the apriltag that it is reading //TODO: fix to get the apriltag that it is reading
public class LimelightTest extends LinearOpMode { public class LimelightTest extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE; 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 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 public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track
int obeliskPipe = 1; int obeliskPipe = 1;
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap); Limelight3A limelight = hardwareMap.get(Limelight3A.class, "Limelight");
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
robot.limelight.pipelineSwitch(pipeline); limelight.pipelineSwitch(pipeline);
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
robot.limelight.start(); limelight.start();
while (opModeIsActive()){ while (opModeIsActive()){
if (mode == 0){ if (mode == 0){
robot.limelight.pipelineSwitch(pipeline); limelight.pipelineSwitch(pipeline);
LLResult result = robot.limelight.getLatestResult(); LLResult result = limelight.getLatestResult();
if (result != null) { if (result != null) {
if (result.isValid()) { if (result.isValid()) {
TELE.addData("tx", result.getTx()); TELE.addData("tx", result.getTx());
@@ -33,8 +33,8 @@ public class LimelightTest extends LinearOpMode {
} }
} }
} else if (mode == 1){ } else if (mode == 1){
robot.limelight.pipelineSwitch(obeliskPipe); limelight.pipelineSwitch(obeliskPipe);
LLResult result = robot.limelight.getLatestResult(); LLResult result = limelight.getLatestResult();
if (result != null && result.isValid()){ if (result != null && result.isValid()){
TELE.addData("ID", obeliskPipe+20); TELE.addData("ID", obeliskPipe+20);
TELE.update(); TELE.update();
@@ -46,8 +46,8 @@ public class LimelightTest extends LinearOpMode {
} }
} }
} else if (mode == 2){ } else if (mode == 2){
robot.limelight.pipelineSwitch(4); limelight.pipelineSwitch(4);
LLResult result = robot.limelight.getLatestResult(); LLResult result = limelight.getLatestResult();
if (result != null) { if (result != null) {
if (result.isValid()) { if (result.isValid()) {
TELE.addData("tx", result.getTx()); TELE.addData("tx", result.getTx());
@@ -56,8 +56,8 @@ public class LimelightTest extends LinearOpMode {
} }
} }
} else if (mode == 3){ } else if (mode == 3){
robot.limelight.pipelineSwitch(5); limelight.pipelineSwitch(5);
LLResult result = robot.limelight.getLatestResult(); LLResult result = limelight.getLatestResult();
if (result != null) { if (result != null) {
if (result.isValid()) { if (result.isValid()) {
TELE.addData("tx", result.getTx()); TELE.addData("tx", result.getTx());
@@ -66,7 +66,7 @@ public class LimelightTest extends LinearOpMode {
} }
} }
} else { } 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"); spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
spin1.setDirection(DcMotorSimple.Direction.REVERSE);
spin2.setDirection(DcMotorSimple.Direction.REVERSE);
pin0 = hardwareMap.get(DigitalChannel.class, "pin0"); pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
pin1 = hardwareMap.get(DigitalChannel.class, "pin1"); pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
@@ -166,7 +169,5 @@ public class Robot {
color2 = hardwareMap.get(RevColorSensorV3.class, "c2"); color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
color3 = hardwareMap.get(RevColorSensorV3.class, "c3"); 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 //PID constants
// TODO: get PIDF 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 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 spin_restPos = 0.0;
public static double turret_scalar = 1.009; public static double turret_scalar = 1.009;
public static double turret_restPos = 0.0; public static double turret_restPos = 0.0;