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

teleop almost there

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-04 16:06:27 -05:00
parent deefa19be4
commit c15b9d58d4
5 changed files with 30 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -53,7 +53,7 @@ public class TeleopV4 extends LinearOpMode {
parkTilter = new ParkTilter(robot);
shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel);
shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
shooter.setState(Shooter.ShooterState.TRACK_GOAL);
spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
@@ -64,9 +64,7 @@ public class TeleopV4 extends LinearOpMode {
if (isStopRequested()) return;
while (opModeIsActive()) {
//Drivetrain
drivetrain.drive(
-gamepad1.right_stick_y,
gamepad1.right_stick_x,
@@ -113,6 +111,11 @@ public class TeleopV4 extends LinearOpMode {
parkTilter.unpark();
}
TELE.addData("Distance From Goal", commander.getDistance());
TELE.addData("Hood Position", commander.getHoodPredicted());
TELE.addData("Transfer Power", robot.transfer.getPower());
TELE.addData("Velocity RPM", commander.getPredictedRPM());
TELE.update();
}

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

@@ -14,6 +14,7 @@ import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
import org.firstinspires.ftc.teamcode.utilsv2.Robot;
import org.firstinspires.ftc.teamcode.utilsv2.Shooter;
import org.firstinspires.ftc.teamcode.utilsv2.Turret;
import org.firstinspires.ftc.teamcode.utilsv2.VelocityCommander;
@Config
@TeleOp
@@ -25,6 +26,7 @@ public class NewShooterTest extends LinearOpMode {
Shooter shooter;
MultipleTelemetry TELE;
Follower follower;
VelocityCommander commander;
public static boolean intake = true;
@@ -61,6 +63,7 @@ public class NewShooterTest extends LinearOpMode {
flywheel = new Flywheel(robot);
turret = new Turret(robot);
commander = new VelocityCommander();
shooter = new Shooter(
robot,
@@ -68,7 +71,8 @@ public class NewShooterTest extends LinearOpMode {
follower,
true,
turret,
flywheel
flywheel,
commander
);
shooter.setState(Shooter.ShooterState.MANUAL);

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

@@ -21,12 +21,12 @@ public class Shooter {
Follower follow;
public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance, Turret turret, Flywheel flywheel) {
public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance, Turret turret, Flywheel flywheel, VelocityCommander com) {
this.robot = rob;
this.fly = flywheel;
this.turr = turret;
this.follow = follower;
this.commander = new VelocityCommander();
this.commander = com;
setRedAlliance(redAlliance);

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

@@ -66,10 +66,6 @@ public class SpindexerTransferIntake {
public void update() {
if (mode == SpindexerMode.RAPID && rapidMode == RapidMode.INTAKE){
robot.setTransferPower(-0.7);
}
switch (mode) {
case RAPID:
@@ -89,6 +85,7 @@ public class SpindexerTransferIntake {
robot.setSpinPos(
ServoPositions.spindexer_A2
);
robot.setTransferPower(-0.7);
robot.setTransferServoPos(
ServoPositions.transferServo_out
);
@@ -106,6 +103,7 @@ public class SpindexerTransferIntake {
if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
setRapidMode(RapidMode.BEFORE_PULSE_OUT);
}
robot.setTransferPower(-0.3);
break;
@@ -163,6 +161,8 @@ public class SpindexerTransferIntake {
setRapidMode(RapidMode.SHOOT);
}
robot.setTransferPower(commander.getTransferPow());
break;
case SHOOT:
@@ -173,6 +173,9 @@ public class SpindexerTransferIntake {
if (stateTime() >= 400) {
setRapidMode(RapidMode.INTAKE);
}
robot.setTransferPower(commander.getTransferPow());
break;
}
break;

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

@@ -8,6 +8,7 @@ public class VelocityCommander {
private final double yAccK = 0; // TODO: Tune
private double hoodPos = 0.88;
private double transferPow = -1;
private double veloRPM = 2000;
public VelocityCommander() {}
@@ -46,7 +47,7 @@ public class VelocityCommander {
}
private final double hoodA = -4.3276177*Math.pow(10, -13);
private final double hoodB = 2.68062979*Math.pow(10, -11);
private final double hoodB = 2.68062979*Math.pow(10, -10);
private final double hoodC = -7.12859632*Math.pow(10, -8);
private final double hoodD = 0.0000106010785;
private final double hoodE = -0.000960693973;
@@ -73,6 +74,9 @@ public class VelocityCommander {
hoodPos = Math.max(0.35, Math.min(0.88, hoodPos));
}
}
public double getHoodPredicted(){
return hoodPos;
}
private void distToTransferPow(double dist){
if (dist < 118){
@@ -83,13 +87,8 @@ public class VelocityCommander {
transferPow = -0.5;
}
}
public double getTransferPow(){return transferPow;}
public double getHoodPredicted(){
return hoodPos;
}
// 27
public double getVeloStationary (double distance){
return distToRPM(distance);
@@ -105,9 +104,12 @@ public class VelocityCommander {
predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight*goalHeight);
distToHood(predictedDist);
return distToRPM(predictedDist);
distToTransferPow(predictedDist);
veloRPM = distToRPM(predictedDist);
return veloRPM;
}
public double getPredictedRPM(){return veloRPM;}
public double getDistance(){return predictedDist;}
}