Drivetrain characterization

.vscode/launch.json

@@ -0,0 +1,27 @@
+{
+    // Use IntelliSense to learn about possible attributes.
+    // Hover to view descriptions of existing attributes.
+    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "type": "java",
+            "name": "Debug (Launch) - Current File",
+            "request": "launch",
+            "mainClass": "${file}"
+        },
+        {
+            "type": "java",
+            "name": "Debug (Launch)-Main<Robot2019>",
+            "request": "launch",
+            "mainClass": "frc.robot.Main",
+            "projectName": "Robot2019"
+        },
+        {
+            "type": "java",
+            "name": "Debug (Launch)-DrivetrainCharacterization",
+            "request": "launch",
+            "mainClass": "DrivetrainCharacterization"
+        }
+    ]
+}

Robot2019/build.gradle

@@ -50,6 +50,9 @@ dependencies {
     nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio)
     nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop)
     testCompile 'junit:junit:4.12'
+    compile group: 'org.apache.commons', name: 'commons-math3', version: '3.6.1'
+    compile group: 'org.apache.commons', name: 'commons-lang3', version: '3.8.1'
+    compile group: 'org.apache.commons', name: 'commons-csv', version: '1.6'
 }
 
 // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')

Robot2019/src/main/java/frc/robot/DrivetrainCharAnalysis.java

