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 bc366be..702840b 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 @@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.tests; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -20,7 +22,9 @@ public class NewShooterTest extends LinearOpMode { Robot robot; Flywheel flywheel; Turret turret; + Shooter shooter; MultipleTelemetry TELE; + Follower follower; public static boolean intake = true; @@ -52,14 +56,16 @@ public class NewShooterTest extends LinearOpMode { TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(new Pose(72, 72, 0)); flywheel = new Flywheel(robot); turret = new Turret(robot); - Shooter shooter = new Shooter( + shooter = new Shooter( robot, TELE, - Constants.createFollower(hardwareMap), + follower, true, turret, flywheel @@ -73,6 +79,8 @@ public class NewShooterTest extends LinearOpMode { while (opModeIsActive()) { + follower.update(); + robot.setHoodPos(hoodPos); shooter.setTurretPosition(turretPos); shooter.setFlywheelVelocity(flywheel_velo); @@ -136,9 +144,10 @@ public class NewShooterTest extends LinearOpMode { TELE.addData("Flywheel Velocity1", (robot.shooter1.getVelocity() * 60) / 28); TELE.addData("Flywheel Velocity2", (robot.shooter2.getVelocity() * 60) / 28); - TELE.addData("Flywheel Averag Velocity", flywheel.getAverageVelocity()); + TELE.addData("Flywheel Average Velocity", flywheel.getAverageVelocity()); TELE.addData("PIDF Coefficients", Flywheel.shooterPIDF); TELE.addData("Power", flywheel.getShooterPower()); + TELE.addData("Distance", shooter.getDistance()); 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 a4dc26e..ec5a462 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 @@ -128,5 +128,7 @@ public class Flywheel { steady = (Math.abs(commandedVelocity - averageVelocity) < 50); } + + public double getShooterPower(){return power;} } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java index 5f893bd..e68ff91 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -74,20 +74,30 @@ public class Shooter { return turr.getObeliskID(); } - + private final double shooterDistFromCenter = 1.545; public void update() { switch (state) { case NOTHING: break; case MANUAL: + + commander.getVeloPredictive( + (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), + (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), + follow.getVelocity().getXComponent(), + follow.getAcceleration().getXComponent(), + follow.getVelocity().getYComponent(), + follow.getAcceleration().getYComponent() + ); + fly.manageFlywheel(flywheelVelocity); turr.manual(turretPosition); break; case TRACK_GOAL: turr.trackGoal( - (goalX - follow.getPose().getX()), - (goalY - follow.getPose().getY()), + (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), + (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), follow.getHeading(), follow.getAngularVelocity(), follow.getVelocity().getXComponent(), @@ -97,8 +107,8 @@ public class Shooter { ); flywheelVelocity = commander.getVeloPredictive( - (goalX - follow.getPose().getX()), - (goalY - follow.getPose().getY()), + (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), + (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), follow.getVelocity().getXComponent(), follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), @@ -109,14 +119,14 @@ public class Shooter { break; case READ_OBELISK: turr.trackObelisk( - (goalX - follow.getPose().getX()), - (goalY - follow.getPose().getY()), + (obeliskX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), + (obeliskY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), follow.getHeading() ); flywheelVelocity = commander.getVeloPredictive( - (goalX - follow.getPose().getX()), - (goalY - follow.getPose().getY()), + (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), + (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), follow.getVelocity().getXComponent(), follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), @@ -129,8 +139,8 @@ public class Shooter { case MANUAL_TURRET_TRACK_FLY: turr.manual(turretPosition); flywheelVelocity = commander.getVeloPredictive( - (goalX - follow.getPose().getX()), - (goalY - follow.getPose().getY()), + (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), + (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), follow.getVelocity().getXComponent(), follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), @@ -142,8 +152,8 @@ public class Shooter { case MANUAL_FLYWHEEL_TRACK_TURR: turr.trackGoal( - (goalX - follow.getPose().getX()), - (goalY - follow.getPose().getY()), + (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), + (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), follow.getHeading(), follow.getAngularVelocity(), follow.getVelocity().getXComponent(), @@ -158,4 +168,7 @@ public class Shooter { } + public double getDistance(){return commander.getDistance();} + + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java index fd8edb8..8febf9f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java @@ -83,7 +83,7 @@ public class Turret { robot.setTurretPos(pos); } - + // 1.545 public void trackGoal(double dx, double dy, double h, double hVel, double xVel, double xAcc, double yVel, double yAcc) { // dx, dy, dz is target - robot diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java index 1c22106..6490e2f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java @@ -15,20 +15,22 @@ public class VelocityCommander { return Math.sqrt(dist*dist + goalH*goalH) * 20; //TODO: Add regression here using goalH } - + // 27 public double getVeloStationary (double distance){ return distToRPM(distance); } + private final double goalHeight = 28; + double predictedDist = 0; public double getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc) { double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK); // Negative bc dy = target - robot - double predictedDist = Math.sqrt(dx*dx + dy*dy); + predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight*goalHeight); return distToRPM(predictedDist); } - + public double getDistance(){return predictedDist;} }