diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..7e0a6e0 --- /dev/null +++ b/.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", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "Robot2019" + }, + { + "type": "java", + "name": "Debug (Launch)-DrivetrainCharacterization", + "request": "launch", + "mainClass": "DrivetrainCharacterization" + } + ] +} \ No newline at end of file diff --git a/Robot2019/build.gradle b/Robot2019/build.gradle index abdb212..a40819e 100644 --- a/Robot2019/build.gradle +++ b/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') diff --git a/Robot2019/src/main/java/frc/robot/DrivetrainCharAnalysis.java b/Robot2019/src/main/java/frc/robot/DrivetrainCharAnalysis.java new file mode 100644 index 0000000..d2f7807 --- /dev/null +++ b/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 records = csvParser.getRecords(); + int num_values = records.size(); + List leftVelocities, rightVelocities, voltages, leftAccelerations, rightAccelerations; + leftVelocities = new ArrayList(); + rightVelocities = new ArrayList(); + voltages = new ArrayList(); + leftAccelerations = new ArrayList(); + rightAccelerations = new ArrayList(); + 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; + } + } +} \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/OI.java b/Robot2019/src/main/java/frc/robot/OI.java index c51bd2d..65fe786 100644 --- a/Robot2019/src/main/java/frc/robot/OI.java +++ b/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)); diff --git a/Robot2019/src/main/java/frc/robot/Robot.java b/Robot2019/src/main/java/frc/robot/Robot.java index ed44912..aa80507 100644 --- a/Robot2019/src/main/java/frc/robot/Robot.java +++ b/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); } /** diff --git a/Robot2019/src/main/java/frc/robot/RobotMap.java b/Robot2019/src/main/java/frc/robot/RobotMap.java index 3bc1607..c9c477c 100644 --- a/Robot2019/src/main/java/frc/robot/RobotMap.java +++ b/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; } diff --git a/Robot2019/src/main/java/frc/robot/commands/DrivetrainAnalysis.java b/Robot2019/src/main/java/frc/robot/commands/DrivetrainAnalysis.java new file mode 100644 index 0000000..654a69c --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/DrivetrainAnalysis.java @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import java.io.FileNotFoundException; +import java.io.PrintWriter; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.DrivetrainCharAnalysis; +import frc.robot.subsystems.Drivetrain; + +public class DrivetrainAnalysis extends Command { + Drivetrain dt; + String file1, file2, file3, file4, outfile; + /** + * Handles all the teleoperated driving functionality + * + * @param dt the Drivetrain object to use, passing it in is useful for testing + * purposes + */ + public DrivetrainAnalysis(Drivetrain dt) { + this.dt = dt; + file1 = "/home/lvuser/drive_char_linear_for.csv"; + file2 = "/home/lvuser/drive_char_stepwise_for.csv"; + file3 = "/home/lvuser/drive_char_linear_back.csv"; + file4 = "/home/lvuser/drive_char_stepwise_back.csv"; + outfile = "/home/lvuser/drive_char_params.csv"; + } + + @Override + protected void execute() { + try { + PrintWriter pw = new PrintWriter(outfile); + pw.close(); + } catch (FileNotFoundException f) { + f.printStackTrace(); + } + DrivetrainCharAnalysis.ordinaryLeastSquares(file1, file2, outfile); + DrivetrainCharAnalysis.ordinaryLeastSquares(file3, file4, outfile); + System.out.println("Drivetrain Characterization Analysis Successful. All data has been dumped into " + outfile); + dt.updateDrivetrainParameters(); + } + + @Override + protected boolean isFinished() { + return true; + } + + @Override + protected void end() { + } + + @Override + protected void interrupted() { + end(); + } +} \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/commands/DrivetrainCharacterization.java b/Robot2019/src/main/java/frc/robot/commands/DrivetrainCharacterization.java new file mode 100644 index 0000000..b9ffb60 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/DrivetrainCharacterization.java @@ -0,0 +1,96 @@ +package frc.robot.commands; + +import java.io.File; +import java.io.FileWriter; +import java.io.IOException; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.subsystems.Drivetrain; + +/* Command which supplies the motors with a desired voltage that increases linearly. +*/ +public class DrivetrainCharacterization extends Command { + public Drivetrain dt; + public FileWriter fw; + private String filename; + public double suppliedVoltage, voltStep, stepwiseVoltage, voltageRuntime, maxVoltage; + + public DrivetrainCharacterization(Drivetrain dt, double voltStep, double stepwiseVoltage, String filename) { + requires(dt); + this.dt = dt; + this.voltStep = voltStep; + this.stepwiseVoltage = stepwiseVoltage; + this.filename = filename; + suppliedVoltage = 0.0; + voltageRuntime = 0.0; + maxVoltage = 9.0; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + File f = new File(filename); + try { + f.createNewFile(); + fw = new FileWriter(f); + fw.write("Timestamp (s),"); + fw.write("Voltage (V),"); + fw.write("LeftMotorVelocity (inches / s),"); + fw.write("RightMotorVelocity (inches / s)\r\n"); + } catch (IOException e) { + System.out.println("Error caught creating FileWriter object: " + e); + } + + dt.setVoltageCompensation(9.0); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + try { + fw.close(); + } catch (IOException e) { + System.out.println("Cannot close FileWriter"); + } + + dt.disableVoltageCompensation(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } + + public void writeMeasuredVelocity(FileWriter fw) { + double leftMotorVelocity, rightMotorVelocity; + StringBuilder sb = new StringBuilder(); + + leftMotorVelocity = dt.getEncRate(Drivetrain.Side.LEFT); + rightMotorVelocity = dt.getEncRate(Drivetrain.Side.RIGHT); + + voltageRuntime += 0.02; // IncreaseVoltage occurs every 1/50 of a second + sb.append(String.valueOf(voltageRuntime) + ","); + sb.append(String.valueOf(suppliedVoltage) + ","); + sb.append(String.valueOf(leftMotorVelocity) + ","); + sb.append(String.valueOf(rightMotorVelocity) + "\r\n"); + + try { + fw.write(sb.toString()); + } catch (IOException e) { + System.out.println("FileWriter object cannot write StringBuilder object: " + e); + } + } +} \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/commands/IncreaseVoltageLinear.java b/Robot2019/src/main/java/frc/robot/commands/IncreaseVoltageLinear.java new file mode 100644 index 0000000..570eae0 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/IncreaseVoltageLinear.java @@ -0,0 +1,40 @@ +package frc.robot.commands; + +import frc.robot.commands.DrivetrainCharacterization; +import frc.robot.subsystems.Drivetrain; + +/* Command which supplies the motors with a desired voltage that increases linearly. +*/ +public class IncreaseVoltageLinear extends DrivetrainCharacterization { + String direction; + + public IncreaseVoltageLinear(Drivetrain dt, double voltStep, double stepwiseVoltage, String filename, String dir) { + super(dt, voltStep, stepwiseVoltage, filename); + direction = dir; + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + double throttle = suppliedVoltage / maxVoltage; + if (direction.equals("backward")) { + throttle *= -1; + } + dt.drive(throttle, throttle); + writeMeasuredVelocity(fw); + + if (suppliedVoltage + voltStep <= maxVoltage) { + suppliedVoltage += voltStep; + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + if (suppliedVoltage == maxVoltage) { + return true; + } else { + return false; + } + } +} \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/commands/IncreaseVoltageStepwise.java b/Robot2019/src/main/java/frc/robot/commands/IncreaseVoltageStepwise.java new file mode 100644 index 0000000..9244cb8 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/IncreaseVoltageStepwise.java @@ -0,0 +1,37 @@ +package frc.robot.commands; + +import frc.robot.commands.DrivetrainCharacterization; +import frc.robot.subsystems.Drivetrain; + +/* Command which supplies the motors with a desired voltage that increases linearly. +*/ +public class IncreaseVoltageStepwise extends DrivetrainCharacterization { + String direction; + + public IncreaseVoltageStepwise(Drivetrain dt, double voltStep, double stepwiseVoltage, String filename, String dir) { + super(dt, voltStep, stepwiseVoltage, filename); + direction = dir; + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + double throttle = suppliedVoltage / maxVoltage; + if (direction.equals("backward")) { + throttle *= -1; + } + dt.drive(throttle, throttle); + writeMeasuredVelocity(fw); + suppliedVoltage = stepwiseVoltage; + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + if (dt.getEncRate(Drivetrain.Side.LEFT) >= 0.75 * dt.getMaxSpeed() || dt.getEncRate(Drivetrain.Side.RIGHT) >= 0.75 * dt.getMaxSpeed()) { + return true; + } else { + return false; + } + } +} \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/commands/NormalDrive.java b/Robot2019/src/main/java/frc/robot/commands/NormalDrive.java new file mode 100644 index 0000000..f30bb46 --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/NormalDrive.java @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class NormalDrive extends Command { + protected void initialize() { + SmartDashboard.putBoolean("Characterized Drive", false); + } + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected void end() { + SmartDashboard.putBoolean("Characterized Drive", true); + } + + @Override + protected void interrupted() { + end(); + } +} diff --git a/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java b/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java index ce21b29..7d4979c 100644 --- a/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java @@ -84,7 +84,11 @@ private void arcadeDrive() { } } - dt.drive(left, right); + if (SmartDashboard.getBoolean("Characterized Drive", true)) { + charDrive(left, right); + } else { + dt.drive(left, right); + } } private void tankDrive() { @@ -103,7 +107,75 @@ private void tankDrive() { right *= SmartDashboard.getNumber("Speed Slow Ratio", 0.5); } - dt.drive(left, right); + if (SmartDashboard.getBoolean("Characterized Drive", true)) { + charDrive(left, right); + } else { + dt.drive(left, right); + } + } + + private void charDrive(double left, double right) { + double leftV, rightV; + dt.setVoltageCompensation(12.0); + double maxV = dt.getMaxVoltage(); + + double DT = 1 / 4.0; + double actualLeftVel = dt.getEncRate(Drivetrain.Side.LEFT); + double actualRightVel = dt.getEncRate(Drivetrain.Side.RIGHT); + double actualAvgVel = 0.5 * (actualLeftVel + actualRightVel); + + double desiredLeftVel = left * dt.getMaxSpeed(); + double desiredRightVel = right * dt.getMaxSpeed(); + double desiredAvgVel = 0.5 * (desiredLeftVel + desiredRightVel); + + double leftDV = desiredLeftVel - actualLeftVel; + double rightDV = desiredRightVel - actualRightVel; + double avgDV = 0.5 * (desiredAvgVel - actualAvgVel); + + double leftA = leftDV / DT; + double rightA = rightDV / DT; + double avgA = avgDV / DT; + + SmartDashboard.putNumber("Left Speed", actualLeftVel); + SmartDashboard.putNumber("Right Speed", actualRightVel); + //System.out.println("Left Speed: " + actualLeftVel + ", Right Speed: " + actualRightVel); + double maxAccel = SmartDashboard.getNumber("Max Acceleration", dt.getMaxSpeed() / 1.0); + + if (Math.abs(avgA) >= maxAccel) { // dt.getMaxSpeed() is a temporary value. The actual value will be determined + // through experimentation + leftA = Math.signum(leftA) * Math.abs(leftA / avgA) * maxAccel; + } + if (Math.abs(avgA) >= maxAccel) { + rightA = Math.signum(rightA) * Math.abs(rightA / avgA) * maxAccel; + } + + //System.out.println("Left Accel: " + leftA + ", Right Accel: " + rightA); + + if (left >= 0.0) { + leftV = dt.calculateVoltage(Drivetrain.Direction.FL, actualLeftVel, leftA); + } else { + leftV = dt.calculateVoltage(Drivetrain.Direction.BL, actualLeftVel, leftA); + } + + if (right >= 0.0) { + rightV = dt.calculateVoltage(Drivetrain.Direction.FR, actualRightVel, rightA); + } else { + rightV = dt.calculateVoltage(Drivetrain.Direction.BR, actualRightVel, rightA); + } + + if (Math.abs(leftV) >= maxV) { + leftV = maxV * Math.signum(leftV); + } + if (Math.abs(rightV) >= maxV) { + rightV = maxV * Math.signum(rightV); + } + + SmartDashboard.putNumber("Left Volts", leftV); + SmartDashboard.putNumber("Right Volts", rightV); + //System.out.println("LeftV: " + leftV + ", RightV: " + rightV); + + dt.drive(leftV / maxV, rightV / maxV); + dt.disableVoltageCompensation(); } @Override diff --git a/Robot2019/src/main/java/frc/robot/subsystems/Drivetrain.java b/Robot2019/src/main/java/frc/robot/subsystems/Drivetrain.java index b5ff08e..7e35027 100644 --- a/Robot2019/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/Robot2019/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -7,6 +7,11 @@ package frc.robot.subsystems; +import java.io.File; +import java.io.FileNotFoundException; +import java.util.Scanner; + +import com.ctre.phoenix.ErrorCode; import com.ctre.phoenix.motorcontrol.can.BaseMotorController; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.kauailabs.navx.frc.AHRS; @@ -20,10 +25,21 @@ public enum Side { LEFT, RIGHT } + public enum Direction { + FL, FR, BL, BR // Forward-Left, Forward-Right, Backward-Left, Backward-Right + } + private WPI_TalonSRX leftMaster, rightMaster; private Encoder leftEnc, rightEnc; private AHRS ahrs; + private double maxVoltage; + + private double flKV, flKA, flVI; + private double frKV, frKA, frVI; + private double blKV, blKA, blVI; + private double brKV, brKA, brVI; + public Drivetrain(WPI_TalonSRX leftMaster, BaseMotorController leftSlave1, BaseMotorController leftSlave2, WPI_TalonSRX rightMaster, BaseMotorController rightSlave1, BaseMotorController rightSlave2, Encoder leftEnc, Encoder rightEnc, AHRS ahrs) { @@ -49,8 +65,12 @@ public Drivetrain(WPI_TalonSRX leftMaster, BaseMotorController leftSlave1, BaseM rightEnc.setDistancePerPulse(pulseFraction * Math.PI * wheelDiameter); leftEnc.reset(); rightEnc.reset(); + rightEnc.setReverseDirection(true); this.ahrs = ahrs; + updateDrivetrainParameters(); + + maxVoltage = 12.0; } /** @@ -74,8 +94,7 @@ public void stop() { } public boolean isStalled() { - return leftMaster.getOutputCurrent() >= 30 || rightMaster.getOutputCurrent() >= 30; // TODO: Find value that - // actually works (test) + return leftMaster.getOutputCurrent() >= 30 || rightMaster.getOutputCurrent() >= 30; } public double getEncDist(Side type) { @@ -105,4 +124,111 @@ public double getGyroRate() { public double getGyroAngle() { return ahrs.getYaw(); } + + public double getMaxSpeed() { // Return must be adjusted in the future; + return 13 * 12; + } + + public void setMaxVoltage(double volts) { + maxVoltage = volts; + } + + public double getMaxVoltage() { + return maxVoltage; + } + + public void setVoltageCompensation(double volts) { + ErrorCode ecVoltSat = leftMaster.configVoltageCompSaturation(volts, 10); + + if (!ecVoltSat.equals(ErrorCode.OK)) { + throw new RuntimeException("Voltage Saturation Configuration could not be set"); + } + + ecVoltSat = rightMaster.configVoltageCompSaturation(volts, 10); + + if (!ecVoltSat.equals(ErrorCode.OK)) { + throw new RuntimeException("Voltage Saturation Configuration could not be set"); + } + + leftMaster.enableVoltageCompensation(true); + rightMaster.enableVoltageCompensation(true); + } + + public void disableVoltageCompensation() { + leftMaster.enableVoltageCompensation(false); + rightMaster.enableVoltageCompensation(false); + } + + public void updateDrivetrainParameters() { + try { + Scanner filereader = new Scanner(new File("/home/lvuser/drive_char_params.csv")); + String line = filereader.next(); + // Forward-Left + flKV = Double.valueOf(line.split(",")[0]); + flKA = Double.valueOf(line.split(",")[1]); + flVI = Double.valueOf(line.split(",")[2]); + System.out.println(flKV + "," + flKA + "," + flVI); + + line = filereader.next(); + // Forward-right + frKV = Double.valueOf(line.split(",")[0]); + frKA = Double.valueOf(line.split(",")[1]); + frVI = Double.valueOf(line.split(",")[2]); + System.out.println(frKV + "," + frKA + "," + frVI); + + line = filereader.next(); + // Backward-Left + blKV = Double.valueOf(line.split(",")[0]); + blKA = Double.valueOf(line.split(",")[1]); + blVI = Double.valueOf(line.split(",")[2]); + System.out.println(blKV + "," + blKA + "," + blVI); + + line = filereader.next(); + // Backward-Right + brKV = Double.valueOf(line.split(",")[0]); + brKA = Double.valueOf(line.split(",")[1]); + brVI = Double.valueOf(line.split(",")[2]); + System.out.println(brKV + "," + brKA + "," + brVI); + filereader.close(); + + // Averaging numbers because they vary so much + double avg = (flKA + frKA + blKA + brKA) / 4; + flKA = avg; + frKA = avg; + blKA = avg; + brKA = avg; + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + } + + public double calculateVoltage(Direction dir, double velocity, double acceleration) { + /*System.out.println("Velocity: " + velocity + ", Acceleration: " + acceleration); + System.out.println(flKV + "," + flKA + "," + flVI); + System.out.println(frKV + "," + frKA + "," + frVI); + System.out.println(blKV + "," + blKA + "," + blVI); + System.out.println(brKV + "," + brKA + "," + brVI);*/ + + if (dir == Direction.FL) { + return flKV * velocity + flKA * acceleration + flVI; + } else if (dir == Direction.FR) { + return frKV * velocity + frKA * acceleration + frVI; + } else if (dir == Direction.BL) { + return blKV * velocity + blKA * acceleration - blVI; + } else { + return brKV * velocity + brKA * acceleration - brVI; + } + } + + public double calculateAcceleration(Direction dir, double velocity, double voltage) { + if (dir == Direction.FL) { + return (voltage - flKV * velocity - flVI) / flKA; + } else if (dir == Direction.FR) { + return (voltage - frKV * velocity - frVI) / frKA; + } else if (dir == Direction.BL) { + return (voltage - blKV * velocity - blVI) / blKA; + } else { + return (voltage - brKV * velocity + brVI) / brKA; + } + } }