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

Merge remote-tracking branch 'origin/danielv5' into update-teleop

Browse Source 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java
This commit is contained in:
Keshav Anand
2026-06-03 14:12:26 -05:00
parent d8cf594828 e658ec044c
commit 209c34b3fd
8 changed files with 485 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java
View File

@@ -22,28 +22,28 @@ 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.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utilsv2.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting;
import org.firstinspires.ftc.teamcode.utils.Turret;
import org.firstinspires.ftc.teamcode.utilsv2.Turret;
import java.util.List;
@Config
@Autonomous (preselectTeleOp = "TeleopV3")
@Autonomous (preselectTeleOp = "TeleopV4")
public class Auto12BallPedroPathing extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
Flywheel flywheel;
Targeting targeting;
Targeting.Settings targetingSettings;
// Flywheel flywheel;
// Targeting targeting;
// Targeting.Settings targetingSettings;
Follower follower;
Turret turret;
Spindexer spindexer;
Servos servos;
// Turret turret;
// Spindexer spindexer;
// Servos servos;
MeasuringLoopTimes loopTimes;
// Wait Times
@@ -222,10 +222,10 @@ public class Auto12BallPedroPathing extends LinearOpMode {
driveShoot(PathState.WAIT_SHOOT3, currentTime);
break;
case WAIT_SHOOT3:
if (spindexer.shootAllComplete()){
spindexer.resetSpindexer();
TELE.addLine("Done Auto");
}
// if (spindexer.shootAllComplete()){
// spindexer.resetSpindexer();
// TELE.addLine("Done Auto");
// }
break;
default:
break;
@@ -237,21 +237,21 @@ public class Auto12BallPedroPathing extends LinearOpMode {
private void intakePowerDown(double stamp, double currentTime) {
double pow = (stamp - currentTime) / 2; // adjust denominator to see how much time to adjust
if (pow < -1) {pow = 0;}
spindexer.setIntakePower(pow);
// spindexer.setIntakePower(pow);
}
private void driveShoot(PathState nextState, double currentTime){
if (!follower.isBusy()){
pathState = nextState;
timeStamp = currentTime;
spindexer.prepareShootAllContinous();
// spindexer.prepareShootAllContinous();
}
}
private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) {
if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime) {
spindexer.resetSpindexer();
if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() ||
// spindexer.resetSpindexer();
pathState = nextState;
follower.followPath(nextPath, true);
spindexer.setIntakePower(1);
// spindexer.setIntakePower(1);
}
}
private void drivePickup(PathState nextState, PathChain nextPath) {
@@ -286,24 +286,25 @@ public class Auto12BallPedroPathing extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
Robot.resetInstance();
robot = Robot.getInstance(hardwareMap);
List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
for (LynxModule hub : allHubs) {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
}
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
flywheel = new Flywheel(hardwareMap);
targeting = new Targeting();
targetingSettings = new Targeting.Settings(0,0);
// flywheel = new Flywheel(hardwareMap);
// targeting = new Targeting();
// targetingSettings = new Targeting.Settings(0,0);
follower = Constants.createFollower(hardwareMap);
follower.setStartingPose(new Pose(72,72,0));
turret = new Turret(robot, TELE, robot.limelight);
spindexer = new Spindexer(hardwareMap);
servos = new Servos(hardwareMap);
// turret = new Turret(robot, TELE, robot.limelight);
// spindexer = new Spindexer(hardwareMap);
// servos = new Servos(hardwareMap);
loopTimes = new MeasuringLoopTimes();
loopTimes.init();
robot.light.setPosition(Color.LightRed);
// robot.light.setPosition(Color.LightRed);
boolean initializeRobot = false;
while (opModeInInit()){
@@ -311,11 +312,11 @@ public class Auto12BallPedroPathing extends LinearOpMode {
if (gamepad1.crossWasPressed() && !initializeRobot){
Color.redAlliance = !Color.redAlliance;
if (Color.redAlliance){
robot.light.setPosition(Color.LightRed);
} else {
robot.light.setPosition(Color.LightBlue);
}
// if (Color.redAlliance){
// robot.light.setPosition(Color.LightRed);
// } else {
// robot.light.setPosition(Color.LightBlue);
// }
double[] xPoses = {startPoseX, shoot0X,
drivePickup1X, pickup1X, shoot1X,
@@ -338,8 +339,8 @@ public class Auto12BallPedroPathing extends LinearOpMode {
buildPaths();
sleep(2000);
turret.setTurret(turrDefault);
servos.setSpinPos(spinStartPos);
// turret.setTurret(turrDefault);
// servos.setSpinPos(spinStartPos);
}
TELE.addData("Red Alliance?", Color.redAlliance);
@@ -352,27 +353,27 @@ public class Auto12BallPedroPathing extends LinearOpMode {
if (isStopRequested()) return;
robot.transfer.setPower(1);
// robot.transfer.setPower(1);
limelightUsed = false;
while (opModeIsActive()){
follower.update();
pathStateMachine();
Pose currentPose = follower.getPose();
teleStartPoseX = currentPose.getX();
teleStartPoseY = currentPose.getY();
teleStartPoseH = Math.toDegrees(currentPose.getHeading());
turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH)));
targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(targetingSettings.flywheelRPM);
servos.setHoodPos(targetingSettings.hoodAngle);
if (driveToShoot()){servos.setSpinPos(spinStartPos);}
else {spindexer.processIntake();}
// teleStartPoseX = currentPose.getX();
// teleStartPoseY = currentPose.getY();
// teleStartPoseH = Math.toDegrees(currentPose.getHeading());
//
// turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH)));
// targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate);
//
// double voltage = robot.voltage.getVoltage();
// flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
// flywheel.manageFlywheel(targetingSettings.flywheelRPM);
// servos.setHoodPos(targetingSettings.hoodAngle);
//
// if (driveToShoot()){servos.setSpinPos(spinStartPos);}
// else {spindexer.processIntake();}
for (LynxModule hub : allHubs) {
hub.clearBulkCache();

366
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java Normal file
View File

@@ -0,0 +1,366 @@
package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.BezierCurve;
import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose;
import com.pedropathing.paths.PathChain;
import com.qualcomm.hardware.lynx.LynxModule;
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.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
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;
@Config
@Autonomous (preselectTeleOp = "TeleopV4")
public class Auto12Ball_Back_Sorted extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
Follower follower;
MeasuringLoopTimes loopTimes;
// Wait Times
public static double shootTime = 2;
public static double openGateTime = 1.5;
// Extra Variables
public static double intakePower = 0.3;
double shootX, shootY, shootH;
// Initialize path state machine
private enum PathState {
PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0,
PICKUP1, DRIVE_OPENGATE, 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 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 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 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 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;
Pose startPose, pushBot, shoot0Control, shoot0,
pickup1Control, pickup1, openGateControl, openGate, shoot1Control, shoot1,
drivePickup2, pickup2, shoot2Control, shoot2,
drivePickup3, pickup3, shoot3Control, shoot3;
private void initializePoses(){
startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH));
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));
shoot1Control = new Pose(shoot1ControlX, shoot1ControlY);
shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H));
drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H));
pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H));
shoot2Control = new Pose(shoot2ControlX, shoot2ControlY);
shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H));
drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H));
pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H));
shoot3Control = new Pose(shoot3ControlX, shoot3ControlY);
shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H));
}
//Building Paths
PathChain startPose_pushBot, pushBot_shoot0,
shoot0_pickup1, pickup1_openGate, openGate_shoot1,
shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2,
shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3;
private void buildPaths(){
startPose_pushBot = follower.pathBuilder()
.addPath(new BezierLine(startPose, pushBot))
.setLinearHeadingInterpolation(startPose.getHeading(), pushBot.getHeading())
.build();
pushBot_shoot0 = follower.pathBuilder()
.addPath(new BezierCurve(pushBot, shoot0Control, shoot0))
.setLinearHeadingInterpolation(pushBot.getHeading(), shoot0.getHeading())
.build();
shoot0_pickup1 = follower.pathBuilder()
.addPath(new BezierCurve(shoot0, pickup1Control, pickup1))
.setLinearHeadingInterpolation(shoot0.getHeading(), pickup1.getHeading())
.build();
pickup1_openGate = follower.pathBuilder()
.addPath(new BezierCurve(pickup1, openGateControl, openGate))
.setLinearHeadingInterpolation(pickup1.getHeading(), openGate.getHeading())
.build();
openGate_shoot1 = follower.pathBuilder()
.addPath(new BezierCurve(openGate, shoot1Control, shoot1))
.setLinearHeadingInterpolation(openGate.getHeading(), shoot1.getHeading())
.build();
shoot1_drivePickup2 = follower.pathBuilder()
.addPath(new BezierLine(shoot1, drivePickup2))
.setLinearHeadingInterpolation(shoot1.getHeading(), drivePickup2.getHeading())
.build();
drivePickup2_pickup2 = follower.pathBuilder()
.addPath(new BezierLine(drivePickup2, pickup2))
.setLinearHeadingInterpolation(drivePickup2.getHeading(), pickup2.getHeading())
.build();
pickup2_shoot2 = follower.pathBuilder()
.addPath(new BezierCurve(pickup2, shoot2Control, shoot2))
.setLinearHeadingInterpolation(pickup2.getHeading(), shoot2.getHeading())
.build();
shoot2_drivePickup3 = follower.pathBuilder()
.addPath(new BezierLine(shoot2, drivePickup3))
.setLinearHeadingInterpolation(shoot2.getHeading(), drivePickup3.getHeading())
.build();
drivePickup3_pickup3 = follower.pathBuilder()
.addPath(new BezierLine(drivePickup3, pickup3))
.setLinearHeadingInterpolation(drivePickup3.getHeading(), pickup3.getHeading())
.build();
pickup3_shoot3 = follower.pathBuilder()
.addPath(new BezierCurve(pickup3, shoot3Control, shoot3))
.setLinearHeadingInterpolation(pickup3.getHeading(), shoot3.getHeading())
.build();
}
//Path State Machine
private boolean startAuto = true;
private double timeStamp = 0;
private void pathStateMachine(){
double currentTime = (double) System.currentTimeMillis() / 1000;
switch(pathState){
case PUSHBOT:
if (startAuto){
follower.followPath(startPose_pushBot, true);
startAuto = false;
shootX = shoot0X;
shootY = shoot0Y;
shootH = shoot0H;
}
if (!follower.isBusy()){
follower.followPath(pushBot_shoot0, true);
pathState = PathState.DRIVE_SHOOT0;
}
break;
case DRIVE_SHOOT0:
if (!follower.isBusy()){
timeStamp = currentTime;
pathState = PathState.WAIT_SHOOT0;
}
break;
case WAIT_SHOOT0:
if (currentTime - timeStamp > shootTime){
follower.followPath(shoot0_pickup1, intakePower, false);
pathState = PathState.PICKUP1;
}
break;
case PICKUP1:
if (!follower.isBusy()){
follower.followPath(pickup1_openGate, true);
pathState = PathState.OPENGATE;
shootX = shoot1X;
shootY = shoot1Y;
shootH = shoot1H;
}
break;
case DRIVE_OPENGATE:
if (!follower.isBusy()){
pathState = PathState.OPENGATE;
timeStamp = currentTime;
}
break;
case OPENGATE:
if (currentTime - timeStamp > openGateTime){
follower.followPath(openGate_shoot1, true);
pathState = PathState.DRIVE_SHOOT1;
}
break;
case DRIVE_SHOOT1:
if (!follower.isBusy()){
pathState = PathState.WAIT_SHOOT1;
timeStamp = currentTime;
}
break;
case WAIT_SHOOT1:
if (currentTime - timeStamp > shootTime){
follower.followPath(shoot1_drivePickup2, true);
pathState = PathState.DRIVE_PICKUP2;
}
break;
case DRIVE_PICKUP2:
if (!follower.isBusy()) {
follower.followPath(drivePickup2_pickup2, intakePower, false);
pathState = PathState.PICKUP2;
}
break;
case PICKUP2:
if (!follower.isBusy()){
follower.followPath(pickup2_shoot2, true);
pathState = PathState.DRIVE_SHOOT2;
}
shootX = shoot2X;
shootY = shoot2Y;
shootH = shoot2H;
break;
case DRIVE_SHOOT2:
if (!follower.isBusy()){
pathState = PathState.WAIT_SHOOT2;
timeStamp = currentTime;
}
break;
case WAIT_SHOOT2:
if (currentTime - timeStamp > shootTime){
follower.followPath(shoot2_drivePickup3, true);
pathState = PathState.DRIVE_PICKUP3;
}
break;
case DRIVE_PICKUP3:
if (!follower.isBusy()){
follower.followPath(drivePickup3_pickup3, intakePower, false);
pathState = PathState.PICKUP3;
}
break;
case PICKUP3:
if (!follower.isBusy()){
follower.followPath(pickup3_shoot3, true);
pathState = PathState.DRIVE_SHOOT3;
}
shootX = shoot3X;
shootY = shoot3Y;
shootH = shoot3H;
break;
case DRIVE_SHOOT3:
if (!follower.isBusy()){
pathState = PathState.WAIT_SHOOT3;
}
break;
case WAIT_SHOOT3:
// add line here to say "done auto'
break;
default:
break;
}
TELE.update(); // use for debugging
}
// Used for changing alliance
private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);}
private double adjustHeadingBasedOnAlliance(double heading){
heading = 180 - heading;
while (heading > 180) {heading-=360;}
while (heading <= -180) {heading+=360;}
return heading;
}
@Override
public void runOpMode() throws InterruptedException {
Robot.resetInstance();
robot = Robot.getInstance(hardwareMap);
List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
for (LynxModule hub : allHubs) {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
}
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
follower = Constants.createFollower(hardwareMap);
follower.setStartingPose(new Pose(72,72,0));
loopTimes = new MeasuringLoopTimes();
loopTimes.init();
boolean initializeRobot = false;
while (opModeInInit()){
follower.update();
if (gamepad1.crossWasPressed() && !initializeRobot){
Color.redAlliance = !Color.redAlliance;
double[] xPoses = {startPoseX, pushBotX, shoot0ControlX, shoot0X,
pickup1ControlX, pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X,
drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
drivePickup3X, pickup3X, shoot3ControlX, shoot3X};
double[] headings = {startPoseH, pushBotH, shoot0H,
pickup1H, openGateH, shoot1H,
drivePickup2H, pickup2H, shoot2H,
drivePickup3H, pickup3H, shoot3H};
for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);}
for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);}
}
if (gamepad1.triangleWasPressed()){
initializeRobot = true;
initializePoses();
follower.setPose(startPose);
buildPaths();
sleep(2000);
}
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.update();
}
waitForStart();
if (isStopRequested()) return;
limelightUsed = false;
while (opModeIsActive()){
follower.update();
pathStateMachine();
Pose currentPose = follower.getPose();
teleStartPoseX = currentPose.getX();
teleStartPoseY = currentPose.getY();
teleStartPoseH = Math.toDegrees(currentPose.getHeading());
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
}
loopTimes.loop();
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
TELE.addData("X:", currentPose.getX());
TELE.addData("Y:", currentPose.getY());
TELE.addData("H:", currentPose.getHeading());
TELE.update();
}
}
}

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

