diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 6ce65a0..1830322 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -33,6 +33,7 @@ import org.firstinspires.ftc.teamcode.utils.Flywheel; 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 java.util.ArrayList; import java.util.List; @@ -53,6 +54,8 @@ public class TeleopV3 extends LinearOpMode { public static boolean manualTurret = true; public double vel = 3000; public boolean autoVel = true; + public boolean targetingVel = true; + public boolean targetingHood = true; public double manualOffset = 0.0; public boolean autoHood = true; public boolean green1 = false; @@ -72,6 +75,8 @@ public class TeleopV3 extends LinearOpMode { Flywheel flywheel; MecanumDrive drive; Spindexer spindexer; + Targeting targeting; + Targeting.Settings targetingSettings; double autoHoodOffset = 0.0; int shooterTicker = 0; @@ -146,6 +151,8 @@ public class TeleopV3 extends LinearOpMode { flywheel = new Flywheel(hardwareMap); drive = new MecanumDrive(hardwareMap, teleStart); spindexer = new Spindexer(hardwareMap); + targeting = new Targeting(); + targetingSettings = new Targeting.Settings(0.0,0.0); PIDFController tController = new PIDFController(tp, ti, td, tf); @@ -402,13 +409,14 @@ public class TeleopV3 extends LinearOpMode { pos = 0.83; } - //SHOOTER: - flywheel.manageFlywheel(vel); + targetingSettings = targeting.calculateSettings + (robotX,robotY,robotHeading,0.0); //VELOCITY AUTOMATIC - - if (autoVel) { + if (targetingVel) { + vel = targetingSettings.flywheelRPM; + } else if (autoVel) { vel = velPrediction(distanceToGoal); } else { vel = manualVel; @@ -430,6 +438,9 @@ public class TeleopV3 extends LinearOpMode { manualVel = 3100; } + //SHOOTER: + flywheel.manageFlywheel(vel); + //TODO: test the camera teleop code TELE.addData("posS2", pos); @@ -480,7 +491,9 @@ public class TeleopV3 extends LinearOpMode { //HOOD: - if (autoHood) { + if (targetingHood) { + robot.hood.setPosition(targetingSettings.hoodAngle); + } else if (autoHood) { robot.hood.setPosition(0.15 + hoodOffset); } else { robot.hood.setPosition(hoodDefaultPos + hoodOffset); @@ -845,16 +858,27 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("shootOrder", shootOrder); TELE.addData("oddColor", oddBallColor); + // Spindexer Debug TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1)); TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition); TELE.addData("spinIntakeState", spindexer.currentIntakeState); TELE.addData("spinTestCounter", spindexer.counter); TELE.addData("autoSpintake", autoSpintake); - TELE.addData("distanceRearCenter", spindexer.distanceRearCenter); - TELE.addData("distanceFrontDriver", spindexer.distanceFrontDriver); - TELE.addData("distanceFrontPassenger", spindexer.distanceFrontPassenger); + //TELE.addData("distanceRearCenter", spindexer.distanceRearCenter); + //TELE.addData("distanceFrontDriver", spindexer.distanceFrontDriver); + //TELE.addData("distanceFrontPassenger", spindexer.distanceFrontPassenger); TELE.addData("shootall commanded", shootAll); + // Targeting Debug + TELE.addData("robotX", robotX); + TELE.addData( "robotY", robotY); + TELE.addData("robotInchesX", targeting.robotInchesX); + TELE.addData( "robotInchesY", targeting.robotInchesY); + TELE.addData("Targeting GridX", targeting.robotGridX); + TELE.addData("Targeting GridY", targeting.robotGridY); + TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); + TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); TELE.addData("timeSinceStamp", getRuntime() - shootStamp); + TELE.update(); ticker++; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java index ef854f1..8a85cac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java @@ -1,22 +1,13 @@ package org.firstinspires.ftc.teamcode.utils; -import static org.firstinspires.ftc.teamcode.constants.ShooterVars.kP; -import static org.firstinspires.ftc.teamcode.constants.ShooterVars.maxStep; -import com.arcrobotics.ftclib.controller.PIDFController; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; public class Flywheel { Robot robot; - MultipleTelemetry TELE; - - double initPos = 0.0; - double stamp = 0.0; - double stamp1 = 0.0; - double ticker = 0.0; - double currentPos = 0.0; + public PIDFCoefficients shooterPIDF1, shooterPIDF2; double velo = 0.0; double velo1 = 0.0; double velo2 = 0.0; @@ -25,6 +16,10 @@ public class Flywheel { boolean steady = false; public Flywheel (HardwareMap hardwareMap) { robot = new Robot(hardwareMap); + 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); } public double getVelo () { @@ -48,10 +43,6 @@ public class Flywheel { robot.shooterPIDF.d = d; robot.shooterPIDF.f = f; } - private double getTimeSeconds () - { - return (double) System.currentTimeMillis()/1000.0; - } // Convert from RPM to Ticks per Second private double RPM_to_TPS (double RPM) { return (RPM*28.0)/60.0;} @@ -62,13 +53,14 @@ public class Flywheel { public double manageFlywheel(double commandedVelocity) { targetVelocity = commandedVelocity; - // Turn PIDF for Target Velocities + // Add code here to set PIDF based on desired RPM //robot.shooterPIDF.p = P; //robot.shooterPIDF.i = I; //robot.shooterPIDF.d = D; //robot.shooterPIDF.f = F; - robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, robot.shooterPIDF); - robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, robot.shooterPIDF); + + robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); + robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java index 14b00f8..7a2acb0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -9,12 +9,17 @@ import com.qualcomm.robotcore.hardware.HardwareMap; public class Targeting { MultipleTelemetry TELE; - double unitConversionFactor = 1.02; + double cancelOffsetX = 7.071067811; + double cancelOffsetY = 7.071067811; + double unitConversionFactor = 0.95; int tileSize = 24; //inches + public double robotInchesX, robotInchesY = 0.0; + public int robotGridX, robotGridY = 0; + public static class Settings { public double flywheelRPM = 0.0; public double hoodAngle = 0.0; @@ -32,47 +37,47 @@ public class Targeting { static { KNOWNTARGETING = new Settings[6][6]; // ROW 0 - Closet to the goals - KNOWNTARGETING[0][0] = new Settings (4500.0, 0.1); - KNOWNTARGETING[0][1] = new Settings (4500.0, 0.1); - KNOWNTARGETING[0][2] = new Settings (4500.0, 0.1); - KNOWNTARGETING[0][3] = new Settings (4500.0, 0.1); - KNOWNTARGETING[0][4] = new Settings (4500.0, 0.1); - KNOWNTARGETING[0][5] = new Settings (4500.0, 0.1); + KNOWNTARGETING[0][0] = new Settings (3000.0, 0.25); + KNOWNTARGETING[0][1] = new Settings (3001.0, 0.25); + KNOWNTARGETING[0][2] = new Settings (3002.0, 0.25); + KNOWNTARGETING[0][3] = new Settings (3302.0, 0.2); + KNOWNTARGETING[0][4] = new Settings (3503.0, 0.15); + KNOWNTARGETING[0][5] = new Settings (3505.0, 0.15); // ROW 1 - KNOWNTARGETING[1][0] = new Settings (4500.0, 0.1); - KNOWNTARGETING[1][1] = new Settings (4500.0, 0.1); - KNOWNTARGETING[1][2] = new Settings (4500.0, 0.1); - KNOWNTARGETING[1][3] = new Settings (4500.0, 0.1); - KNOWNTARGETING[1][4] = new Settings (4500.0, 0.1); - KNOWNTARGETING[1][5] = new Settings (4500.0, 0.1); + KNOWNTARGETING[1][0] = new Settings (3010.0, 0.25); + KNOWNTARGETING[1][1] = new Settings (3011.0, 0.25); + KNOWNTARGETING[1][2] = new Settings (3012.0, 0.25); + KNOWNTARGETING[1][3] = new Settings (3313.0, 0.15); + KNOWNTARGETING[1][4] = new Settings (3514.0, 0.15); + KNOWNTARGETING[1][5] = new Settings (3515.0, 0.15); // ROW 2 - KNOWNTARGETING[2][0] = new Settings (4500.0, 0.1); - KNOWNTARGETING[2][1] = new Settings (4500.0, 0.1); - KNOWNTARGETING[2][2] = new Settings (4500.0, 0.1); - KNOWNTARGETING[2][3] = new Settings (4500.0, 0.1); - KNOWNTARGETING[2][4] = new Settings (4500.0, 0.1); - KNOWNTARGETING[2][5] = new Settings (4500.0, 0.1); + KNOWNTARGETING[2][0] = new Settings (3020.0, 0.1); + KNOWNTARGETING[2][1] = new Settings (3000.0, 0.25); + KNOWNTARGETING[2][2] = new Settings (3000.0, 0.15); + KNOWNTARGETING[2][3] = new Settings (3000.0, 0.15); + KNOWNTARGETING[2][4] = new Settings (3524.0, 0.15); + KNOWNTARGETING[2][5] = new Settings (3525.0, 0.15); // ROW 3 - KNOWNTARGETING[3][0] = new Settings (4500.0, 0.1); - KNOWNTARGETING[3][1] = new Settings (4500.0, 0.1); - KNOWNTARGETING[3][2] = new Settings (4500.0, 0.1); - KNOWNTARGETING[3][3] = new Settings (4500.0, 0.1); - KNOWNTARGETING[3][4] = new Settings (4500.0, 0.1); - KNOWNTARGETING[3][5] = new Settings (4500.0, 0.1); + KNOWNTARGETING[3][0] = new Settings (3030.0, 0.15); + KNOWNTARGETING[3][1] = new Settings (3031.0, 0.15); + KNOWNTARGETING[3][2] = new Settings (3000.0, 0.15); + KNOWNTARGETING[3][3] = new Settings (3000.0, 0.15); + KNOWNTARGETING[3][4] = new Settings (3000.0, 0.03); + KNOWNTARGETING[3][5] = new Settings (3535.0, 0.1); // ROW 4 - KNOWNTARGETING[4][0] = new Settings (4500.0, 0.1); - KNOWNTARGETING[4][1] = new Settings (4500.0, 0.1); - KNOWNTARGETING[4][2] = new Settings (4500.0, 0.1); - KNOWNTARGETING[4][3] = new Settings (4500.0, 0.1); - KNOWNTARGETING[4][4] = new Settings (4500.0, 0.1); - KNOWNTARGETING[4][5] = new Settings (4500.0, 0.1); + KNOWNTARGETING[4][0] = new Settings (4540.0, 0.1); + KNOWNTARGETING[4][1] = new Settings (4541.0, 0.1); + KNOWNTARGETING[4][2] = new Settings (4542.0, 0.1); + KNOWNTARGETING[4][3] = new Settings (4543.0, 0.1); + KNOWNTARGETING[4][4] = new Settings (4544.0, 0.1); + KNOWNTARGETING[4][5] = new Settings (4545.0, 0.1); // ROW 1 - KNOWNTARGETING[5][0] = new Settings (4500.0, 0.1); - KNOWNTARGETING[5][1] = new Settings (4500.0, 0.1); - KNOWNTARGETING[5][2] = new Settings (4500.0, 0.1); - KNOWNTARGETING[5][3] = new Settings (4500.0, 0.1); - KNOWNTARGETING[5][4] = new Settings (4500.0, 0.1); - KNOWNTARGETING[5][5] = new Settings (4500.0, 0.1); + KNOWNTARGETING[5][0] = new Settings (4550.0, 0.1); + KNOWNTARGETING[5][1] = new Settings (4551.0, 0.1); + KNOWNTARGETING[5][2] = new Settings (4552.0, 0.1); + KNOWNTARGETING[5][3] = new Settings (4553.0, 0.1); + KNOWNTARGETING[5][4] = new Settings (4554.0, 0.1); + KNOWNTARGETING[5][5] = new Settings (4555.0, 0.1); } public Targeting() @@ -82,19 +87,25 @@ public class Targeting { public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity) { Settings recommendedSettings = new Settings(0.0, 0.0); + double cos45 = Math.cos(Math.toRadians(-45)); + double sin45 = Math.sin(Math.toRadians(-45)); + double rotatedY = (robotX -40.0) * sin45 + (robotY +7.0) * cos45; + double rotatedX = (robotX -40.0) * cos45 - (robotY +7.0) * sin45; + // Convert robot coordinates to inches - double robotInchesX = robotX * unitConversionFactor; - double robotInchesY = robotY * unitConversionFactor; + robotInchesX = rotatedX * unitConversionFactor; + robotInchesY = rotatedY * unitConversionFactor; // Find approximate location in the grid - robotGridX = Math.floorDiv((int) robotInchesX, tileSize); - robotGridY = Math.floorDiv((int) robotInchesY, tileSize); + robotGridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) +1); + robotGridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); // Use Grid Location to perform lookup // Keep it simple for now but may want to interpolate results - recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM; - recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle; - + if ((robotGridY < 6) && (robotGridX <6)) { + recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM; + recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle; + } return recommendedSettings; } }