From 2f23aed7f1997beff59907637e8477239741a78b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 15:35:38 -0600 Subject: [PATCH] vel correction setup --- src/main/deploy/ShooterData.csv | 24 ++++++------ src/main/deploy/VelocityCorrectionData.csv | 12 ++++++ src/main/java/frc4388/robot/Constants.java | 2 +- .../robot/commands/ShooterCommands/Shoot.java | 39 +------------------ .../frc4388/robot/subsystems/BoomBoom.java | 13 +------ 5 files changed, 27 insertions(+), 63 deletions(-) create mode 100644 src/main/deploy/VelocityCorrectionData.csv diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index c11dc0f..ccef197 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,12 +1,12 @@ -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, -150 ,-66.22 ,10000 ,0, -164.5 ,-75.52 ,10000 ,0, -186 ,-76.24 ,10000 ,0, -207 ,-104.07 ,11000 ,0, -227 ,-105.32 ,11500 ,0, -255.5 ,-105.8 ,13500 ,0, -999 ,-105.8 ,13500 ,0, \ No newline at end of file +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) +0 ,-29.5 ,8000 +78.5 ,-29.5 ,8000 +99 ,-39.62 ,8500 +127.25 ,-49.12 ,9500 +150 ,-66.22 ,10000 +164.5 ,-75.52 ,10000 +186 ,-76.24 ,10000 +207 ,-104.07 ,11000 +227 ,-105.32 ,11500 +255.5 ,-105.8 ,13500 +999 ,-105.8 ,13500 \ No newline at end of file diff --git a/src/main/deploy/VelocityCorrectionData.csv b/src/main/deploy/VelocityCorrectionData.csv new file mode 100644 index 0000000..21d13a2 --- /dev/null +++ b/src/main/deploy/VelocityCorrectionData.csv @@ -0,0 +1,12 @@ +Distance (in) ,Duration (s) +0 ,0 +78.5 ,0 +99 ,0 +127.25 ,0 +150 ,0 +164.5 ,0 +186 ,0 +207 ,0 +227 ,0 +255.5 ,0 +999 ,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 2f43844..17d6f24 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -210,7 +210,7 @@ public final class Constants { public static final double TURRET_FORWARD_LIMIT = 0.0; public static final double TURRET_REVERSE_LIMIT = -95.0; //Find //Shooter gains for actual Drum - public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); // TODO: tune values + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); /* Hood Constants */ public static final int SHOOTER_ANGLE_ADJUST_ID = 20; diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 94bb04f..d39f4ce 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -96,7 +96,7 @@ public class Shoot extends CommandBase { * Updates error for custom PID. */ public void updateError() { - m_targetPoint = new Pose2d(hTargetDistanceFromHub(), vTargetDistanceFromHub(), SwerveDriveConstants.HUB_POSE.getRotation()); + m_targetPoint = SwerveDriveConstants.HUB_POSE; m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get()); // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees()); error = (m_targetAngle - turretDummy.get() + 360) % 360; @@ -152,43 +152,6 @@ public class Shoot extends CommandBase { output = kP * proportional + kI * integral + kD * derivative; normOutput = output/360 * inverted; } - // TODO: horizontal velocity correction - public double hTargetDistanceFromHub() { - - double hVel = m_swerve.getChassisSpeeds()[0]; - double velBeforeCorrection = m_boomBoom.getVelocity(m_distance); - double vDistanceFromHub = m_odoY; - double approxTravelTime = vDistanceFromHub / velBeforeCorrection; - double hTargetDistanceFromHub = hVel * approxTravelTime; - - // return hTargetDistanceFromHub; - return 0.0; // this is for no velocity correction - } - - public Pose2d findTargetPoint() { - - // position vector and radius - Translation2d position = new Translation2d(m_odoX, m_odoY); - double radius = position.getNorm(); - - // equation of circle = x^2 + y^2 = m_distance^2 - // derivative of circle = 2x + 2y * y' = 0 --> y' = -x/y - - // velocity vector (x, y) - Translation2d cartesianVelocity = new Translation2d(m_swerve.getChassisSpeeds()[0], m_swerve.getChassisSpeeds()[1]); - - // unit tangential vector - Translation2d tangential = new Translation2d(0, 0); - // velocity vector (tangential, radial) - Translation2d polarVelocity = new Translation2d(0, 0); - - return SwerveDriveConstants.HUB_POSE; - } - - // TODO: vertical velocity correction - public double vTargetDistanceFromHub() { - return 0.0; // this is for no velocity correction - } // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 9e7d4b8..c81fd38 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -48,7 +48,7 @@ public class BoomBoom extends SubsystemBase { // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later public static class ShooterTableEntry { - public Double distance, hoodExt, drumVelocity, duration; + public Double distance, hoodExt, drumVelocity; } private ShooterTableEntry[] m_shooterTable; @@ -116,17 +116,6 @@ public class BoomBoom extends SubsystemBase { return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); } - /** - * This is a function that takes a value (distance) and returns a value (duration) that is a linear - * interpolation of the two values (duration) at the two closest points in the table (m_shooterTable) - * to the given value (distance). - * @param distance Distance in shooter table - * @return Shot duration in seconds - */ - public Double getDuration(final Double distance) { - return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.duration).doubleValue(); - } - /** * Using the given lookup value (x) and lookup getter function, locates the nearest entries in the * given table to be used as the lower (x0) and upper (x1) bounds for interpolation. Returns the