Merge Drive Motor feed-forward from Dev

Finished XML config storage for our custom interpolation class
Shenzhen-Robotics-Alliance · Jul 11, 2024 · fb1779a · fb1779a
2 parents ac6fde5 + 300f89b
commit fb1779a
Show file tree

Hide file tree

Showing 8 changed files with 97 additions and 35 deletions.
diff --git a/src/main/deploy/configs/InterpolatedMotorFeedForward/DrivingMotorOpenLoop.xml b/src/main/deploy/configs/InterpolatedMotorFeedForward/DrivingMotorOpenLoop.xml
@@ -0,0 +1,18 @@
+<InterpolatedMotorFeedForward>
+    <info>
+        <interpolatedVariablesCount type="int">1</interpolatedVariablesCount>
+        <length type="int">2</length>
+    </info>
+
+    <variable0>
+        <name type="string">motorVelocity</name>
+        <sample0 type="double">0</sample0>
+        <sample1 type="double">4.8</sample1>
+    </variable0>
+
+    <variable1>
+        <name type="string">motorPower</name>
+        <sample0 type="double">0</sample0>
+        <sample1 type="double">1</sample1>
+    </variable1>
+</InterpolatedMotorFeedForward>
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -4,7 +4,6 @@
 
 package frc.robot;
 
