diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index d833fcb..b3d93af 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,17 +1,17 @@ -Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) -0 ,-29.5 ,8000 -78.5 ,-29.5 ,8500 -88 ,-34.2 ,8600 -90 ,-35.47 ,9500 -99 ,-39.62 ,9500 -111 ,-42 ,9500 -127.25 ,-49.12 ,9500 -141 ,-59.4 ,9900 -150 ,-66.22 ,10000 -164.5 ,-75.52 ,10000 -189.9 ,-81.39 ,11000 -207 ,-104.07 ,11000 -227 ,-105.32 ,11500 -239 ,-105.5 ,12380 -255.5 ,-105.8 ,13500 -999 ,-105.8 ,13500 +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Turret Offset (deg) +0 ,-29.5 ,8000 ,10 +78.5 ,-29.5 ,8500 ,8.5 +88 ,-34.2 ,8600 ,8.5 +90 ,-35.47 ,9500 ,8.5 +99 ,-39.62 ,9500 ,8.0 +111 ,-42 ,9500 ,8.0 +127.25 ,-49.12 ,9500 ,8.0 +141 ,-59.4 ,9900 ,7.5 +150 ,-66.22 ,10000 ,7.5 +164.5 ,-75.52 ,10000 ,6.5 +189.9 ,-81.39 ,11000 ,6.0 +207 ,-104.07 ,11000 ,5.5 +227 ,-105.32 ,11500 ,5.0 +239 ,-105.5 ,12380 ,4.5 +255.5 ,-105.8 ,13500 ,4.0 +999 ,-105.8 ,13500 ,3.0 diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 6659b19..b6ecdaf 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -55,7 +55,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; + public Double distance, hoodExt, drumVelocity, turretOffset; } private ShooterTableEntry[] m_shooterTable; @@ -101,6 +101,17 @@ public class BoomBoom extends SubsystemBase { return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); } + /** + * This is a function that takes a value (distance) and returns a value (turretOffset) that is a linear + * interpolation of the two values (turretOffset) at the two closest points in the table (m_shooterTable) + * to the given value (distance). + * @param distance Distance in shooter table + * @return Turret offset in degrees + */ + public Double getOffset(final Double distance) { + return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.turretOffset).doubleValue(); + } + @Override public void periodic() { // This method will be called once per scheduler run