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

stash

Browse Source
This commit is contained in:
KeshavAnandCode
2025-12-02 20:27:33 -06:00
parent cdec64eb8f
commit 335e62ee3c
2 changed files with 49 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java
View File

@@ -13,4 +13,7 @@ public class ShooterVars {
public static int initTolerance = 1000; public static int initTolerance = 1000;
public static int maxVel = 4000; public static int maxVel = 4000;
public static double waitTransfer = 0.4;
public static double waitTransferOut = 0.6;
} }

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
View File

@@ -3,9 +3,13 @@ package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Poses.goalPose; import static org.firstinspires.ftc.teamcode.constants.Poses.goalPose;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransfer;
import static org.firstinspires.ftc.teamcode.tests.ShooterTest.kP; import static org.firstinspires.ftc.teamcode.tests.ShooterTest.kP;
import static org.firstinspires.ftc.teamcode.tests.ShooterTest.maxStep; import static org.firstinspires.ftc.teamcode.tests.ShooterTest.maxStep;
import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.restPos;
import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.scalar;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
@@ -371,6 +375,48 @@ public class TeleopV2 extends LinearOpMode {
return countTrue > countWindow / 2.0; // more than 50% true return countTrue > countWindow / 2.0; // more than 50% true
} }
boolean spindexPosEqual(double spindexer) {
return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 &&
scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01);
}
// Fields to keep state across calls
private double transferStamp = 0.0;
private int ticker = 1;
public boolean shootTeleop(double spindexer) {
// Spin motors
robot.spin1.setPosition(spindexer);
robot.spin2.setPosition(1 - spindexer);
// Default transfer position
robot.transferServo.setPosition(transferServo_out);
if (spindexPosEqual(spindexer)) {
if (ticker == 1) {
transferStamp = getRuntime();
ticker++;
}
if (getRuntime() - transferStamp > waitTransfer) {
robot.transferServo.setPosition(transferServo_in);
} else {
robot.transferServo.setPosition(transferServo_out);
}
} else {
robot.transferServo.setPosition(transferServo_out);
ticker = 1;
transferStamp = getRuntime();
}
TELE.addLine("shooting");
TELE.update();
// Return true while transfer servo hasn't reached "in" position
return !transferPosEqual(transferServo_in);
}
} }