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

readjusted shooter test

Browse Source 
@KeshavAnandCode please merge
This commit is contained in:
DanTheMan-byte
2026-06-02 19:14:58 -05:00
parent ccc6a608fc
commit 7665957c7a
6 changed files with 376 additions and 371 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -22,13 +22,13 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants; 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.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.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer; import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting; 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; import java.util.List;
@@ -286,7 +286,8 @@ public class Auto12BallPedroPathing extends LinearOpMode {
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap); Robot.resetInstance();
robot = Robot.getInstance(hardwareMap);
List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class); List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
for (LynxModule hub : allHubs) { for (LynxModule hub : allHubs) {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);

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

@@ -1,340 +1,341 @@
package org.firstinspires.ftc.teamcode.autonomous; //package org.firstinspires.ftc.teamcode.autonomous;
//
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; //import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; //import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; //import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; //import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; //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.teleStartPoseY;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; //import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
//
import com.acmerobotics.dashboard.FtcDashboard; //import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; //import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; //import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.pedropathing.follower.Follower; //import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.BezierCurve; //import com.pedropathing.geometry.BezierCurve;
import com.pedropathing.geometry.BezierLine; //import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose; //import com.pedropathing.geometry.Pose;
import com.pedropathing.paths.PathChain; //import com.pedropathing.paths.PathChain;
import com.qualcomm.hardware.lynx.LynxModule; //import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; //import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; //import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
//
import org.firstinspires.ftc.teamcode.constants.Color; //import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants; //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.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.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer; //import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting; //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; //import java.util.List;
//
@Config //@Config
@Autonomous (preselectTeleOp = "TeleopV4") //@Autonomous (preselectTeleOp = "TeleopV4")
public class Auto12Ball_Back_Sorted extends LinearOpMode { //public class Auto12Ball_Back_Sorted extends LinearOpMode {
Robot robot; // Robot robot;
MultipleTelemetry TELE; // MultipleTelemetry TELE;
// Flywheel flywheel; // // Flywheel flywheel;
// Targeting targeting; //// Targeting targeting;
// Targeting.Settings targetingSettings; //// Targeting.Settings targetingSettings;
Follower follower; // Follower follower;
// Turret turret; // // Turret turret;
// Spindexer spindexer; //// Spindexer spindexer;
// Servos servos; //// Servos servos;
MeasuringLoopTimes loopTimes; // MeasuringLoopTimes loopTimes;
//
// Wait Times // // Wait Times
public static double shootTime = 2; // public static double shootTime = 2;
//
// Extra Variables // // Extra Variables
public static double intakePower = 0.3; // public static double intakePower = 0.3;
double shootX, shootY, shootH; // double shootX, shootY, shootH;
//
// Initialize path state machine // // Initialize path state machine
private enum PathState { // private enum PathState {
PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0, // PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0,
PICKUP1, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1, // PICKUP1, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1,
DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2, // DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2,
DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3 // DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3
} // }
PathState pathState = PathState.DRIVE_SHOOT0; // PathState pathState = PathState.DRIVE_SHOOT0;
//
// Poses // // Poses
public static double startPoseX = 84, startPoseY = 7, startPoseH = 90; // public static double startPoseX = 84, startPoseY = 7, startPoseH = 90;
public static double pushBotX = 94, pushBotY = 9, pushBotH = 100; // public static double pushBotX = 94, pushBotY = 9, pushBotH = 100;
public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0; // public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0;
public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0; // public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0;
public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165; // public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165;
public static double openGateX = 129, openGateY = 74, openGateH = 0; // public static double openGateX = 129, openGateY = 74, openGateH = 0;
public static double shoot1ControlX = 112, shoot1ControlY = 75; // public static double shoot1ControlX = 112, shoot1ControlY = 75;
public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12; // public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12;
public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0; // public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0;
public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0; // public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0;
public static double shoot2ControlX = 102, shoot2ControlY = 63; // public static double shoot2ControlX = 102, shoot2ControlY = 63;
public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50; // public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50;
public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0; // public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0;
public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0; // public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0;
public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446; // public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446;
public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80; // public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80;
Pose startPose, pushBot, shoot0, // Pose startPose, pushBot, shoot0,
pickup1, openGateControl, openGate, shoot1Control, shoot1, // pickup1, openGateControl, openGate, shoot1Control, shoot1,
drivePickup2, pickup2, shoot2Control, shoot2, // drivePickup2, pickup2, shoot2Control, shoot2,
drivePickup3, pickup3, shoot3Control, shoot3; // drivePickup3, pickup3, shoot3Control, shoot3;
private void initializePoses(){ // private void initializePoses(){
startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH)); // startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH));
pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH)); // pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH));
shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H)); // shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H));
pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H)); // pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H));
openGateControl = new Pose(openGateControlX, openGateControlY); // openGateControl = new Pose(openGateControlX, openGateControlY);
openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH)); // openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH));
shoot1Control = new Pose(shoot1ControlX, shoot1ControlY); // shoot1Control = new Pose(shoot1ControlX, shoot1ControlY);
shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H)); // shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H));
drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H)); // drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H));
pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H)); // pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H));
shoot2Control = new Pose(shoot2ControlX, shoot2ControlY); // shoot2Control = new Pose(shoot2ControlX, shoot2ControlY);
shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H)); // shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H));
drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H)); // drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H));
pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H)); // pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H));
shoot3Control = new Pose(shoot3ControlX, shoot3ControlY); // shoot3Control = new Pose(shoot3ControlX, shoot3ControlY);
shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H)); // shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H));
} // }
//
//Building Paths // //Building Paths
PathChain startPose_pushBot, pushBot_shoot0, // PathChain startPose_pushBot, pushBot_shoot0,
shoot0_pickup1, pickup1_openGate, openGate_shoot1, // shoot0_pickup1, pickup1_openGate, openGate_shoot1,
shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2, // shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2,
shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3; // shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3;
private void buildPaths(){ // private void buildPaths(){
startPose_shoot0 = follower.pathBuilder() // startPose_pushBot = follower.pathBuilder()
.addPath(new BezierLine(startPose, shoot0)) // .addPath(new BezierLine(startPose, pushBot))
.setLinearHeadingInterpolation(startPose.getHeading(), shoot0.getHeading()) // .setLinearHeadingInterpolation(startPose.getHeading(), pushBot.getHeading())
.build(); // .build();
} // }
//
//Path State Machine // //Path State Machine
private boolean startAuto = true; // private boolean startAuto = true;
private double timeStamp = 0; // private double timeStamp = 0;
private void pathStateMachine(){ // private void pathStateMachine(){
double currentTime = (double) System.currentTimeMillis() / 1000; // double currentTime = (double) System.currentTimeMillis() / 1000;
switch(pathState){ // switch(pathState){
case DRIVE_SHOOT0: // case DRIVE_SHOOT0:
if (startAuto){ // if (startAuto){
follower.followPath(startPose_shoot0, true); // follower.followPath(startPose_shoot0, true);
startAuto = false; // startAuto = false;
shootX = shoot0X; // shootX = shoot0X;
shootY = shoot0Y; // shootY = shoot0Y;
shootH = shoot0H; // shootH = shoot0H;
}
driveShoot(PathState.WAIT_SHOOT0, currentTime);
break;
case WAIT_SHOOT0:
waitShoot(PathState.DRIVE_PICKUP1, shoot0_drivePickup1, currentTime);
break;
case DRIVE_PICKUP1:
drivePickup(PathState.PICKUP1, drivePickup1_pickup1);
break;
case PICKUP1:
pickup(PathState.DRIVE_SHOOT1, pickup1_shoot1);
shootX = shoot1X;
shootY = shoot1Y;
shootH = shoot1H;
break;
case DRIVE_SHOOT1:
intakePowerDown(timeStamp, currentTime);
driveShoot(PathState.WAIT_SHOOT1, currentTime);
break;
case WAIT_SHOOT1:
waitShoot(PathState.DRIVE_PICKUP2, shoot1_drivePickup2, currentTime);
break;
case DRIVE_PICKUP2:
drivePickup(PathState.PICKUP2, drivePickup2_pickup2);
break;
case PICKUP2:
pickup(PathState.DRIVE_SHOOT2, pickup2_shoot2);
shootX = shoot2X;
shootY = shoot2Y;
shootH = shoot2H;
break;
case DRIVE_SHOOT2:
intakePowerDown(timeStamp, currentTime);
driveShoot(PathState.WAIT_SHOOT2, currentTime);
break;
case WAIT_SHOOT2:
waitShoot(PathState.DRIVE_PICKUP3, shoot2_drivePickup3, currentTime);
break;
case DRIVE_PICKUP3:
drivePickup(PathState.PICKUP3, drivePickup3_pickup3);
break;
case PICKUP3:
pickup(PathState.DRIVE_SHOOT3, pickup3_shoot3);
shootX = shoot3X;
shootY = shoot3Y;
shootH = shoot3H;
break;
case DRIVE_SHOOT3:
intakePowerDown(timeStamp, currentTime);
driveShoot(PathState.WAIT_SHOOT3, currentTime);
break;
case WAIT_SHOOT3:
// if (spindexer.shootAllComplete()){
// spindexer.resetSpindexer();
// TELE.addLine("Done Auto");
// } // }
break; // driveShoot(PathState.WAIT_SHOOT0, currentTime);
default: // break;
break; // case WAIT_SHOOT0:
} // waitShoot(PathState.DRIVE_PICKUP1, shoot0_drivePickup1, currentTime);
TELE.update(); // use for debugging // break;
} // case DRIVE_PICKUP1:
// drivePickup(PathState.PICKUP1, drivePickup1_pickup1);
// Voids for State Machine // break;
private void intakePowerDown(double stamp, double currentTime) { // case PICKUP1:
double pow = (stamp - currentTime) / 2; // adjust denominator to see how much time to adjust // pickup(PathState.DRIVE_SHOOT1, pickup1_shoot1);
if (pow < -1) {pow = 0;} // shootX = shoot1X;
// spindexer.setIntakePower(pow); // shootY = shoot1Y;
} // shootH = shoot1H;
private void driveShoot(PathState nextState, double currentTime){ // break;
if (!follower.isBusy()){ // case DRIVE_SHOOT1:
pathState = nextState; // intakePowerDown(timeStamp, currentTime);
timeStamp = currentTime; // driveShoot(PathState.WAIT_SHOOT1, currentTime);
// spindexer.prepareShootAllContinous(); // break;
} // case WAIT_SHOOT1:
} // waitShoot(PathState.DRIVE_PICKUP2, shoot1_drivePickup2, currentTime);
private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) { // break;
if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() || // case DRIVE_PICKUP2:
// spindexer.resetSpindexer(); // drivePickup(PathState.PICKUP2, drivePickup2_pickup2);
pathState = nextState; // break;
follower.followPath(nextPath, true); // case PICKUP2:
// spindexer.setIntakePower(1); // pickup(PathState.DRIVE_SHOOT2, pickup2_shoot2);
} // shootX = shoot2X;
} // shootY = shoot2Y;
private void drivePickup(PathState nextState, PathChain nextPath) { // shootH = shoot2H;
if (!follower.isBusy()) { // break;
pathState = nextState; // case DRIVE_SHOOT2:
follower.followPath(nextPath, intakePower, false); // intakePowerDown(timeStamp, currentTime);
} // driveShoot(PathState.WAIT_SHOOT2, currentTime);
} // break;
private void pickup(PathState nextState, PathChain nextPath) { // case WAIT_SHOOT2:
if (!follower.isBusy()) { // waitShoot(PathState.DRIVE_PICKUP3, shoot2_drivePickup3, currentTime);
pathState = nextState; // break;
follower.followPath(nextPath, true); // case DRIVE_PICKUP3:
} // drivePickup(PathState.PICKUP3, drivePickup3_pickup3);
} // break;
// case PICKUP3:
// Used for changing alliance // pickup(PathState.DRIVE_SHOOT3, pickup3_shoot3);
private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);} // shootX = shoot3X;
private double adjustHeadingBasedOnAlliance(double heading){ // shootY = shoot3Y;
heading = 180 - heading; // shootH = shoot3H;
while (heading > 180) {heading-=360;} // break;
while (heading <= -180) {heading+=360;} // case DRIVE_SHOOT3:
return heading; // intakePowerDown(timeStamp, currentTime);
} // driveShoot(PathState.WAIT_SHOOT3, currentTime);
// break;
@Override // case WAIT_SHOOT3:
public void runOpMode() throws InterruptedException { //// if (spindexer.shootAllComplete()){
robot = new Robot(hardwareMap); //// spindexer.resetSpindexer();
List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class); //// TELE.addLine("Done Auto");
for (LynxModule hub : allHubs) { //// }
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); // break;
} // default:
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); // break;
// flywheel = new Flywheel(hardwareMap); // }
// targeting = new Targeting(); // TELE.update(); // use for debugging
// 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);
loopTimes = new MeasuringLoopTimes();
loopTimes.init();
// robot.light.setPosition(Color.LightRed);
boolean initializeRobot = false;
while (opModeInInit()){
follower.update();
if (gamepad1.crossWasPressed() && !initializeRobot){
Color.redAlliance = !Color.redAlliance;
// if (Color.redAlliance){
// robot.light.setPosition(Color.LightRed);
// } else {
// robot.light.setPosition(Color.LightBlue);
// }
double[] xPoses = {startPoseX, shoot0X,
drivePickup1X, pickup1X, shoot1X,
drivePickup2ControlX, drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
drivePickup3ControlX, drivePickup3X, pickup3X, shoot3ControlX, shoot3X};
double[] headings = {startPoseH, shoot0H,
drivePickup1H, pickup1H, 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);
// turret.setTurret(turrDefault);
// servos.setSpinPos(spinStartPos);
}
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;
// 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))); // // Voids for State Machine
// targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate); // 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);
// }
// private void driveShoot(PathState nextState, double currentTime){
// if (!follower.isBusy()){
// pathState = nextState;
// timeStamp = currentTime;
//// spindexer.prepareShootAllContinous();
// }
// }
// private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) {
// if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() ||
//// spindexer.resetSpindexer();
// pathState = nextState;
// follower.followPath(nextPath, true);
//// spindexer.setIntakePower(1);
// }
// }
// private void drivePickup(PathState nextState, PathChain nextPath) {
// if (!follower.isBusy()) {
// pathState = nextState;
// follower.followPath(nextPath, intakePower, false);
// }
// }
// private void pickup(PathState nextState, PathChain nextPath) {
// if (!follower.isBusy()) {
// pathState = nextState;
// follower.followPath(nextPath, true);
// }
// }
// //
// double voltage = robot.voltage.getVoltage(); // // Used for changing alliance
// flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); // private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);}
// flywheel.manageFlywheel(targetingSettings.flywheelRPM); // private double adjustHeadingBasedOnAlliance(double heading){
// servos.setHoodPos(targetingSettings.hoodAngle); // heading = 180 - heading;
// while (heading > 180) {heading-=360;}
// while (heading <= -180) {heading+=360;}
// return heading;
// }
// //
// if (driveToShoot()){servos.setSpinPos(spinStartPos);} // @Override
// else {spindexer.processIntake();} // public void runOpMode() throws InterruptedException {
// Robot.resetInstance();
for (LynxModule hub : allHubs) { // robot = Robot.getInstance(hardwareMap);
hub.clearBulkCache(); // List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
} // for (LynxModule hub : allHubs) {
loopTimes.loop(); // hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
// }
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); // TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); //// flywheel = new Flywheel(hardwareMap);
TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); //// targeting = new Targeting();
TELE.addData("X:", currentPose.getX()); //// targetingSettings = new Targeting.Settings(0,0);
TELE.addData("Y:", currentPose.getY()); // follower = Constants.createFollower(hardwareMap);
TELE.addData("H:", currentPose.getHeading()); // follower.setStartingPose(new Pose(72,72,0));
TELE.update(); //// 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);
//
// boolean initializeRobot = false;
// while (opModeInInit()){
// follower.update();
//
// if (gamepad1.crossWasPressed() && !initializeRobot){
// Color.redAlliance = !Color.redAlliance;
//// if (Color.redAlliance){
//// robot.light.setPosition(Color.LightRed);
//// } else {
//// robot.light.setPosition(Color.LightBlue);
//// }
//
//// double[] xPoses = {startPoseX, shoot0X,
//// drivePickup1X, pickup1X, shoot1X,
//// drivePickup2ControlX, drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
//// drivePickup3ControlX, drivePickup3X, pickup3X, shoot3ControlX, shoot3X};
////
//// double[] headings = {startPoseH, shoot0H,
//// drivePickup1H, pickup1H, 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);
//
//// turret.setTurret(turrDefault);
//// servos.setSpinPos(spinStartPos);
// }
//
// 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;
//
//// 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();}
//
// 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();
// }
// }
//}

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

