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

a lot of changes happened

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-08 15:29:22 -05:00
parent 855dac7122
commit b2a1ea32d9
10 changed files with 599 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -18,6 +18,7 @@ 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.teleop.TeleopV4;
import org.firstinspires.ftc.teamcode.tests.NewShooterTest;
import org.firstinspires.ftc.teamcode.utilsv2.*;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
@@ -369,6 +370,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
park.unpark();
}
NewShooterTest.transferPower = -0.8;
if (initializeRobot){
//add obelisk read here
shooter.setState(Shooter.ShooterState.READ_OBELISK);

69
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto15Ball_Back.java
View File

@@ -18,6 +18,7 @@ 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.teleop.TeleopV4;
import org.firstinspires.ftc.teamcode.tests.NewShooterTest;
import org.firstinspires.ftc.teamcode.utilsv2.*;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
@@ -37,8 +38,8 @@ public class Auto15Ball_Back extends LinearOpMode {
SpindexerTransferIntake spindexer;
// Wait Times
public static double rapidWaitTime = 0.5;
public static double rapidShootTime = 0.8;
public static double rapidWaitTime = 0.7;
public static double rapidShootTime = 2.6;
public static double loadPickupTime = 3;
// Initialize path state machine
@@ -51,20 +52,20 @@ public class Auto15Ball_Back extends LinearOpMode {
PathState pathState = PathState.WAIT_VELOCITY;
// Poses
public static double startPoseX = 12, startPoseY = -64, startPoseH = 90;
public static double startPoseX = 16, startPoseY = -64, startPoseH = 90;
public static double pickup3ControlX = 12, pickup3ControlY = -37;
public static double pickup3X = 61, pickup3Y = -37, pickup3H = 0;
public static double shoot3X = 16, shoot3Y = -57, shoot3H = 0;
public static double shoot3X = 16, shoot3Y = -55, shoot3H = 0;
public static double pickupLoadControlX = 21.23654066437572, pickupLoadControlY = -62.311626575028637;
public static double pickupLoadX = 63, pickupLoadY = -63, pickupLoadH = 0;
public static double shootLoadControlX = 21.23654066437572, shootLoadControlY = -62.311626575028637;
public static double shootLoadX = 16, shootLoadY = -57, shootLoadH = 0;
public static double shootLoadX = 16, shootLoadY = -55, shootLoadH = 0;
public static double intakeGateControl1X = 51.9656357388316, intakeGateControl1Y = -65.506277205040073;
public static double intakeGateControl2X = 60.13459335624285, intakeGateControl2Y = -67.300458190148911;
public static double intakeGateX = 59, intakeGateY = -12, intakeGateH = 90;
public static double intakeGateX = 63, intakeGateY = -35, intakeGateH = 90;
public static double shootGateControlX = 53.8705612829324, shootGateControlY = -35.14501718213059;
public static double shootGateX = 16, shootGateY = -57, shootGateH = 0;
public static double leaveX = 40, leaveY = 14, leaveH = 0;
public static double shootGateX = 16, shootGateY = -55, shootGateH = 0;
public static double leaveX = 36, leaveY = -58, leaveH = 0;
double[] xPoses = {startPoseX, pickup3ControlX, pickup3X, shoot3X,
pickupLoadControlX, pickupLoadX, shootLoadControlX, shootLoadX,
intakeGateControl1X, intakeGateControl2X, intakeGateX, shootGateControlX, shootGateX, leaveX};
@@ -146,29 +147,27 @@ public class Auto15Ball_Back extends LinearOpMode {
double currentTime = (double) System.currentTimeMillis() / 1000;
switch(pathState){
case WAIT_VELOCITY:
spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
shooter.setFlywheelVelocity(2400);
robot.setHoodPos(0.64);
shooter.setFlywheelVelocity(3250);
robot.setHoodPos(0.65);
if (flywheel.getSteady()){
startAuto++;
}
if (startAuto > 5){
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
if (startAuto > 6){
spindexer.startBackShooting();
timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT0;
}
timeStamp = currentTime;
break;
case WAIT_SHOOT0:
if (currentTime - timeStamp > rapidShootTime ||
(spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
if (currentTime - timeStamp > rapidShootTime){
follower.followPath(startPose_pickup3, false);
pathState = PathState.PICKUP3;
}
break;
case PICKUP3:
if (!follower.isBusy()){
follower.followPath(pickup3_shoot3, false);
follower.followPath(pickup3_shoot3, true);
pathState = PathState.DRIVE_SHOOT3;
timeStamp = currentTime;
}
@@ -177,15 +176,13 @@ public class Auto15Ball_Back extends LinearOpMode {
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){
timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT3;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
spindexer.startBackShooting();
} else if (follower.isBusy()){
timeStamp = currentTime;
}
break;
case WAIT_SHOOT3:
if (currentTime - timeStamp > rapidShootTime ||
(spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
if (currentTime - timeStamp > rapidShootTime){
follower.followPath(shoot3_pickupLoad, false);
pathState = PathState.PICKUP_LOAD;
timeStamp = currentTime;
@@ -193,7 +190,7 @@ public class Auto15Ball_Back extends LinearOpMode {
break;
case PICKUP_LOAD:
if (currentTime - timeStamp > loadPickupTime){
follower.followPath(pickupLoad_shootLoad, false);
follower.followPath(pickupLoad_shootLoad, true);
pathState = PathState.DRIVE_SHOOT_LOAD;
timeStamp = currentTime;
}
@@ -202,15 +199,13 @@ public class Auto15Ball_Back extends LinearOpMode {
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){
timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT_LOAD;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
spindexer.startBackShooting();
} else if (follower.isBusy()){
timeStamp = currentTime;
}
break;
case WAIT_SHOOT_LOAD:
if (currentTime - timeStamp > rapidShootTime ||
(spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
if (currentTime - timeStamp > rapidShootTime){
follower.followPath(shootLoad_intakeGate, false);
pathState = PathState.INTAKE_GATE;
timeStamp = currentTime;
@@ -218,7 +213,7 @@ public class Auto15Ball_Back extends LinearOpMode {
break;
case INTAKE_GATE:
if (!follower.isBusy()){
follower.followPath(intakeGate_shootGate, false);
follower.followPath(intakeGate_shootGate, true);
pathState = PathState.DRIVE_SHOOT_GATE;
timeStamp = currentTime;
}
@@ -227,19 +222,17 @@ public class Auto15Ball_Back extends LinearOpMode {
if (!follower.isBusy() && currentTime - timeStamp > rapidWaitTime){
timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT_GATE;
spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
spindexer.startBackShooting();
} else if (follower.isBusy()){
timeStamp = currentTime;
}
break;
case WAIT_SHOOT_GATE:
if (currentTime - timeStamp > rapidShootTime ||
(spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
follower.followPath(shootGate_intakeGate, false);
pathState = PathState.INTAKE_GATE;
if (currentTime - timeStamp > rapidShootTime){
// follower.followPath(shootGate_intakeGate, false);
follower.followPath(shootGate_leave, true);
pathState = PathState.LEAVE;
timeStamp = currentTime;
// TODO: add logic for leave
}
break;
case LEAVE:
@@ -285,14 +278,14 @@ public class Auto15Ball_Back extends LinearOpMode {
while (opModeInInit()){
park.unpark();
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
follower.update();
if (gamepad1.squareWasPressed()){
robot.setSpinPos(ServoPositions.spindexer_A2);
robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Closed);
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Closed);
robot.setTurretPos(Turret.neutralPosition);
}
if (gamepad1.crossWasPressed() && !initializeRobot){
@@ -320,6 +313,8 @@ public class Auto15Ball_Back extends LinearOpMode {
park.unpark();
}
NewShooterTest.transferPower = -0.6;
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());

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java
View File

@@ -18,6 +18,7 @@ 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.teleop.TeleopV4;
import org.firstinspires.ftc.teamcode.tests.NewShooterTest;
import org.firstinspires.ftc.teamcode.utilsv2.*;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
@@ -413,6 +414,8 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
park.unpark();
}
NewShooterTest.transferPower = -0.8;
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());

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

@@ -16,6 +16,7 @@ public class ServoPositions {
public static double spindexer_A3 = 0.54;
public static double spindexer_B1 = 0.73;
public static double spindexer_B2 = 0.92;
public static double spindexer_StopShooting = 0.44;
public static double spindexer_intakePos1 = 0.18; //0.13;

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

@@ -9,6 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
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.utils.MeasuringLoopTimes;
import org.firstinspires.ftc.teamcode.utilsv2.*;
@@ -36,6 +37,8 @@ public class TeleopV4 extends LinearOpMode {
private boolean firstTickFull = true;
private boolean intakeFull = true;
private boolean shooting = false;
public static int flywheelOffset = 0;
public static double hoodOffset = 0;
@Override
public void runOpMode() throws InterruptedException {
@@ -85,12 +88,15 @@ public class TeleopV4 extends LinearOpMode {
if (gamepad1.triangleWasPressed()){
VelocityCommander.lockFront = true;
VelocityCommander.lockBack = false;
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
} else if (gamepad1.squareWasPressed()){
VelocityCommander.lockBack = true;
VelocityCommander.lockFront = false;
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SPINDEXER_BACK);
} else if (gamepad1.circleWasPressed()){
VelocityCommander.lockBack = false;
VelocityCommander.lockFront = false;
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
}
TELE.addLine("Initialization is done");
TELE.addData("Starting Position", follower.getPose());
@@ -137,23 +143,29 @@ public class TeleopV4 extends LinearOpMode {
}
if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){
shooter.setFlywheelVelocity(2500);
robot.setHoodPos(0.6);
shooter.setFlywheelVelocity(2500 + flywheelOffset);
robot.setHoodPos(0.6 + hoodOffset);
}
if (gamepad1.triangleWasPressed()){
VelocityCommander.lockFront = true;
VelocityCommander.lockBack = false;
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.INTAKE);
TELE.addData("Front?:", true);
TELE.addData("Back?:", false);
} else if (gamepad1.squareWasPressed()){
VelocityCommander.lockBack = true;
VelocityCommander.lockFront = false;
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SPINDEXER_BACK);
spindexerTransferIntake.setSortedIntakeMode(SpindexerTransferIntake.SortedIntakeStates.IDLE);
TELE.addData("Front?:", false);
TELE.addData("Back?:", true);
} else if (gamepad1.circleWasPressed()){
VelocityCommander.lockBack = false;
VelocityCommander.lockFront = false;
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.INTAKE);
TELE.addData("Front?:", false);
TELE.addData("Back?:", true);
}
@@ -161,50 +173,65 @@ public class TeleopV4 extends LinearOpMode {
shooter.update(robot.voltage.getVoltage());
spindexerTransferIntake.update();
SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
if (VelocityCommander.lockBack){
robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
if (gamepad1.leftBumperWasPressed()){
spindexerTransferIntake.startBackShooting();
firstTickFull = true;
}
if (gamepad1.leftBumperWasPressed() &&
(state == SpindexerTransferIntake.RapidMode.INTAKE ||
state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
state == SpindexerTransferIntake.RapidMode.PULSE_OUT ||
state == SpindexerTransferIntake.RapidMode.PULSE_IN ||
state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {
spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
intakeFull = false;
firstTickFull = true;
shooting = true;
}
if (state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT){
shooting = false;
}
if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed() && !shooting){
intakeFull = true;
if (spindexerTransferIntake.getSortedIntakeStates() == SpindexerTransferIntake.SortedIntakeStates.REVERSE && firstTickFull){
gamepad1.rumble(100);
firstTickFull = false;
}
TELE.addData("Intake State", spindexerTransferIntake.getSortedIntakeStates());
} else {
intakeFull = false;
firstTickFull = true;
}
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
if (intakeFull && firstTickFull){
gamepad1.rumble(100);
firstTickFull = false;
}
if (gamepad1.leftBumperWasPressed() &&
(state == SpindexerTransferIntake.RapidMode.INTAKE ||
state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
state == SpindexerTransferIntake.RapidMode.PULSE_OUT ||
state == SpindexerTransferIntake.RapidMode.PULSE_IN ||
state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {
if (gamepad1.right_trigger > 0.5 &&
(state == SpindexerTransferIntake.RapidMode.INTAKE ||
state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
intakeFull = false;
firstTickFull = true;
shooting = true;
}
spindexerTransferIntake.setRapidMode(
SpindexerTransferIntake.RapidMode.HOLD_BALLS
);
}
if (state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT){
shooting = false;
}
if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
robot.setIntakePower(1);
robot.setTransferPower(-0.7);
if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed() && !shooting){
intakeFull = true;
} else {
intakeFull = false;
firstTickFull = true;
}
if (intakeFull && firstTickFull){
gamepad1.rumble(100);
firstTickFull = false;
}
if (gamepad1.right_trigger > 0.5 &&
(state == SpindexerTransferIntake.RapidMode.INTAKE ||
state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
spindexerTransferIntake.setRapidMode(
SpindexerTransferIntake.RapidMode.HOLD_BALLS
);
}
if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
robot.setIntakePower(1);
robot.setTransferPower(-0.7);
}
}
if (gamepad2.leftBumperWasPressed()){
@@ -219,6 +246,29 @@ public class TeleopV4 extends LinearOpMode {
parkTilter.unpark();
}
if (gamepad2.leftBumperWasPressed()){
limelightUsed = false;
} else if (gamepad2.rightBumperWasPressed()){
limelightUsed = true;
}
if (gamepad2.dpadUpWasPressed()){
hoodOffset+=0.02;
} else if (gamepad2.dpadDownWasPressed()){
hoodOffset-=0.02;
}
if (gamepad2.dpadRightWasPressed()){
flywheelOffset+=100;
} else if (gamepad2.dpadLeftWasPressed()){
flywheelOffset-=100;
}
if (gamepad2.crossWasPressed()){
flywheelOffset = 0;
hoodOffset = 0;
}
loopTimes.loop();
// TELE.addData("Loop Time Average", loopTimes.getAvgLoopTime());
// TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin());

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

@@ -1,7 +1,5 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -13,8 +11,11 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.teleop.TeleopV4;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
import org.firstinspires.ftc.teamcode.utilsv2.*;
import static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed;
@TeleOp
@Config
public class NewShooterTest extends LinearOpMode {
@@ -28,11 +29,14 @@ public class NewShooterTest extends LinearOpMode {
Flywheel flywheel;
VelocityCommander commander;
ParkTilter parkTilter;
MeasuringLoopTimes loopTimes;
private boolean firstTickFull = true;
private boolean intakeFull = true;
private boolean shooting = false;
public static int flywheelVelo = 0;
public static double hoodPos = 0.5;
public static double transferPower = -0.8;
public static boolean overrideTransferPower = false;
@Override
public void runOpMode() throws InterruptedException {
@@ -51,6 +55,7 @@ public class NewShooterTest extends LinearOpMode {
follower.setStartingPose(new Pose(0,0,0));
sleep(500);
follower.setPose(new Pose(0,0,0));
follower.update();
flywheel = new Flywheel(robot);
turret = new Turret(robot);
@@ -58,6 +63,9 @@ public class NewShooterTest extends LinearOpMode {
parkTilter = new ParkTilter(robot);
parkTilter.unpark();
loopTimes = new MeasuringLoopTimes();
loopTimes.init();
shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
shooter.setRedAlliance(Color.redAlliance);
@@ -69,9 +77,30 @@ public class NewShooterTest extends LinearOpMode {
limelightUsed = true;
TELE.addLine("Initialization Complete");
TELE.addLine("Initialization is done");
TELE.update();
while (opModeInInit()){
if (gamepad1.triangleWasPressed()){
VelocityCommander.lockFront = true;
VelocityCommander.lockBack = false;
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
} else if (gamepad1.squareWasPressed()){
VelocityCommander.lockBack = true;
VelocityCommander.lockFront = false;
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SPINDEXER_BACK);
spindexerTransferIntake.setSortedIntakeMode(SpindexerTransferIntake.SortedIntakeStates.IDLE);
} else if (gamepad1.circleWasPressed()){
VelocityCommander.lockBack = false;
VelocityCommander.lockFront = false;
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
}
TELE.addLine("Initialization is done");
TELE.addData("Front?:", VelocityCommander.lockFront);
TELE.addData("Back?:", VelocityCommander.lockBack);
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
@@ -97,6 +126,7 @@ public class NewShooterTest extends LinearOpMode {
}
follower.update();
Pose currentPose = follower.getPose();
if (gamepad1.dpadLeftWasPressed()){
shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
@@ -111,56 +141,114 @@ public class NewShooterTest extends LinearOpMode {
robot.setHoodPos(hoodPos);
}
if (gamepad1.triangleWasPressed()){
VelocityCommander.lockFront = true;
VelocityCommander.lockBack = false;
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
TELE.addData("Front?:", true);
TELE.addData("Back?:", false);
} else if (gamepad1.squareWasPressed()){
VelocityCommander.lockBack = true;
VelocityCommander.lockFront = false;
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SPINDEXER_BACK);
TELE.addData("Front?:", false);
TELE.addData("Back?:", true);
} else if (gamepad1.circleWasPressed()){
VelocityCommander.lockBack = false;
VelocityCommander.lockFront = false;
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
TELE.addData("Front?:", false);
TELE.addData("Back?:", true);
}
// shooter.setTurretPosition(turretPos);
shooter.update(robot.voltage.getVoltage());
spindexerTransferIntake.update();
if (VelocityCommander.lockBack){
if (gamepad1.leftBumperWasPressed() && spindexerTransferIntake.getSortedIntakeStates() == SpindexerTransferIntake.SortedIntakeStates.REVERSE){
spindexerTransferIntake.startBackShooting();
firstTickFull = true;
}
if (spindexerTransferIntake.getSortedIntakeStates() == SpindexerTransferIntake.SortedIntakeStates.REVERSE && firstTickFull){
gamepad1.rumble(100);
firstTickFull = false;
}
} else {
SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
if (gamepad1.leftBumperWasPressed() &&
(state == SpindexerTransferIntake.RapidMode.INTAKE ||
state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
state == SpindexerTransferIntake.RapidMode.PULSE_OUT ||
state == SpindexerTransferIntake.RapidMode.PULSE_IN ||
state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {
spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
intakeFull = false;
firstTickFull = true;
shooting = true;
}
if (state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT){
shooting = false;
}
if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed() && !shooting){
intakeFull = true;
} else {
intakeFull = false;
firstTickFull = true;
}
if (intakeFull && firstTickFull){
gamepad1.rumble(100);
firstTickFull = false;
}
if (gamepad1.right_trigger > 0.5 &&
(state == SpindexerTransferIntake.RapidMode.INTAKE ||
state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
spindexerTransferIntake.setRapidMode(
SpindexerTransferIntake.RapidMode.HOLD_BALLS
);
}
if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
robot.setIntakePower(1);
robot.setTransferPower(-0.7);
}
}
if (gamepad2.leftBumperWasPressed()){
limelightUsed = false;
} else if (gamepad2.rightBumperWasPressed()){
limelightUsed = true;
}
SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
if (gamepad1.leftBumperWasPressed() &&
(state == SpindexerTransferIntake.RapidMode.INTAKE ||
state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
state == SpindexerTransferIntake.RapidMode.PULSE_OUT ||
state == SpindexerTransferIntake.RapidMode.PULSE_IN ||
state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {
spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
}
if (gamepad1.right_trigger > 0.5 &&
(state == SpindexerTransferIntake.RapidMode.INTAKE ||
state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
spindexerTransferIntake.setRapidMode(
SpindexerTransferIntake.RapidMode.HOLD_BALLS
);
}
if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
robot.setIntakePower(1);
}
if (gamepad1.dpad_down){
parkTilter.park();
} else if (gamepad1.dpad_up) {
parkTilter.unpark();
}
loopTimes.loop();
// TELE.addData("Loop Time Average", loopTimes.getAvgLoopTime());
// TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin());
// TELE.addData("Loop Time Min", loopTimes.getMinLoopTimeOneMin());
//
TELE.addData("Distance From Goal", commander.getDistance());
TELE.addData("Hood Position", commander.getHoodPredicted());
TELE.addData("Transfer Power", commander.getTransferPow());
// TELE.addData("Hood Position", commander.getHoodPredicted());
// TELE.addData("Transfer Power", robot.transfer.getPower());
TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
TELE.addData("Manuel Velocity RPM", flywheelVelo);
TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
TELE.addData("TX:", turret.getTX());
//
// TELE.addData("Current Position", currentPose);
//
// TELE.addData("Current LL Pipeline", turret.pipeline());
//
TELE.update();
}

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

@@ -7,6 +7,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.pedropathing.follower.Follower;
import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.teleop.TeleopV4;
@Config
public class Shooter {
@@ -138,8 +139,8 @@ public class Shooter {
flywheelVelocity = commander.getPredictedRPM();
robot.setHoodPos(commander.getHoodPredicted());
fly.manageFlywheel(flywheelVelocity);
robot.setHoodPos(commander.getHoodPredicted() + TeleopV4.hoodOffset);
fly.manageFlywheel(flywheelVelocity + TeleopV4.flywheelOffset);
fly.setF(voltage);
break;
case READ_OBELISK:
@@ -180,9 +181,8 @@ public class Shooter {
);
flywheelVelocity = commander.getPredictedRPM();
robot.setHoodPos(commander.getHoodPredicted());
fly.manageFlywheel(flywheelVelocity);
robot.setHoodPos(commander.getHoodPredicted() + TeleopV4.hoodOffset);
fly.manageFlywheel(flywheelVelocity + TeleopV4.flywheelOffset);
break;
case MANUAL_FLYWHEEL_TRACK_TURR:

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

@@ -179,7 +179,9 @@ public class SpindexerTransferIntake {
public enum SpindexerMode {
RAPID,
SORTED,
SHOOT_SORTED
SHOOT_SORTED,
SPINDEXER_BACK,
SHOOT_BACK
}
public enum BallStates {
@@ -241,6 +243,15 @@ public class SpindexerTransferIntake {
SpindexerMode.SHOOT_SORTED
);
}
public void startBackShooting(){
setShootState(
SortedShootState.IDLE
);
setSpindexerMode(
SpindexerMode.SHOOT_BACK
);
}
public void setRapidMode(RapidMode newMode) {
if (rapidMode != newMode) {
rapidMode = newMode;
@@ -263,6 +274,10 @@ public class SpindexerTransferIntake {
return this.rapidMode;
}
public SpindexerMode getSpindexerMode(){return this.mode;}
public SortedIntakeStates getSortedIntakeStates(){return this.sortedIntakeStates;}
private long stateTime() {
return System.currentTimeMillis() - stateStartTime;
}
@@ -538,20 +553,130 @@ public class SpindexerTransferIntake {
}
break;
case REVERSE:
robot.setTransferPower(-0.3);
robot.setIntakePower(-0.1);
if (ballTicks > 4){
robot.setTransferPower(0);
} else if (ballTicks > 1){
robot.setTransferPower(1);
} else {
robot.setTransferPower(-1);
}
ballTicks++;
robot.setIntakePower(-0.7);
robot.setSpinPos(ServoPositions.spindexer_StopShooting);
break;
}
break;
case SPINDEXER_BACK:
robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
switch (sortedIntakeStates) {
case NOTHING:
break;
case IDLE:
ballColors[0] = BallStates.UNKNOWN;
ballColors[1] = BallStates.UNKNOWN;
ballColors[2] = BallStates.UNKNOWN;
robot.setRapidFireBlockerPos(
ServoPositions.rapidFireBlocker_Open
);
robot.setSpindexBlockerPos(
ServoPositions.spindexBlocker_Closed
);
robot.setSpinPos(
ServoPositions.spindexer_A1
);
robot.setTransferServoPos(
ServoPositions.transferServo_out
);
robot.setIntakePower(1);
robot.setTransferPower(-0.7);
if (sortedStateTime() > 200) {
setSortedIntakeMode(SortedIntakeStates.INTAKE_1);
}
break;
case INTAKE_1:
robot.setIntakePower(1);
robot.setTransferPower(-0.7);
if (robot.insideBeam.isPressed()) {
ballTicks++;
if (ballTicks > 0) {
robot.setSpinPos(ServoPositions.spindexer_A2);
setSortedIntakeMode(SortedIntakeStates.DELAY_1);
ballTicks = 0;
greenTicks = 0;
}
}
break;
case DELAY_1:
robot.setSpinPos(ServoPositions.spindexer_A2);
if (sortedStateTime() > spinMovementTime) {
setSortedIntakeMode(SortedIntakeStates.INTAKE_2);
}
break;
case INTAKE_2:
robot.setIntakePower(1);
robot.setTransferPower(-1);
if (robot.insideBeam.isPressed()) {
ballTicks++;
if (ballTicks > 0) {
robot.setSpinPos(ServoPositions.spindexer_A3);
setSortedIntakeMode(SortedIntakeStates.DELAY_2);
ballTicks = 0;
greenTicks = 0;
}
}
break;
case DELAY_2:
robot.setSpinPos(
ServoPositions.spindexer_A3
);
if (sortedStateTime() > spinMovementTime) {
setSortedIntakeMode(
SortedIntakeStates.INTAKE_3
);
}
break;
case INTAKE_3:
robot.setIntakePower(1);
robot.setTransferPower(-1);
if (robot.insideBeam.isPressed()) {
ballTicks++;
if (ballTicks > 0) {
setSortedIntakeMode(SortedIntakeStates.REVERSE);
ballTicks = 0;
greenTicks = 0;
}
}
break;
case REVERSE:
if (ballTicks > 3){
robot.setTransferPower(0);
} else if (ballTicks > 1){
robot.setTransferPower(1);
} else {
robot.setTransferPower(-1);
}
ballTicks++;
robot.setIntakePower(-0.7);
robot.setSpinPos(ServoPositions.spindexer_StopShooting);
break;
}
break;
case SHOOT_SORTED:
ballTicks = 0;
robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
if (Shooter.manualFlywheel) {
robot.setTransferPower(NewShooterTest.transferPower);
} else {
robot.setTransferPower(commander.getTransferPow());
robot.setTransferPower(-0.8);
}
@@ -740,6 +865,203 @@ public class SpindexerTransferIntake {
}
break;
case SHOOT_BACK:
ballTicks = 0;
robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
if (Shooter.manualFlywheel) {
robot.setTransferPower(NewShooterTest.transferPower);
} else {
robot.setTransferPower(-0.8);
}
switch (shootState) {
case IDLE:
shootOrder = buildShootOrder(
ballColors,
desiredPattern
);
setShootState(SortedShootState.MOVE_TO_1);
mode = SpindexerMode.SHOOT_SORTED;
break;
case MOVE_TO_1:
robot.setSpinPos(ServoPositions.spindexer_A3);
robot.setRapidFireBlockerPos(
ServoPositions.rapidFireBlocker_Open
);
robot.setSpindexBlockerPos(
ServoPositions.spindexBlocker_Closed
);
setShootState(
SortedShootState.WAIT_FOR_1
);
break;
case WAIT_FOR_1:
if (shootStateTime() > 150) {
setShootState(
SortedShootState.SHOOT_1
);
}
break;
case SHOOT_1:
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
robot.setTransferServoPos(ServoPositions.transferServo_in);
if (shootStateTime() > 300) {
setShootState(
SortedShootState.RETRACT_1
);
}
break;
case RETRACT_1:
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Closed);
robot.setTransferServoPos(ServoPositions.transferServo_out);
if (shootStateTime() > 150) {
setShootState(
SortedShootState.MOVE_TO_2
);
}
break;
case MOVE_TO_2:
robot.setSpinPos(ServoPositions.spindexer_A2);
setShootState(
SortedShootState.WAIT_FOR_2
);
break;
case WAIT_FOR_2:
if (shootStateTime() > 150) {
setShootState(
SortedShootState.SHOOT_2
);
}
break;
case SHOOT_2:
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
robot.setTransferServoPos(ServoPositions.transferServo_in);
if (shootStateTime() > 300) {
setShootState(
SortedShootState.RETRACT_2
);
}
break;
case RETRACT_2:
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Closed);
robot.setTransferServoPos(ServoPositions.transferServo_out);
if (shootStateTime() > 150) {
setShootState(
SortedShootState.MOVE_TO_3
);
}
break;
case MOVE_TO_3:
robot.setSpinPos(ServoPositions.spindexer_A1);
setShootState(
SortedShootState.WAIT_FOR_3
);
break;
case WAIT_FOR_3:
if (shootStateTime() > 150) {
setShootState(
SortedShootState.SHOOT_3
);
}
break;
case SHOOT_3:
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
robot.setTransferServoPos(ServoPositions.transferServo_in);
if (shootStateTime() > 300) {
setShootState(
SortedShootState.RETRACT_3
);
}
break;
case RETRACT_3:
robot.setTransferServoPos(ServoPositions.transferServo_out);
if (shootStateTime() > 150) {
setShootState(
SortedShootState.DONE
);
}
break;
case DONE:
robot.setRapidFireBlockerPos(
ServoPositions.rapidFireBlocker_Open
);
robot.setSpindexBlockerPos(
ServoPositions.spindexBlocker_Closed
);
robot.setSpinPos(
ServoPositions.spindexer_A1
);
robot.setTransferServoPos(
ServoPositions.transferServo_out
);
robot.setIntakePower(1);
robot.setTransferPower(-0.7);
if (shootStateTime() > 250) {
setSortedIntakeMode(
SortedIntakeStates.IDLE
);
mode = SpindexerMode.SPINDEXER_BACK;
}
break;
}
break;
}
}

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

@@ -25,8 +25,8 @@ public class Turret {
PIDController bearingPID;
boolean bearingAligned = false;
public int LL_COAST_TICKS = 5;
public static double TARGET_POSITION_TOLERANCE = 0.65;
public static double alphaTX = 0.5;
public static double TARGET_POSITION_TOLERANCE = 1.5;
public static double alphaTX = 0.3;
private double targetTx = 0;
private double currentTrackOffset = 0;
private double llCoast = 0;

20
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java
View File

@@ -10,8 +10,8 @@ public class VelocityCommander {
public static double yAccK = 0.025; // TODO: Tune
public static boolean lockFront = false;
public static boolean lockBack = false;
public static int farBound = 140;
public static int closeBound = 110;
public static int farBound = 138;
public static int closeBound = 112;
public static double errorHoodAdjustment = 0.0005;
private double hoodPos = 0.88;
private double transferPow = -1;
@@ -24,6 +24,10 @@ public class VelocityCommander {
final double veloC = -0.0739531;
final double veloD = 5.16759;
final double veloE = 62.45781;
final double veloF = -0.0000833333;
final double veloG = 0.0377857;
final double veloH = -4.55067;
final double veloI = 456.97486;
private double distToRPM (double dist){
double currentVelo;
if (lockFront && dist > closeBound){
@@ -33,8 +37,8 @@ public class VelocityCommander {
}
if (dist < 54) {
velo = 2000;
} else if (dist > 181){
velo = 3600;
} else if (dist > 174){
velo = 3700;
} else {
currentVelo = veloA*Math.pow(dist, 4) +
veloB*Math.pow(dist, 3) +
@@ -50,6 +54,8 @@ public class VelocityCommander {
final double hoodB = -0.0000204165;
final double hoodC = -0.00252089;
final double hoodD = 1.06154;
final double hoodE = -0.002;
final double hoodF = 0.918;
private void distToHood (double dist){
if (dist > 174){
hoodPos = 0.48;
@@ -69,9 +75,9 @@ public class VelocityCommander {
private void distToTransferPow(double dist, double voltage){
if (dist < 140){
transferPow = -0.8;
transferPow = -0.75;
} else {
transferPow = -0.5;
transferPow = -0.6;
}
}
public double getTransferPow(){return transferPow;}
@@ -94,7 +100,7 @@ public class VelocityCommander {
distToTransferPow(predictedDist, voltage);
distToRPM(predictedDist);
hoodPos += adjustHood(predictedDist, velocity, velo);
// hoodPos += adjustHood(predictedDist, velocity, velo);
}
public double adjustHood(double dist, double currentVelocity, double targetVelocity){