@@ -0,0 +1,215 @@
+package frc.robot;
+
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.FileReader;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.io.Reader;
+import java.util.ArrayList;
+import java.util.List;
+
+import org.apache.commons.math3.stat.regression.OLSMultipleLinearRegression;
+import org.apache.commons.lang3.ArrayUtils;
+import org.apache.commons.csv.CSVRecord;
+import org.apache.commons.csv.CSVFormat;
+import org.apache.commons.csv.CSVParser;
+
+public class DrivetrainCharAnalysis {
+    // Ordinary Least Squares approach (multi-variable approach)
+    public static void ordinaryLeastSquares(String file1, String file2, String outfile) {
+        double leftKv, rightKv;
+        double leftKa, rightKa;
+        double leftVoltageIntercept, rightVoltageIntercept;
+        int spread = 30;
+
+        double[][] returns;
+        double[] params, leftVelocities, rightVelocities, voltages, leftAccelerations, rightAccelerations;
+
+        returns = parseCSV(file1, spread);
+        leftVelocities = returns[0];
+        rightVelocities = returns[1];
+        voltages = returns[2];
+        leftAccelerations = returns[3];
+        rightAccelerations = returns[4];
+
+        returns = parseCSV(file2, spread);
+        leftVelocities = ArrayUtils.addAll(leftVelocities, returns[0]);
+        rightVelocities = ArrayUtils.addAll(rightVelocities, returns[1]);
+        voltages = ArrayUtils.addAll(voltages, returns[2]);
+        leftAccelerations = ArrayUtils.addAll(leftAccelerations, returns[3]);
+        rightAccelerations = ArrayUtils.addAll(rightAccelerations, returns[4]);
+
+        double[][] xs = new double[voltages.length][2];
+        double[] ys = new double[voltages.length];
+        for (int i = 0; i < voltages.length; i++) {
+            xs[i][0] = leftVelocities[i];
+            xs[i][1] = leftAccelerations[i];
+            ys[i] = voltages[i];
+        }
+
+        OLSMultipleLinearRegression algorithm = new OLSMultipleLinearRegression();
+        algorithm.newSampleData(ys, xs);
+        params = algorithm.estimateRegressionParameters();
+        // System.out.println(params.length);
+        leftKv = params[1];
+        leftKa = params[2];
+        leftVoltageIntercept = params[0];
+
+        xs = new double[voltages.length][2];
+        ys = new double[voltages.length];
+        for (int i = 0; i < voltages.length; i++) {
+            xs[i][0] = rightVelocities[i];
+            xs[i][1] = rightAccelerations[i];
+            ys[i] = voltages[i];
+        }
+
+        algorithm = new OLSMultipleLinearRegression();
+        algorithm.newSampleData(ys, xs);
+        params = algorithm.estimateRegressionParameters();
+        // System.out.println(params.length);
+        rightKv = params[1];
+        rightKa = params[2];
+        rightVoltageIntercept = params[0];
+
+        FileWriter f;
+        try {
+            f = new FileWriter(new File(outfile), true);
+            f.append(leftKv + "," + leftKa + "," + leftVoltageIntercept + "\r\n");
+            f.append(rightKv + "," + rightKa + "," + rightVoltageIntercept + "\r\n");
+            f.close();
+        } catch (IOException e) {
+            e.printStackTrace();
+        }
+    }
+
+    // OUTDATED
+    // Simplistic approach to the problem.
+    public static void simpleRegression(String file1, String file2, String outfile) {
+        // Argument 1 should be the filepath of the linear increase CSV file.
+        // Argument 2 should be the filepath of the stepwise increase CSV file.
+        double kv = 0.0; // The parameter for velocity in the drivetrain characterization formula
+        double ka = 0.0; // The parameter for acceleration in the drivetrain characterization formula
+        double voltageIntercept = 0.0;
+        int spread = 18; // How much data should our difference quotient cover?
+
+        double[][] returns;
+        double[] params, velocities, voltages, accelerations, stepwise_x;
+
+        returns = parseCSV(file1, spread);
+        velocities = returns[0];
+        voltages = returns[1];
+        accelerations = returns[2];
+
+        // System.out.println(linearVelocities);
+        params = simpleRegressionFormula(voltages, velocities);
+        kv = 1 / params[1]; // Voltseconds / inches
+        voltageIntercept = -params[0] / params[1]; // Think of this as -(b/m) in y = mx + b
+
+        returns = parseCSV(file2, spread);
+        velocities = returns[0];
+        voltages = returns[1];
+        accelerations = returns[2];
+        stepwise_x = velocities; // To define the size of stepwise_x
+
+        for (int i = 0; i < velocities.length; i++) {
+            stepwise_x[i] = voltages[i] - (kv * velocities[i] + voltageIntercept);
+        }
+
+        params = simpleRegressionFormula(stepwise_x, accelerations);
+        ka = 1 / params[1]; // Volt * (seconds^2) / inches.
+
+        System.out.print("Velocity Constant is " + Double.toString(12 * kv) /* Change inches to feet in printout */);
+        System.out.print(" and the Acceleration Constant is " + Double.toString(12 * ka));
+        System.out.print(" with a voltage intercept of " + Double.toString(voltageIntercept) + "\n");
+
+        FileWriter f;
+        try {
+            f = new FileWriter(new File(outfile));
+            f.append(kv + "," + ka + "," + voltageIntercept);
+            f.close();
+        } catch (IOException e) {
+            e.printStackTrace();
+        }
+    }
+
+    public static double[] simpleRegressionFormula(double[] xs, double[] ys) {
+        double sx, sxx, sy, sxy, a, b;
+        sx = sxx = sy = sxy = a = b = 0.0;
+        int n = xs.length;
+
+        for (int i = 0; i < n; i++) {
+            sx += xs[i];
+            sxx += xs[i] * xs[i];
+            sxy += xs[i] * ys[i];
+            sy += ys[i];
+        }
+
+        b = (n * sxy - sx * sy) / (n * sxx - sx * sx);
+        a = (sy - b * sx) / n;
+
+        double[] params = { a, b };
+        return params;
+    }
+
+    public static double[][] parseCSV(String filename, int spread) {
+        try {
+            Reader in = new FileReader(filename);
+            CSVParser csvParser = new CSVParser(in, CSVFormat.DEFAULT);
+
+            List<CSVRecord> records = csvParser.getRecords();
+            int num_values = records.size();
+            List<Double> leftVelocities, rightVelocities, voltages, leftAccelerations, rightAccelerations;
+            leftVelocities = new ArrayList<Double>();
+            rightVelocities = new ArrayList<Double>();
+            voltages = new ArrayList<Double>();
+            leftAccelerations = new ArrayList<Double>();
+            rightAccelerations = new ArrayList<Double>();
+            leftAccelerations.add(0.0);
+            rightAccelerations.add(0.0);
+
+            for (int n = 0; n < num_values; n++) {
+                CSVRecord record = records.get(n);
+                if (!record.get(0).equals("Timestamp (s)")) {
+                    double v1 = Math.abs(Double.valueOf(record.get(2)));
+                    double v2 = Math.abs(Double.valueOf(record.get(3)));
+                    double v = Double.valueOf(record.get(1));
+
+                    if (leftVelocities.size() >= spread) {
+                        double a1 = (v1 - leftVelocities.get(leftVelocities.size() - spread)) / (spread * 0.02);
+                        double a2 = (v2 - rightVelocities.get(rightVelocities.size() - spread)) / (spread * 0.02);
+                        leftAccelerations.add(Math.abs(a1));
+                        rightAccelerations.add(Math.abs(a2));
+                    }
+
+                    if (leftVelocities.size() < num_values - spread) {
+                        leftVelocities.add(v1);
+                        rightVelocities.add(v2);
+                        voltages.add(v);
+                    }
+                }
+            }
+
+            Double[][] Returns = { leftVelocities.toArray(new Double[leftVelocities.size()]), 
+                rightVelocities.toArray(new Double[rightVelocities.size()]), 
+                voltages.toArray(new Double[voltages.size()]), 
+                leftAccelerations.toArray(new Double[leftAccelerations.size()]), 
+                rightAccelerations.toArray(new Double[rightAccelerations.size()]) };
+
+            double[][] returns = new double[5][voltages.size()];
+            for (int i = 0; i < 5; i++) {
+                for (int j = 0; j < voltages.size(); j++) {
+                    returns[i][j] = Returns[i][j];
+                }
+            }    
+
+            return returns;
+        } catch (FileNotFoundException fnf) {
+            fnf.printStackTrace();
+            return null;
+        } catch (IOException io) {
+            io.printStackTrace();
+            return null;
+        }
+    }
+}