-import edu.wpi.first.units.Unit;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.subsystems.MapleSubsystem;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -7,7 +7,6 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.units.Unit;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -55,7 +54,7 @@ public RobotContainer(String chassisName) {
         switch (Robot.CURRENT_ROBOT_MODE) {
             case REAL -> {
                 // Real robot, instantiate hardware IO implementations
-//                drive = new Drive(
+//                drive = new SwerveDrive(
 //                        new GyroIOPigeon2(),
 //                        new ModuleIOSparkMax(0),
 //                        new ModuleIOSparkMax(1),

diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java
@@ -33,11 +33,7 @@ public SwerveModule(ModuleIO io, String name) {
         this.io = io;
         this.name = name;
 
-        driveOpenLoop = new InterpolatedMotorFeedForward(
-                "DriveWheelOpenLoop",
-                new double[] {0, 1},
-                new double[] {0, 5.2}
-        );
+        driveOpenLoop = InterpolatedMotorFeedForward.fromDeployedDirectory("DrivingMotorOpenLoop");
         turnCloseLoop = new MapleSimplePIDController(Constants.SwerveModuleConfigs.steerHeadingCloseLoopConfig, 0);
 
         CommandScheduler.getInstance().unregisterSubsystem(this);

diff --git a/src/main/java/frc/robot/tests/InterpolationTableTest.java b/src/main/java/frc/robot/tests/InterpolationTableTest.java
@@ -1,29 +1,33 @@
 package frc.robot.tests;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.utils.Config.MapleConfigFile;
 import frc.robot.utils.Config.MapleInterpolationTable;
 
+import java.io.IOException;
+
 public class InterpolationTableTest implements UnitTest {
     private final MapleInterpolationTable testTable;
     public InterpolationTableTest() {
-        testTable = new MapleInterpolationTable(
-                "testTable",
-                new MapleInterpolationTable.Variable("x", 1, 2, 3, 4),
-                new MapleInterpolationTable.Variable("y", 2, 3, 4, 5)
-        );
+        try {
+            testTable = MapleInterpolationTable.fromConfigFile(MapleConfigFile.fromDeployedConfig("InterpolatedMotorFeedForward", "DrivingMotorOpenLoop"));
+        } catch (IOException e) {
+            throw new RuntimeException(e);
+        }
     }
 
     @Override
     public void testStart() {
-        SmartDashboard.putNumber("InterpolationTables/testValueX", 1.0);
+        SmartDashboard.putNumber("InterpolationTables/testValueVelocity", 1.0);
     }
 
     @Override
     public void testPeriodic() {
-        final double x = SmartDashboard.getNumber("InterpolationTables/testValueX", 1.0);
+        assert testTable != null;
+        final double vel = SmartDashboard.getNumber("InterpolationTables/testValueVelocity", 1.0);
         SmartDashboard.putNumber(
-                "InterpolationTables/testOutPutY",
-                testTable.interpolateVariable("y", x)
+                "InterpolationTables/testOutPutPower",
+                testTable.interpolateVariable("motorPower", vel)
         );
     }
 }
diff --git a/src/main/java/frc/robot/utils/Config/MapleConfigFile.java b/src/main/java/frc/robot/utils/Config/MapleConfigFile.java
@@ -21,8 +21,8 @@
 import org.w3c.dom.NodeList;
 
 public class MapleConfigFile {
-    private final String configType;
-    private final String configName;
+    public final String configType;
+    public final String configName;
 
     public static final class ConfigBlock {
         private final String blockName;

diff --git a/src/main/java/frc/robot/utils/Config/MapleInterpolationTable.java b/src/main/java/frc/robot/utils/Config/MapleInterpolationTable.java
@@ -5,13 +5,12 @@
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
-import java.io.IOException;
 import java.util.HashMap;
 import java.util.Map;
 import java.util.Objects;
 
 public class MapleInterpolationTable {
-    private final String tableName;
+    public final String tableName;
     private final Variable independentVariable;
     private final Map<String, Variable> interpolatedVariables;
 
@@ -27,21 +26,35 @@ public Variable(String name, double... values) {
         }
 
         public void initializeTuningPanelOnDashboard() {
-//            if (Objects.equals(tableName, "Unknown"))
-//                return;
-//            SmartDashboard.putNumberArray("InterpolationTables/" + tableName + "/" + variableName, values);
+            if (Objects.equals(tableName, "Unknown"))
+                return;
+            SmartDashboard.putNumberArray("InterpolationTables/" + tableName + "/" + variableName, values);
         }
 
         public void updateValuesFromDashboard() {
             if (Objects.equals(tableName, "Unknown"))
                 return;
 
-            final double[] updatedValues = SmartDashboard.getNumberArray("InterpolationTables/" + tableName + "/" + variableName, values);
+            final double[] updatedValues = SmartDashboard.getNumberArray("InterpolationTables/" + tableName + "/" + variableName, new double[]{});
             if (updatedValues.length != values.length)
                 return;
-
             System.arraycopy(updatedValues, 0, values, 0, values.length);
         }
+
+        public void toBlock(MapleConfigFile.ConfigBlock block) {
+            block.putStringConfig("name", variableName);
+            for (int i = 0; i < values.length; i++)
+                block.putDoubleConfig("sample"+i, values[i]);
+        }
+
+        public static Variable fromBlock(MapleConfigFile.ConfigBlock block, int length) {
+            final String name = block.getStringConfig("name");
+            final double[] values = new double[length];
+            for (int i = 0; i < length; i++)
+                values[i] = block.getDoubleConfig("sample"+i);
+
+            return new Variable(name, values);
+        }
     }
     public MapleInterpolationTable(String name, Variable independentVariable, Variable... interpolatedVariables) {
         this.tableName = name;
@@ -92,12 +105,27 @@ public double interpolateVariable(String interpolatedVariableName, double indepe
 
     public MapleConfigFile toConfigFile(String configType) {
         final MapleConfigFile configFile = new MapleConfigFile(configType, tableName);
-        // TODO write this part
+
+        final MapleConfigFile.ConfigBlock infoBlock = configFile.getBlock("info");
+        infoBlock.putIntConfig("interpolatedVariablesCount", interpolatedVariables.size());
+        infoBlock.putIntConfig("length", independentVariable.values.length);
+        independentVariable.toBlock(configFile.getBlock("variable0"));
+        int i = 1;
+        for (Variable interpolatedVariable:interpolatedVariables.values())
+            interpolatedVariable.toBlock(configFile.getBlock("variable" + (i++)));
+
         return configFile;
     }
 
-    public static MapleInterpolationTable fromDeployDirectory(String configType, String name) throws IOException {
-        final MapleConfigFile configFile = MapleConfigFile.fromDeployedConfig(configType, name);
-        return new MapleInterpolationTable(name, null); // TODO: write this part
+    public static MapleInterpolationTable fromConfigFile(MapleConfigFile configFile) {
+        final MapleConfigFile.ConfigBlock infoBlock = configFile.getBlock("info");
+        final int interpolatedVariablesCount = infoBlock.getIntConfig("interpolatedVariablesCount"),
+                length = infoBlock.getIntConfig("length");
+
+        final Variable independentVariable = Variable.fromBlock(configFile.getBlock("variable0"), length);
+        final Variable[] interpolateVariables = new Variable[interpolatedVariablesCount];
+        for (int i = 0; i < interpolatedVariablesCount; i++)
+            interpolateVariables[i] = Variable.fromBlock(configFile.getBlock("variable"+(i+1)), length);
+        return new MapleInterpolationTable(configFile.configName, independentVariable, interpolateVariables); // TODO: write this part
     }
 }
diff --git a/src/main/java/frc/robot/utils/MechanismControl/InterpolatedMotorFeedForward.java b/src/main/java/frc/robot/utils/MechanismControl/InterpolatedMotorFeedForward.java
@@ -3,19 +3,24 @@
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import frc.robot.utils.Config.MapleConfigFile;
 import frc.robot.utils.Config.MapleInterpolationTable;
-import org.littletonrobotics.junction.Logger;
+
+import java.io.IOException;
 
 public class InterpolatedMotorFeedForward extends SimpleMotorFeedforward {
     private final String name;
     private final MapleInterpolationTable interpolationTable;
     public InterpolatedMotorFeedForward(String name, double[] motorPower, double[] velocityMeasured) {
-        super(0, 0);
-        this.name = name;
-        this.interpolationTable = new MapleInterpolationTable(
+        this(new MapleInterpolationTable(
                 name,
                 new MapleInterpolationTable.Variable("motorVelocity", velocityMeasured),
                 new MapleInterpolationTable.Variable("motorPower", motorPower)
-        );
+        ));
+    }
+
+    protected InterpolatedMotorFeedForward(MapleInterpolationTable interpolationTable) {
+        super(0, 0);
+        this.name = interpolationTable.tableName;
+        this.interpolationTable = interpolationTable;
     }
 
     @Override
@@ -33,4 +38,17 @@ public void saveFeedForwardConfigToUSB() {
         final MapleConfigFile configFile = interpolationTable.toConfigFile("InterpolatedMotorFeedForward");
         configFile.saveConfigToUSBSafe();
     }
+
+    public static InterpolatedMotorFeedForward fromDeployedDirectory(String name) {
+        final MapleInterpolationTable interpolationTable;
+        try {
+            interpolationTable = MapleInterpolationTable.fromConfigFile(MapleConfigFile.fromDeployedConfig(
+                    "InterpolatedMotorFeedForward", name
+            ));
+        } catch (IOException e) {
+            throw new RuntimeException(e);
+        }
+
+        return new InterpolatedMotorFeedForward(interpolationTable);
+    }
 }