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

fly wheel by velocity - in progress

Browse Source
This commit is contained in:
DanTheMan-byte
2025-11-13 22:18:29 -06:00
parent 5e41560fd5
commit b6c8ea1a28
5 changed files with 46 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java
View File

@@ -170,16 +170,16 @@ public class Blue extends LinearOpMode {
while(opModeInInit()) {
if (gamepad2.dpadUpWasPressed()){
hoodDefault -= 0.02;
hoodDefault -= 0.01;
}
if (gamepad2.dpadDownWasPressed()){
hoodDefault += 0.02;
hoodDefault += 0.01;
}
robot.hood.setPosition(hoodDefault);
shooter.setTurretPosition(0.3);
shooter.setTurretPosition(turret_blue);
aprilTag.initTelemetry();

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java
View File

@@ -99,45 +99,45 @@ public class Red extends LinearOpMode {
TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0))
TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
TrajectoryActionBuilder traj2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.turnTo(Math.toRadians(135))
.strafeToLinearHeading(new Vector2d(x2, y2), h2 );
TrajectoryActionBuilder traj3 = drive.actionBuilder(new Pose2d(x2, y2, h2))
TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(x2, y2, h2))
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
TrajectoryActionBuilder traj4 = drive.actionBuilder(new Pose2d(x1, y1, h1))
TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x2_b, y2_b), h2_b )
.strafeToLinearHeading(new Vector2d(x3, y3), h3 );
TrajectoryActionBuilder traj5 = drive.actionBuilder(new Pose2d(x3, y3, h3))
TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(x3, y3, h3))
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1))
TrajectoryActionBuilder park = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x1, y1+30), h1 );
while(opModeInInit()) {
if (gamepad2.dpadUpWasPressed()){
hoodDefault -= 0.02;
hoodDefault -= 0.01;
}
if (gamepad2.dpadDownWasPressed()){
hoodDefault += 0.02;
hoodDefault += 0.01;
}
robot.hood.setPosition(hoodDefault);
shooter.setTurretPosition(0.33);
shooter.setTurretPosition(turret_red);
aprilTag.initTelemetry();
@@ -165,7 +165,7 @@ public class Red extends LinearOpMode {
Actions.runBlocking(
new ParallelAction(
traj1.build()
shoot0.build()
)
);
@@ -245,12 +245,12 @@ public class Red extends LinearOpMode {
robot.intake.setPower(1);
Actions.runBlocking(
traj2.build()
pickup1.build()
);
Actions.runBlocking(
traj3.build()
shoot1.build()
);
shooter.setManualPower(1);
@@ -329,12 +329,12 @@ public class Red extends LinearOpMode {
robot.intake.setPower(1);
Actions.runBlocking(
traj4.build()
pickup2.build()
);
Actions.runBlocking(
traj5.build()
shoot2.build()
);
shooter.setManualPower(1);
@@ -411,7 +411,7 @@ public class Red extends LinearOpMode {
spindexer.outtake3();
Actions.runBlocking(
traj6.build()
park.build()
);
drive.updatePoseEstimate();

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

@@ -16,8 +16,12 @@ public class ServoPositions {
public static double transferServo_out = 0.13;
public static double transferServo_in = 0.4;
public static double transferServoPos = 0.5;
public static double hoodDefault = 0.35;
public static double turret_red = 0.33;
public static double turret_blue = 0.3;
}

26
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
View File

@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.subsystems;
import static java.lang.Runtime.getRuntime;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.controller.PIDController;
@@ -45,6 +47,7 @@ public class Shooter implements Subsystem {
private double p = 0.0003, i = 0, d = 0.00001;
private PIDController controller;
private double pow = 0.0;
@@ -100,8 +103,9 @@ public class Shooter implements Subsystem {
telemetry.addData("hoodPos", gethoodPosition());
telemetry.addData("turretPos", getTurretPosition());
telemetry.addData("PID Coefficients", "P: %.6f, I: %.6f, D: %.6f", p, i, d);
telemetry.addData("Current Fly 1", fly1.getCurrent(CurrentUnit.AMPS));
telemetry.addData("Current Fly 2", fly2.getCurrent(CurrentUnit.AMPS));
telemetry.addData("Power Fly 1", fly1.getPower());
telemetry.addData("Pow Fly 2", fly2.getPower());
telemetry.addData("Pow", pow);
}
@@ -118,7 +122,7 @@ public class Shooter implements Subsystem {
public void setTurretPosition(double pos) {turretPos = pos;}
public double getVelocity() {
return ((double) ((fly1.getVelocity(AngleUnit.DEGREES) + fly2.getVelocity(AngleUnit.DEGREES)) /2));
return ((double) ((fly1.getVelocity(AngleUnit.DEGREES))));
}
public void setVelocity(double vel){velocity = vel;}
@@ -292,13 +296,23 @@ public class Shooter implements Subsystem {
}
else if (Objects.equals(shooterMode, "VEL")){
fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
fly1.setPower(pow);
fly2.setPower(pow);
fly1.setVelocity(velocity);
fly2.setPower(fly1.getPower());
if (velocity == 0.0){
fly1.setPower(0);
fly2.setPower(0);
} else if (-velocity < -Math.abs(fly1.getVelocity())){
while (-velocity < -Math.abs(fly1.getVelocity())){
pow += 0.001;
fly1.setPower(pow);
fly2.setPower(pow);
}
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java
View File

@@ -1,7 +1,8 @@
package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodDefault;
import static org.firstinspires.ftc.teamcode.constants.Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
@@ -215,7 +216,6 @@ public class TeleopV1 extends LinearOpMode {
waitForStart();
if (isStopRequested()) return;
@@ -233,6 +233,7 @@ public class TeleopV1 extends LinearOpMode {
TELE.addData("off", offset);
robot.transferServo.setPosition(transferServoPos);
robot.hood.setPosition(pos);