Robot2019/src/main/java/frc/robot/OI.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
 import frc.robot.commands.ActuateClimberRails;
+import frc.robot.commands.NormalDrive;
 import frc.robot.commands.Climb;
 import frc.robot.commands.EjectCargo;
 import frc.robot.commands.IntakeOnlyCargo;
@@ -31,6 +32,7 @@ public class OI {
   Joystick leftJoy, rightJoy, manipulator;
 
   JoystickButton leftSlowBtn, rightSlowBtn;
+  JoystickButton normDriveBtn;
   JoystickButton toggleHatchBtn;
   JoystickButton cargoIntakeBtn, cargoEjectBtn;
   JoystickButton climberRailBtn;
@@ -48,6 +50,9 @@ public class OI {
     rightSlowBtn = new JoystickButton(rightJoy, 1);
     rightSlowBtn.whileHeld(new SlowDrive(SlowDrive.Side.RIGHT));
 
+    normDriveBtn = new JoystickButton(leftJoy, 3);
+    normDriveBtn.whileHeld(new NormalDrive());
+
     toggleHatchBtn = new JoystickButton(manipulator, Manip.X); // TODO: set ports to correct values
     toggleHatchBtn.whenPressed(new ToggleHatch(hp));
 

Robot2019/src/main/java/frc/robot/Robot.java

@@ -9,6 +9,10 @@
 
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.commands.DrivetrainAnalysis;
+import frc.robot.commands.IncreaseVoltageLinear;
+import frc.robot.commands.IncreaseVoltageStepwise;
 import frc.robot.commands.TeleopDrive;
 import frc.robot.subsystems.Cargo;
 import frc.robot.subsystems.Climber;
@@ -21,6 +25,7 @@ public class Robot extends TimedRobot {
   private static OI oi;
   private static Cargo cargo;
   private static Climber climber;
+  private static String fname1, fname2, fname3, fname4;
 
   @Override
   public void robotInit() {
@@ -32,7 +37,23 @@ public void robotInit() {
 
     oi = new OI(dt, hp, cargo, climber, RobotMap.driveCamera, RobotMap.hatchCamera, RobotMap.cameraServer);
 
+    fname1 = "/home/lvuser/drive_char_linear_for.csv";
+    fname2 = "/home/lvuser/drive_char_stepwise_for.csv";
+    fname3 = "/home/lvuser/drive_char_linear_back.csv";
+    fname4 = "/home/lvuser/drive_char_stepwise_back.csv";
+    IncreaseVoltageLinear ivlf = new IncreaseVoltageLinear(dt, 0.25 / 50, 6.0, fname1, "forward");
+    IncreaseVoltageStepwise ivsf = new IncreaseVoltageStepwise(dt, 0.25 / 50, 6.0, fname2, "forward");
+    IncreaseVoltageLinear ivlb = new IncreaseVoltageLinear(dt, 0.25 / 50, 6.0, fname3, "backward");
+    IncreaseVoltageStepwise ivsb = new IncreaseVoltageStepwise(dt, 0.25 / 50, 6.0, fname4, "backward");
+    DrivetrainAnalysis dca = new DrivetrainAnalysis(dt);
+    SmartDashboard.putData("Increase Voltage Linearly Forward", ivlf);
+    SmartDashboard.putData("Increase Voltage Stepwise Forward", ivsf);
+    SmartDashboard.putData("Increase Voltage Linearly Backward", ivlb);
+    SmartDashboard.putData("Increase Voltage Stepwise Backward", ivsb);
+    SmartDashboard.putData("Drivetrain Characterization Analysis", dca);
+
     dt.setDefaultCommand(new TeleopDrive(dt, oi.leftJoy, oi.rightJoy));
+    SmartDashboard.putNumber("Max Acceleration", dt.getMaxSpeed() / 1.0);
   }
 
   /**

Robot2019/src/main/java/frc/robot/RobotMap.java

@@ -104,6 +104,8 @@ private static BaseMotorController createConfiguredMotorController(int port) {
 
   private static WPI_TalonSRX createConfiguredTalon(int port) {
     WPI_TalonSRX tsrx = new WPI_TalonSRX(port);
+    ErrorCode ecDeadband;
+
     // Put all configurations for the talon motor controllers in here.
     // All values are from last year's code.
     catchError(tsrx.configNominalOutputForward(0, 10));
@@ -118,11 +120,17 @@ private static WPI_TalonSRX createConfiguredTalon(int port) {
     catchError(tsrx.configNeutralDeadband(0.001, 10));
     tsrx.setNeutralMode(NeutralMode.Brake);
 
+    ecDeadband = tsrx.configNeutralDeadband(0.001, 10);
+    if (!ecDeadband.equals(ErrorCode.OK)) {
+      throw new RuntimeException(ecDeadband.toString());
+    }
+
     return tsrx;
   }
 
   private static WPI_VictorSPX createConfiguredVictor(int port) {
     WPI_VictorSPX vspx = new WPI_VictorSPX(port);
+    ErrorCode ecDeadband;
 
     // Put all configurations for the victor motor controllers in here.
     catchError(vspx.configNominalOutputForward(0, 10));
@@ -132,6 +140,11 @@ private static WPI_VictorSPX createConfiguredVictor(int port) {
     catchError(vspx.configNeutralDeadband(0.001, 10));
     vspx.setNeutralMode(NeutralMode.Brake);
 
+    ecDeadband = vspx.configNeutralDeadband(0.001, 10);
+    if (!ecDeadband.equals(ErrorCode.OK)) {
+      throw new RuntimeException(ecDeadband.toString());
+    }
+
     return vspx;
   }