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.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;
@@ -286,7 +286,8 @@ 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);

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;
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.Turret.limelightUsed;
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.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.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
import org.firstinspires.ftc.teamcode.utils.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 java.util.List;
@Config
@Autonomous (preselectTeleOp = "TeleopV4")
public class Auto12Ball_Back_Sorted extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
// Flywheel flywheel;
// Targeting targeting;
// Targeting.Settings targetingSettings;
Follower follower;
// Turret turret;
// Spindexer spindexer;
// Servos servos;
MeasuringLoopTimes loopTimes;
// Wait Times
public static double shootTime = 2;
// 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, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1,
DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2,
DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3
}
PathState pathState = PathState.DRIVE_SHOOT0;
// Poses
public static double startPoseX = 84, startPoseY = 7, startPoseH = 90;
public static double pushBotX = 94, pushBotY = 9, pushBotH = 100;
public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0;
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, shoot0,
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));
shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H));
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_shoot0 = follower.pathBuilder()
.addPath(new BezierLine(startPose, shoot0))
.setLinearHeadingInterpolation(startPose.getHeading(), shoot0.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 DRIVE_SHOOT0:
if (startAuto){
follower.followPath(startPose_shoot0, true);
startAuto = false;
shootX = shoot0X;
shootY = shoot0Y;
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");
//package org.firstinspires.ftc.teamcode.autonomous;
//
//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.Turret.limelightUsed;
//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.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.utils.Servos;
//import org.firstinspires.ftc.teamcode.utils.Spindexer;
//import org.firstinspires.ftc.teamcode.utils.Targeting;
//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;
// // Flywheel flywheel;
//// Targeting targeting;
//// Targeting.Settings targetingSettings;
// Follower follower;
// // Turret turret;
//// Spindexer spindexer;
//// Servos servos;
// MeasuringLoopTimes loopTimes;
//
// // Wait Times
// public static double shootTime = 2;
//
// // 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, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1,
// DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2,
// DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3
// }
// PathState pathState = PathState.DRIVE_SHOOT0;
//
// // Poses
// public static double startPoseX = 84, startPoseY = 7, startPoseH = 90;
// public static double pushBotX = 94, pushBotY = 9, pushBotH = 100;
// public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0;
// 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, shoot0,
// 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));
// shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H));
// 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();
// }
//
// //Path State Machine
// private boolean startAuto = true;
// private double timeStamp = 0;
// private void pathStateMachine(){
// double currentTime = (double) System.currentTimeMillis() / 1000;
// switch(pathState){
// case DRIVE_SHOOT0:
// if (startAuto){
// follower.followPath(startPose_shoot0, true);
// startAuto = false;
// shootX = shoot0X;
// shootY = shoot0Y;
// shootH = shoot0H;
// }
break;
default:
break;
}
TELE.update(); // use for debugging
}
// Voids for State Machine
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);
}
}
// 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 = new Robot(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);
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());
// 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;
// default:
// break;
// }
// TELE.update(); // use for debugging
// }
//
// turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH)));
// targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate);
// // Voids for State Machine
// 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();
// flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
// flywheel.manageFlywheel(targetingSettings.flywheelRPM);
// servos.setHoodPos(targetingSettings.hoodAngle);
// // 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;
// }
//
// 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();
}
}
}
// @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());
//// 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);
// 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 org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utilsv2.Robot;
@Config
@TeleOp
@@ -51,7 +51,8 @@ public class Hardware_Tester extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
Robot.resetInstance();
robot = Robot.getInstance(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
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.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
import org.firstinspires.ftc.teamcode.utilsv2.Robot;
import org.firstinspires.ftc.teamcode.utilsv2.Shooter;
@@ -17,6 +18,10 @@ public class NewShooterTest extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
Flywheel rpmFlywheel;
public static boolean intake = true;
public static boolean shoot = false;
public static double intakePower = 1.0;
@@ -26,6 +31,8 @@ public class NewShooterTest extends LinearOpMode {
public static double hoodPos = 0.51;
public static double flywheel = 0;
public static double shooterP = 255, shooterI = 0, shooterD = 0, shooterF = 75;
private enum ShootState {
IDLE,
WAIT_GATE,
@@ -38,10 +45,10 @@ public class NewShooterTest extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
Robot.resetInstance();
robot = Robot.getInstance(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
Shooter shooter = new Shooter(
robot,
new MultipleTelemetry(
@@ -52,6 +59,8 @@ public class NewShooterTest extends LinearOpMode {
true
);
rpmFlywheel = new Flywheel(robot);
shooter.setState(Shooter.ShooterState.MANUAL);
waitForStart();
@@ -63,6 +72,9 @@ public class NewShooterTest extends LinearOpMode {
robot.setHoodPos(hoodPos);
shooter.setTurretPosition(turretPos);
shooter.setFlywheelVelocity(flywheel);
double voltage = robot.voltage.getVoltage();
rpmFlywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage);
robot.setSpinPos(ServoPositions.spindexer_A2);
if (intake && !shoot) {
@@ -76,9 +88,7 @@ public class NewShooterTest extends LinearOpMode {
robot.setIntakePower(intakePower);
robot.setTransferServoPos(
ServoPositions.transferServo_out);
}
else if (shoot) {
} else if (shoot) {
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();
}
}

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

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