diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java index 1fa8480..48a3ba8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java @@ -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 allHubs = hardwareMap.getAll(LynxModule.class); for (LynxModule hub : allHubs) { hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java index b9ad6c6..3af8eae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java @@ -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 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(); - } - } -} \ No newline at end of file +// @Override +// public void runOpMode() throws InterruptedException { +// Robot.resetInstance(); +// robot = Robot.getInstance(hardwareMap); +// List 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(); +// } +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java index bfaba30..2f95d38 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java @@ -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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java index fafc88c..97d58f1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java @@ -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(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java index a5cfada..030f018 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java @@ -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 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()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java index 224cdca..da7bed3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java @@ -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");