@@ -18,9 +18,12 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
public class Constants {
public static FollowerConstants followerConstants = new FollowerConstants()
.mass(14.37888)
.forwardZeroPowerAcceleration(-29.512)
.lateralZeroPowerAcceleration(-72.872)
.centripetalScaling(0);
.forwardZeroPowerAcceleration(-30.322)
.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))
.centripetalScaling(0.0005);
public static MecanumConstants driveConstants = new MecanumConstants()
.maxPower(1)
@@ -32,15 +35,15 @@ public class Constants {
.leftRearMotorDirection(DcMotorSimple.Direction.REVERSE)
.rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD)
.rightRearMotorDirection(DcMotorSimple.Direction.FORWARD)
.xVelocity(64.675)
.yVelocity(49.583);
.xVelocity(84.376)
.yVelocity(64.052);
public static double breakingStrength = 1;
public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, breakingStrength, 1);
public static PinpointConstants localizerConstants = new PinpointConstants()
.forwardPodY(-3.676)
.strafePodX(3.780)
.forwardPodY(3.7795)
.strafePodX(-3.676)
.distanceUnit(DistanceUnit.INCH)
.hardwareMapName("pinpoint")
.encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD)

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
View File

@@ -416,7 +416,7 @@ class ForwardVelocityTuner extends OpMode {
if (!end) {
if (Math.abs(follower.getPose().getX()) > (DISTANCE + 72)) {
if (Math.abs(follower.getPose().getX()) > (DISTANCE)) {
end = true;
stopRobot();
} else {
@@ -524,7 +524,7 @@ class LateralVelocityTuner extends OpMode {
drawCurrentAndHistory();
if (!end) {
if (Math.abs(follower.getPose().getY()) > (DISTANCE + 72)) {
if (Math.abs(follower.getPose().getY()) > (DISTANCE)) {
end = true;
stopRobot();
} else {

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java
View File

@@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.tests;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -8,7 +9,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utilsv2.Robot;
@Config
@TeleOp
@@ -50,8 +51,9 @@ public class Hardware_Tester extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry();
Robot.resetInstance();
robot = Robot.getInstance(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
@@ -122,7 +124,8 @@ public class Hardware_Tester extends LinearOpMode {
// Sensor Data
// TELE.addData("Beam Break 1?", robot.beam1.isPressed());
TELE.addData("Beam Break insideBeam?", robot.insideBeam.isPressed());
TELE.addData("Beam Break outsideBeam?", robot.outsideBeam.isPressed());
// TELE.addData("Beam Break 2?", robot.beam2.isPressed());
// TELE.addData("Beam Break 3?", robot.beam3.isPressed());

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

@@ -20,16 +20,20 @@ public class NewShooterTest extends LinearOpMode {
Robot robot;
Flywheel flywheel;
Turret turret;
MultipleTelemetry TELE;
public static boolean intake = true;
public static boolean shoot = false;
public static double intakePower = 1.0;
public static double transferShootPower = -1;
public static double transferIntakePower = -1.0;
public static double transferIntakePower = -1;
public static double turretPos = 0.51;
public static double hoodPos = 0.51;
public static double flywheel_velo = 0;
public static double shooterP = 255, shooterI = 0, shooterD = 0, shooterF = 75;
private enum ShootState {
IDLE,
WAIT_GATE,
@@ -46,15 +50,15 @@ public class NewShooterTest extends LinearOpMode {
robot = Robot.getInstance(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
flywheel = new Flywheel(robot);
turret = new Turret(robot);
Shooter shooter = new Shooter(
robot,
new MultipleTelemetry(
telemetry,
FtcDashboard.getInstance().getTelemetry()
),
TELE,
Constants.createFollower(hardwareMap),
true,
turret,
@@ -72,6 +76,9 @@ public class NewShooterTest extends LinearOpMode {
robot.setHoodPos(hoodPos);
shooter.setTurretPosition(turretPos);
shooter.setFlywheelVelocity(flywheel_velo);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage);
robot.setSpinPos(ServoPositions.spindexer_A2);
if (intake && !shoot) {
@@ -85,9 +92,7 @@ public class NewShooterTest extends LinearOpMode {
robot.setIntakePower(intakePower);
robot.setTransferServoPos(
ServoPositions.transferServo_out);
}
else if (shoot) {
} else if (shoot) {
robot.setIntakePower(intakePower);
@@ -129,6 +134,13 @@ public class NewShooterTest extends LinearOpMode {
}
}
TELE.addData("Flywheel Velocity1", (robot.shooter1.getVelocity() * 60) / 28);
TELE.addData("Flywheel Velocity2", (robot.shooter2.getVelocity() * 60) / 28);
TELE.addData("Flywheel Averag Velocity", flywheel.getAverageVelocity());
TELE.addData("PIDF Coefficients", Flywheel.shooterPIDF);
TELE.addData("Power", flywheel.getShooterPower());
TELE.update();
shooter.update();
}
}

47
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java
View File

@@ -1,8 +1,8 @@
package org.firstinspires.ftc.teamcode.utilsv2;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import org.firstinspires.ftc.teamcode.utilsv2.Robot;
@@ -12,7 +12,12 @@ import java.util.LinkedList;
public class Flywheel {
Robot robot;
public PIDFCoefficients shooterPIDF1, shooterPIDF2;
// public PIDFCoefficients shooterPIDF1, shooterPIDF2;
public static PIDFCoefficients shooterPIDF;
public static double shooterPIDF_P = 255;
public static double shooterPIDF_I = 0.0;
public static double shooterPIDF_D = 0.0;
public static double shooterPIDF_F = 75;
private double velo = 0.0;
private double velo1 = 0.0;
@@ -27,10 +32,8 @@ public class Flywheel {
private final LinkedList<Double> velocityHistory = new LinkedList<>();
public Flywheel(Robot rob) {
robot = rob;
shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
shooterPIDF2 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12);
}
public double getVelo() {
@@ -54,28 +57,24 @@ public class Flywheel {
}
// Set the robot PIDF for the next cycle.
private double prevF = 0;
public static double voltagePIDFDifference = 0.8;
public void setPIDF(double p, double i, double d, double f) {
shooterPIDF1.p = p;
shooterPIDF1.i = i;
shooterPIDF1.d = d;
shooterPIDF1.f = f;
shooterPIDF.p = p;
shooterPIDF.i = i;
shooterPIDF.d = d;
shooterPIDF.f = f;
shooterPIDF2.p = p;
shooterPIDF2.i = i;
shooterPIDF2.d = d;
shooterPIDF2.f = f;
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
}
private double prevF = 0;
public static double voltagePIDFDifference = 0.8;
public void setF(double f){
if (Math.abs(prevF - f) > voltagePIDFDifference) {
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
shooterPIDF.f = f;
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
prevF = f;
}
}
@@ -108,6 +107,7 @@ public class Flywheel {
averageVelocity = sum / velocityHistory.size();
}
double power;
public void manageFlywheel(double commandedVelocity) {
if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
@@ -115,8 +115,8 @@ public class Flywheel {
}
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
power = robot.shooter1.getPower();
robot.shooter2.setPower(power);
velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
@@ -128,4 +128,5 @@ public class Flywheel {
steady = (Math.abs(commandedVelocity - averageVelocity) < 50);
}
public double getShooterPower(){return power;}
}

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

@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.utilsv2;
import android.view.View;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.robotcore.hardware.AnalogInput;
@@ -19,6 +21,7 @@ public class Robot {
// Singleton instance
private static Robot instance;
/**
* Returns the existing Robot instance or creates one if it doesn't exist.
*/
@@ -45,11 +48,6 @@ public class Robot {
public DcMotorEx backRight;
public DcMotorEx intake;
public DcMotorEx transfer;
public PIDFCoefficients shooterPIDF;
public static double shooterPIDF_P = 255;
public static double shooterPIDF_I = 0.0;
public static double shooterPIDF_D = 0.0;
public static double shooterPIDF_F = 75;
// public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
public DcMotorEx shooter1;
public DcMotorEx shooter2;
@@ -65,7 +63,6 @@ public class Robot {
public Servo spin2;
public TouchSensor insideBeam;
public TouchSensor outsideBeam;
public RevColorSensorV3 revSensor;
public VoltageSensor voltage;
@@ -106,13 +103,10 @@ public class Robot {
shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F);
// shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12);
shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
shooter1.setVelocity(0);
shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
shooter2.setVelocity(0);
// shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
hood = hardwareMap.get(Servo.class, "hood");