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

sorting auto is done

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-05 18:25:12 -05:00
parent f7a9f6aaf5
commit c36cac12f2
9 changed files with 238 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

119
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java
View File

@@ -18,11 +18,10 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
import org.firstinspires.ftc.teamcode.utilsv2.*;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
import org.firstinspires.ftc.teamcode.utilsv2.Robot;
import org.firstinspires.ftc.teamcode.utilsv2.Turret;
import java.util.List;
@@ -33,45 +32,52 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
MultipleTelemetry TELE;
Follower follower;
MeasuringLoopTimes loopTimes;
Shooter shooter;
Turret turret;
Flywheel flywheel;
VelocityCommander commander;
SpindexerTransferIntake spindexer;
// Wait Times
public static double shootTime = 2;
public static double openGateTime = 1.5;
public static double sortedShootTime = 2;
public static double rapidWaitTime = 0.25;
public static double rapidShootTime = 1;
public static double openGateTime = 2.5;
public static double pushTime = 2;
// Extra Variables
public static double intakePower = 0.3;
public static double intakePower = 0.5;
double shootX, shootY, shootH;
// Initialize path state machine
private enum PathState {
PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0,
PICKUP1, DRIVE_OPENGATE, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1,
PICKUP1, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1,
DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2,
DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3
}
PathState pathState = PathState.PUSHBOT;
// Poses
public static double startPoseX = 84, startPoseY = 7, startPoseH = 90;
public static double pushBotX = 94, pushBotY = 9, pushBotH = 100;
public static double shoot0ControlX = 88.29667812142038, shoot0ControlY = 52.03493699885454;
public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0;
public static double pickup1ControlX = 109.29381443298968, pickup1ControlY = 82.70618556701031;
public static double startPoseX = 91.5, startPoseY = 15, startPoseH = 90;
public static double pushBotX = 97, pushBotY = 18, pushBotH = 100;
public static double shoot0ControlX = 94.29667812142038, shoot0ControlY = 55.03493699885454;
public static double shoot0X = 95, shoot0Y = 83, shoot0H = 0;
public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0;
public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165;
public static double openGateX = 129, openGateY = 74, openGateH = 0;
public static double openGateX = 131, openGateY = 74, openGateH = 0;
public static double shoot1ControlX = 112, shoot1ControlY = 75;
public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12;
public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0;
public static double shoot1X = 95, shoot1Y = 83, shoot1H = 0;
public static double drivePickup2X = 98, drivePickup2Y = 58.5, drivePickup2H = 0;
public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0;
public static double shoot2ControlX = 102, shoot2ControlY = 63;
public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50;
public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0;
public static double shoot2X = 95, shoot2Y = 83, shoot2H = 0;
public static double drivePickup3X = 98, drivePickup3Y = 34.5, drivePickup3H = 0;
public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0;
public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446;
public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80;
public static double shoot3X = 84, shoot3Y = 120, shoot3H = -90;
Pose startPose, pushBot, shoot0Control, shoot0,
pickup1Control, pickup1, openGateControl, openGate, shoot1Control, shoot1,
pickup1, openGateControl, openGate, shoot1Control, shoot1,
drivePickup2, pickup2, shoot2Control, shoot2,
drivePickup3, pickup3, shoot3Control, shoot3;
private void initializePoses(){
@@ -79,7 +85,6 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH));
shoot0Control = new Pose(shoot0ControlX, shoot0ControlY);
shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H));
pickup1Control = new Pose(pickup1ControlX, pickup1ControlY);
pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H));
openGateControl = new Pose(openGateControlX, openGateControlY);
openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH));
@@ -112,7 +117,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
.build();
shoot0_pickup1 = follower.pathBuilder()
.addPath(new BezierCurve(shoot0, pickup1Control, pickup1))
.addPath(new BezierLine(shoot0, pickup1))
.setLinearHeadingInterpolation(shoot0.getHeading(), pickup1.getHeading())
.build();
@@ -164,42 +169,46 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
double currentTime = (double) System.currentTimeMillis() / 1000;
switch(pathState){
case PUSHBOT:
spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
shooter.setFlywheelVelocity(2400);
robot.setHoodPos(0.64);
if (startAuto){
follower.followPath(startPose_pushBot, true);
follower.followPath(startPose_pushBot, false);
startAuto = false;
timeStamp = currentTime;
shootX = shoot0X;
shootY = shoot0Y;
shootH = shoot0H;
}
if (!follower.isBusy()){
if (!follower.isBusy() || currentTime - timeStamp > pushTime){
follower.followPath(pushBot_shoot0, true);
pathState = PathState.DRIVE_SHOOT0;
timeStamp = currentTime;
}
break;
case DRIVE_SHOOT0:
if (!follower.isBusy()){
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){
timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT0;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
}
break;
case WAIT_SHOOT0:
if (currentTime - timeStamp > shootTime){
if (currentTime - timeStamp > rapidShootTime &&
(spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
follower.followPath(shoot0_pickup1, intakePower, false);
pathState = PathState.PICKUP1;
spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SORTED);
}
break;
case PICKUP1:
if (!follower.isBusy()){
follower.followPath(pickup1_openGate, true);
follower.followPath(pickup1_openGate, false);
pathState = PathState.OPENGATE;
shootX = shoot1X;
shootY = shoot1Y;
shootH = shoot1H;
}
break;
case DRIVE_OPENGATE:
if (!follower.isBusy()){
pathState = PathState.OPENGATE;
timeStamp = currentTime;
}
break;
@@ -213,10 +222,11 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
if (!follower.isBusy()){
pathState = PathState.WAIT_SHOOT1;
timeStamp = currentTime;
spindexer.startSortedShoot();
}
break;
case WAIT_SHOOT1:
if (currentTime - timeStamp > shootTime){
if (currentTime - timeStamp > sortedShootTime){
follower.followPath(shoot1_drivePickup2, true);
pathState = PathState.DRIVE_PICKUP2;
}
@@ -240,16 +250,19 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
if (!follower.isBusy()){
pathState = PathState.WAIT_SHOOT2;
timeStamp = currentTime;
spindexer.startSortedShoot();
}
break;
case WAIT_SHOOT2:
if (currentTime - timeStamp > shootTime){
if (currentTime - timeStamp > sortedShootTime){
follower.followPath(shoot2_drivePickup3, true);
pathState = PathState.DRIVE_PICKUP3;
}
break;
case DRIVE_PICKUP3:
if (!follower.isBusy()){
shooter.setFlywheelVelocity(2300);
robot.setHoodPos(0.68);
follower.followPath(drivePickup3_pickup3, intakePower, false);
pathState = PathState.PICKUP3;
}
@@ -266,6 +279,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
case DRIVE_SHOOT3:
if (!follower.isBusy()){
pathState = PathState.WAIT_SHOOT3;
spindexer.startSortedShoot();
}
break;
case WAIT_SHOOT3:
@@ -296,19 +310,33 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
}
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
follower = Constants.createFollower(hardwareMap);
sleep(1000);
follower.setStartingPose(new Pose(72,72,0));
loopTimes = new MeasuringLoopTimes();
loopTimes.init();
turret = new Turret(robot);
flywheel = new Flywheel(robot);
commander = new VelocityCommander();
shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
spindexer = new SpindexerTransferIntake(robot, TELE, commander);
ParkTilter park = new ParkTilter(robot);
boolean initializeRobot = false;
while (opModeInInit()){
follower.update();
if (gamepad1.squareWasPressed()){
robot.setSpinPos(ServoPositions.spindexer_A2);
robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Closed);
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
}
if (gamepad1.crossWasPressed() && !initializeRobot){
Color.redAlliance = !Color.redAlliance;
shooter.setRedAlliance(Color.redAlliance);
double[] xPoses = {startPoseX, pushBotX, shoot0ControlX, shoot0X,
pickup1ControlX, pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X,
pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X,
drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
drivePickup3X, pickup3X, shoot3ControlX, shoot3X};
@@ -326,14 +354,26 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
initializePoses();
follower.setPose(startPose);
buildPaths();
// turret.switchPipeline(Turret.PipelineMode.OBELISK);
robot.limelight.start();
sleep(2000);
turret.switchPipeline(Turret.PipelineMode.OBELISK);
robot.limelight.start();
limelightUsed = true;
park.unpark();
}
if (initializeRobot){
//add obelisk read here
shooter.setState(Shooter.ShooterState.READ_OBELISK);
int ID = turret.getObeliskID();
spindexer.setDesiredPatternAuto(ID);
TELE.addData("ID", ID);
shooter.update(robot.voltage.getVoltage());
}
TELE.addData("Red Alliance?", Color.redAlliance);
TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot);
TELE.addData("Start Pose", follower.getPose());
TELE.addData("Current LL Pipeline", turret.pipeline());
TELE.update();
}
@@ -341,9 +381,10 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
if (isStopRequested()) return;
limelightUsed = false;
while (opModeIsActive()){
shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
shooter.update(robot.voltage.getVoltage());
follower.update();
pathStateMachine();
Pose currentPose = follower.getPose();
@@ -351,6 +392,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
teleStartPoseY = currentPose.getY();
teleStartPoseH = Math.toDegrees(currentPose.getHeading());
spindexer.update();
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
}

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

@@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class ServoPositions {
public static double rapidFireBlocker_Closed = 0.35;
public static double rapidFireBlocker_Closed = 0.32;
public static double rapidFireBlocker_Open = 0.5;
public static double spindexBlocker_Closed = 0.31;
@@ -34,9 +34,9 @@ public class ServoPositions {
public static double shootAllSpindexerSpeedIncrease = 0.01;
public static double transferServo_out = 0.57;
public static double transferServo_out = 0.28;
public static double transferServo_in = 0.77;
public static double transferServo_in = 0.54;
public static double hoodAuto = 0.27;

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
View File

@@ -5,7 +5,7 @@ import com.pedropathing.control.FilteredPIDFCoefficients;
import com.pedropathing.control.PIDFCoefficients;
import com.pedropathing.follower.Follower;
import com.pedropathing.follower.FollowerConstants;
import com.pedropathing.ftc.FollowerBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.FollowerBuilder;
import com.pedropathing.ftc.drivetrains.MecanumConstants;
import com.pedropathing.ftc.localization.constants.PinpointConstants;
import com.pedropathing.paths.PathConstraints;
@@ -22,7 +22,7 @@ public class Constants {
.lateralZeroPowerAcceleration(-60.876)
.translationalPIDFCoefficients(new PIDFCoefficients(0.15, 0, 0.015, 0.02))
.headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02))
.drivePIDFCoefficients(new FilteredPIDFCoefficients(0.02, 0, 0.00001, 0.6, 0.015))
.drivePIDFCoefficients(new FilteredPIDFCoefficients(0.013, 0, 0.00001, 0.6, 0.015))
.centripetalScaling(0.0005);
public static MecanumConstants driveConstants = new MecanumConstants()
@@ -51,10 +51,16 @@ public class Constants {
.strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD);
public static Follower createFollower(HardwareMap hardwareMap) {
return new FollowerBuilder(followerConstants, hardwareMap)
FollowerBuilder followerBuilder;
followerBuilder = new FollowerBuilder(followerConstants, hardwareMap)
.pathConstraints(pathConstraints)
.mecanumDrivetrain(driveConstants)
.pinpointLocalizer(localizerConstants)
.build();
.pinpointLocalizer(localizerConstants);
followerBuilder.resetPinpoint();
followerBuilder.recalibrateIMU();
return followerBuilder.build();
}
}

106
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/FollowerBuilder.java Normal file
View File

@@ -0,0 +1,106 @@
package org.firstinspires.ftc.teamcode.pedroPathing;
import com.pedropathing.drivetrain.Drivetrain;
import com.pedropathing.follower.Follower;
import com.pedropathing.follower.FollowerConstants;
import com.pedropathing.ftc.drivetrains.*;
import com.pedropathing.ftc.localization.constants.DriveEncoderConstants;
import com.pedropathing.ftc.localization.constants.OTOSConstants;
import com.pedropathing.ftc.localization.constants.PinpointConstants;
import com.pedropathing.ftc.localization.constants.ThreeWheelConstants;
import com.pedropathing.ftc.localization.constants.ThreeWheelIMUConstants;
import com.pedropathing.ftc.localization.constants.TwoWheelConstants;
import com.pedropathing.ftc.localization.localizers.DriveEncoderLocalizer;
import com.pedropathing.ftc.localization.localizers.OTOSLocalizer;
import com.pedropathing.ftc.localization.localizers.PinpointLocalizer;
import com.pedropathing.ftc.localization.localizers.ThreeWheelIMULocalizer;
import com.pedropathing.ftc.localization.localizers.ThreeWheelLocalizer;
import com.pedropathing.ftc.localization.localizers.TwoWheelLocalizer;
import com.pedropathing.localization.Localizer;
import com.pedropathing.paths.PathConstraints;
import com.qualcomm.robotcore.hardware.HardwareMap;
/** This is the FollowerBuilder.
* It is used to create Followers with a specific drivetrain + localizer without having to use a full constructor
*
* @author Baron Henderson - 20077 The Indubitables
*/
public class FollowerBuilder {
private final FollowerConstants constants;
private PathConstraints constraints;
private final HardwareMap hardwareMap;
private Localizer localizer;
private Drivetrain drivetrain;
public FollowerBuilder(FollowerConstants constants, HardwareMap hardwareMap) {
this.constants = constants;
this.hardwareMap = hardwareMap;
constraints = PathConstraints.defaultConstraints;
}
public FollowerBuilder setLocalizer(Localizer localizer) {
this.localizer = localizer;
return this;
}
public FollowerBuilder driveEncoderLocalizer(DriveEncoderConstants lConstants) {
return setLocalizer(new DriveEncoderLocalizer(hardwareMap, lConstants));
}
public FollowerBuilder OTOSLocalizer(OTOSConstants lConstants) {
return setLocalizer(new OTOSLocalizer(hardwareMap, lConstants));
}
PinpointLocalizer pinpointLocalizer;
public FollowerBuilder pinpointLocalizer(PinpointConstants lConstants) {
pinpointLocalizer = new PinpointLocalizer(hardwareMap, lConstants);
return setLocalizer(pinpointLocalizer);
}
public void resetPinpoint(){
pinpointLocalizer.resetIMU();
}
public void recalibrateIMU(){
pinpointLocalizer.recalibrate();
}
public FollowerBuilder threeWheelIMULocalizer(ThreeWheelIMUConstants lConstants) {
return setLocalizer(new ThreeWheelIMULocalizer(hardwareMap, lConstants));
}
public FollowerBuilder threeWheelLocalizer(ThreeWheelConstants lConstants) {
return setLocalizer(new ThreeWheelLocalizer(hardwareMap, lConstants));
}
public FollowerBuilder twoWheelLocalizer(TwoWheelConstants lConstants) {
return setLocalizer(new TwoWheelLocalizer(hardwareMap, lConstants));
}
public FollowerBuilder setDrivetrain(Drivetrain drivetrain) {
this.drivetrain = drivetrain;
return this;
}
public FollowerBuilder mecanumDrivetrain(MecanumConstants mecanumConstants) {
return setDrivetrain(new Mecanum(hardwareMap, mecanumConstants));
}
@Deprecated
public FollowerBuilder mecanumExDrivetrain(MecanumConstants mecanumConstants) {
return setDrivetrain(new MecanumEx(hardwareMap, mecanumConstants));
}
public FollowerBuilder swerveDrivetrain(SwerveConstants swerveConstants, SwervePod... pods) {
return setDrivetrain(new Swerve(hardwareMap, swerveConstants, pods));
}
public FollowerBuilder pathConstraints(PathConstraints pathConstraints) {
this.constraints = pathConstraints;
PathConstraints.setDefaultConstraints(pathConstraints);
return this;
}
public Follower build() {
return new Follower(constants, localizer, drivetrain, constraints);
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
View File

@@ -161,6 +161,8 @@ public class TeleopV4 extends LinearOpMode {
TELE.addData("Current Position", currentPose);
TELE.addData("Current LL Pipeline", turret.pipeline());
TELE.update();
}

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

@@ -34,7 +34,7 @@ public class NewShooterTest extends LinearOpMode {
public static int flywheelVelo = 0;
public static double hoodPos = 0.5;
public static double transferPower = -0.7;
public static double transferPower = -0.8;
// public static double turretPos = 0.51;
@Override

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java
View File

@@ -140,11 +140,9 @@ public class Shooter {
break;
case READ_OBELISK:
manualFlywheel = false;
turr.trackObelisk(
(obeliskX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
(obeliskY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
follow.getHeading()
);
robot.setTurretPos(Turret.neutralPosition);
turr.detectObelisk();
commander.getVeloPredictive(
(goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
@@ -158,7 +156,7 @@ public class Shooter {
flywheelVelocity = commander.getPredictedRPM();
fly.manageFlywheel(flywheelVelocity);
fly.manageFlywheel(0);
fly.setF(voltage);
break;

33
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
View File

@@ -216,6 +216,16 @@ public class SpindexerTransferIntake {
desiredPattern = pattern;
}
public void setDesiredPatternAuto(int id) {
if (id == 22){
desiredPattern = DesiredPattern.PGP;
} else if (id == 23){
desiredPattern = DesiredPattern.PPG;
} else {
desiredPattern = DesiredPattern.GPP;
}
}
public void startSortedShoot() {
shootOrder = buildShootOrder(
@@ -404,6 +414,8 @@ public class SpindexerTransferIntake {
case SORTED:
robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
switch (sortedIntakeStates) {
case NOTHING:
break;
@@ -424,14 +436,14 @@ public class SpindexerTransferIntake {
ServoPositions.transferServo_out
);
robot.setIntakePower(1);
robot.setTransferPower(-1);
robot.setTransferPower(-0.7);
if (sortedStateTime() > 200) {
setSortedIntakeMode(SortedIntakeStates.INTAKE_1);
}
break;
case INTAKE_1:
robot.setIntakePower(1);
robot.setTransferPower(-1);
robot.setTransferPower(-0.7);
if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
@@ -458,8 +470,6 @@ public class SpindexerTransferIntake {
}
break;
case INTAKE_2:
robot.setIntakePower(1);
robot.setTransferPower(-1);
robot.setIntakePower(1);
robot.setTransferPower(-1);
if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
@@ -470,9 +480,9 @@ public class SpindexerTransferIntake {
ballTicks++;
if (ballTicks > 15) {
if (greenTicks > 10){
ballColors[0] = BallStates.GREEN;
ballColors[1] = BallStates.GREEN;
} else {
ballColors[0] = BallStates.PURPLE;
ballColors[1] = BallStates.PURPLE;
}
robot.setSpinPos(ServoPositions.spindexer_A3);
setSortedIntakeMode(SortedIntakeStates.DELAY_2);
@@ -505,9 +515,9 @@ public class SpindexerTransferIntake {
ballTicks++;
if (ballTicks > 15) {
if (greenTicks > 10){
ballColors[0] = BallStates.GREEN;
ballColors[2] = BallStates.GREEN;
} else {
ballColors[0] = BallStates.PURPLE;
ballColors[2] = BallStates.PURPLE;
}
setSortedIntakeMode(SortedIntakeStates.REVERSE);
ballTicks = 0;
@@ -525,7 +535,12 @@ public class SpindexerTransferIntake {
break;
case SHOOT_SORTED:
robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
if (Shooter.manualFlywheel) {
robot.setTransferPower(NewShooterTest.transferPower);
} else {
robot.setTransferPower(commander.getTransferPow());
}
switch (shootState) {
@@ -697,7 +712,7 @@ public class SpindexerTransferIntake {
ServoPositions.transferServo_out
);
robot.setIntakePower(1);
robot.setTransferPower(-1);
robot.setTransferPower(-0.7);
if (shootStateTime() > 250) {

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java
View File

@@ -16,7 +16,7 @@ public class Turret {
Robot robot;
private final double servoTicksPer180 = 0.58;
private final double neutralPosition = 0.51;
public static double neutralPosition = 0.51;
private final double turretMin = 0.05;
private final double turretMax = 0.95;
public static boolean limelightUsed = true;
@@ -83,11 +83,12 @@ public class Turret {
pipeline = 2;
}
}
if (pipeline == 0){prevPipeline = 0;}
if (pipeline != prevPipeline){
robot.limelight.pipelineSwitch(pipeline);
}
prevPipeline = pipeline;
}
public int pipeline(){return prevPipeline;}
public void trackObelisk(double dx, double dy, double h) {
@@ -107,7 +108,6 @@ public class Turret {
robot.setTurretPos(servoAngle);
detectObelisk();
}
@@ -116,21 +116,15 @@ public class Turret {
return obeliskID;
}
private int detectObelisk() {
switchPipeline(PipelineMode.OBELISK);
public void detectObelisk() {
result = robot.limelight.getLatestResult();
if (result != null && result.isValid()) {
List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
double prevTx = -1000;
for (LLResultTypes.FiducialResult fiducial : fiducials) {
double currentTx = fiducial.getTargetXDegrees();
if (currentTx > prevTx){
obeliskID = fiducial.getFiducialId();
}
}
}
return obeliskID;
}
public void manual (double pos) {
robot.setTurretPos(pos);