@@ -9,7 +9,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.NormalizedRGBA; import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utilsv2.Robot;
@Config @Config
@TeleOp @TeleOp
@@ -51,7 +51,8 @@ public class Hardware_Tester extends LinearOpMode {
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap); Robot.resetInstance();
robot = Robot.getInstance(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

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

@@ -8,6 +8,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
import org.firstinspires.ftc.teamcode.utilsv2.Robot; import org.firstinspires.ftc.teamcode.utilsv2.Robot;
import org.firstinspires.ftc.teamcode.utilsv2.Shooter; import org.firstinspires.ftc.teamcode.utilsv2.Shooter;
@@ -17,6 +18,10 @@ public class NewShooterTest extends LinearOpMode {
Robot robot; Robot robot;
MultipleTelemetry TELE;
Flywheel rpmFlywheel;
public static boolean intake = true; public static boolean intake = true;
public static boolean shoot = false; public static boolean shoot = false;
public static double intakePower = 1.0; public static double intakePower = 1.0;
@@ -26,6 +31,8 @@ public class NewShooterTest extends LinearOpMode {
public static double hoodPos = 0.51; public static double hoodPos = 0.51;
public static double flywheel = 0; public static double flywheel = 0;
public static double shooterP = 255, shooterI = 0, shooterD = 0, shooterF = 75;
private enum ShootState { private enum ShootState {
IDLE, IDLE,
WAIT_GATE, WAIT_GATE,
@@ -38,10 +45,10 @@ public class NewShooterTest extends LinearOpMode {
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
Robot.resetInstance();
robot = Robot.getInstance(hardwareMap); robot = Robot.getInstance(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
Shooter shooter = new Shooter( Shooter shooter = new Shooter(
robot, robot,
new MultipleTelemetry( new MultipleTelemetry(
@@ -52,6 +59,8 @@ public class NewShooterTest extends LinearOpMode {
true true
); );
rpmFlywheel = new Flywheel(robot);
shooter.setState(Shooter.ShooterState.MANUAL); shooter.setState(Shooter.ShooterState.MANUAL);
waitForStart(); waitForStart();
@@ -63,6 +72,9 @@ public class NewShooterTest extends LinearOpMode {
robot.setHoodPos(hoodPos); robot.setHoodPos(hoodPos);
shooter.setTurretPosition(turretPos); shooter.setTurretPosition(turretPos);
shooter.setFlywheelVelocity(flywheel); shooter.setFlywheelVelocity(flywheel);
double voltage = robot.voltage.getVoltage();
rpmFlywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage);
robot.setSpinPos(ServoPositions.spindexer_A2); robot.setSpinPos(ServoPositions.spindexer_A2);
if (intake && !shoot) { if (intake && !shoot) {
@@ -76,9 +88,7 @@ public class NewShooterTest extends LinearOpMode {
robot.setIntakePower(intakePower); robot.setIntakePower(intakePower);
robot.setTransferServoPos( robot.setTransferServoPos(
ServoPositions.transferServo_out); ServoPositions.transferServo_out);
} } else if (shoot) {
else if (shoot) {
robot.setIntakePower(intakePower); robot.setIntakePower(intakePower);
@@ -120,6 +130,9 @@ public class NewShooterTest extends LinearOpMode {
} }
} }
TELE.addData("Flywheel Velocity", (robot.shooter1.getVelocity() * 60) / 28);
TELE.update();
shooter.update(); shooter.update();
} }
} }

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

@@ -1,12 +1,10 @@
package org.firstinspires.ftc.teamcode.utilsv2; package org.firstinspires.ftc.teamcode.utilsv2;
import com.qualcomm.robotcore.hardware.DcMotor; 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 com.qualcomm.robotcore.hardware.PIDFCoefficients;
import org.firstinspires.ftc.teamcode.utilsv2.Robot;
import java.util.LinkedList; import java.util.LinkedList;
public class Flywheel { public class Flywheel {
@@ -27,10 +25,8 @@ public class Flywheel {
private final LinkedList<Double> velocityHistory = new LinkedList<>(); private final LinkedList<Double> velocityHistory = new LinkedList<>();
public Flywheel(Robot rob) { public Flywheel(Robot rob) {
robot = rob; robot = rob;
shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F); shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / 12);
shooterPIDF2 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
} }
public double getVelo() { public double getVelo() {
@@ -65,19 +61,14 @@ public class Flywheel {
shooterPIDF1.d = d; shooterPIDF1.d = d;
shooterPIDF1.f = f; shooterPIDF1.f = f;
shooterPIDF2.p = p; robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
shooterPIDF2.i = i;
shooterPIDF2.d = d;
shooterPIDF2.f = f;
if (Math.abs(prevF - f) > voltagePIDFDifference) { // if (Math.abs(prevF - f) > voltagePIDFDifference) {
//
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); // robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
//
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); // prevF = f;
// }
prevF = f;
}
} }
// Convert from RPM to Ticks per Second // Convert from RPM to Ticks per Second
@@ -115,8 +106,8 @@ public class Flywheel {
} }
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
double power = robot.shooter1.getPower();
robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); robot.shooter2.setPower(power);
velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); velo1 = TPS_to_RPM(robot.shooter1.getVelocity());

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

@@ -106,13 +106,11 @@ public class Robot {
shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
shooter1.setDirection(DcMotorSimple.Direction.REVERSE); 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.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
shooter1.setVelocity(0); shooter1.setVelocity(0);
shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
shooter2.setVelocity(0);
hood = hardwareMap.get(Servo.class, "hood"); hood = hardwareMap.get(Servo.class, "hood");