From ca4d87977ad205d5b2a5a720d55e22a5ec3e87d4 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Wed, 16 Mar 2022 10:32:15 -0600 Subject: [PATCH] Track target working --- src/main/deploy/ShooterData.csv | 4 +++- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 3 ++- .../commands/ShooterCommands/TrackTarget.java | 20 +++++++++++++------ .../frc4388/robot/subsystems/BoomBoom.java | 2 +- .../robot/subsystems/VisionOdometry.java | 2 ++ 7 files changed, 25 insertions(+), 12 deletions(-) diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index bdd5dc9..52e1e46 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,4 +1,5 @@ Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Duration (s), +0 ,-29.5 ,8000 ,0, 78.5 ,-29.5 ,8000 ,0, 99 ,-39.62 ,8500 ,0, 127.25 ,-49.12 ,9500 ,0, @@ -7,4 +8,5 @@ Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Duration (s), 186 ,-76.24 ,10000 ,0, 207 ,-104.07 ,11000 ,0, 227 ,-105.32 ,11500 ,0, -255.5 ,-97.8 ,14500 ,0, +255.5 ,-105.8 ,13500 ,0, +999 ,-105.8 ,13500 ,0, \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bd25aab..6ecf795 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -236,8 +236,8 @@ public final class Constants { public static final double LIME_HEIGHT = 26; public static final double TARGET_RADIUS = 4*12; //TODO: Convert to metric (does this still need to be converted?) public static final double H_FOV = 59.6; - public static final double V_FOV = 49.7; - public static final double LIME_HIXELS = 960; + public static final double V_FOV = 45.7; + public static final double LIME_HIXELS = 920; public static final double LIME_VIXELS = 720; public static final double TURRET_kP = 0.1; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index af3774a..30e77dd 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -216,7 +216,7 @@ public class Robot extends TimedRobot { LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); } - m_robotContainer.m_robotVisionOdometry.setLEDs(false); + m_robotContainer.m_robotVisionOdometry.setLEDs(true); } @Override diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8b43872..a68376e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -321,7 +321,8 @@ public class RobotContainer { //B > Shoot with Lime new JoystickButton(getOperatorController(), XboxController.Button.kB.value) .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) - .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))); + .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))) + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); // .whileHeld% diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 4fee5c8..b313fa8 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -9,6 +9,7 @@ import java.util.ArrayList; import org.opencv.core.Point; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.VisionConstants; @@ -76,7 +77,8 @@ public class TrackTarget extends CommandBase { DesmosServer.putPoint("average", average); output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; - output *= 2; + output *= 4; + // output *= 0.5; DesmosServer.putDouble("output", output); m_turret.runTurretWithInput(output); @@ -94,15 +96,21 @@ public class TrackTarget extends CommandBase { DesmosServer.putDouble("distanceReg", regressedDistance); //Vision odemetry circle fit based pose estimate - Point targetOffset = m_visionOdometry.getTargetOffset(); - DesmosServer.putPoint("targetOff", targetOffset); + // Point targetOffset = m_visionOdometry.getTargetOffset(); + // DesmosServer.putPoint("targetOff", targetOffset); - vel = m_boomBoom.getVelocity(distance); - hood = m_boomBoom.getHood(distance); + vel = m_boomBoom.getVelocity(regressedDistance + 30); + hood = m_boomBoom.getHood(regressedDistance + 30); // m_boomBoom.runDrumShooter(vel); m_boomBoom.runDrumShooterVelocityPID(vel); m_hood.runAngleAdjustPID(hood); + SmartDashboard.putNumber("Regressed Distance", regressedDistance); + SmartDashboard.putNumber("Distance", distance); + SmartDashboard.putNumber("Hood Target Angle Track", hood); + SmartDashboard.putNumber("Vel Target Track", vel); + + // isExecuted = true; } catch (Exception e){ @@ -113,7 +121,7 @@ public class TrackTarget extends CommandBase { } public final double distanceRegression(double distance) { - return (1.09517561985 * distance + 20.1846165624); + return (79.6078 * Math.pow(1.01343, distance) - 56.6671); } public void updateRegressionDesmos() { diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index bc8d8cf..9e7d4b8 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -217,7 +217,7 @@ public class BoomBoom extends SubsystemBase { public void runDrumShooterVelocityPID(double targetVel) { SmartDashboard.putNumber("Target Drum Velocity", 10000 + pidOffset); - m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel + pidOffset); // Init + m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init // New BoomBoom controller stuff // Controls a motor with the output of the BangBang controller diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index f1048b0..2a0abd2 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -59,6 +59,8 @@ public class VisionOdometry extends SubsystemBase { public ArrayList getTargetPoints() throws VisionObscuredException { PhotonPipelineResult result = m_camera.getLatestResult(); latency = result.getLatencyMillis(); + + System.out.println("Result: " + result.hasTargets() + ", latency: " + latency); if(!result.hasTargets()) throw new VisionObscuredException();