diff --git a/.gitignore b/.gitignore
index 74f34cb1..62e84356 100644
--- a/.gitignore
+++ b/.gitignore
@@ -153,5 +153,7 @@ gradle-app.setting
# Cache of project
.gradletasknamecache
+.idea/jarRepositories.xml
+
# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898
# gradle/wrapper/gradle-wrapper.properties
diff --git a/.idea/inspectionProfiles/Project_Default.xml b/.idea/inspectionProfiles/Project_Default.xml
deleted file mode 100644
index c2ee3df3..00000000
--- a/.idea/inspectionProfiles/Project_Default.xml
+++ /dev/null
@@ -1,37 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml
new file mode 100644
index 00000000..dd4c951e
--- /dev/null
+++ b/.idea/inspectionProfiles/profiles_settings.xml
@@ -0,0 +1,7 @@
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/runConfigurations/Deploy_PIDTuningRobot.xml b/.idea/runConfigurations/Deploy_PIDTuningRobot.xml
new file mode 100644
index 00000000..85f7c3bc
--- /dev/null
+++ b/.idea/runConfigurations/Deploy_PIDTuningRobot.xml
@@ -0,0 +1,21 @@
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+
+
\ No newline at end of file
diff --git a/lib/src/main/java/org/team1540/base/util/robots/PIDTuningRobot.java b/lib/src/main/java/org/team1540/base/util/robots/PIDTuningRobot.java
new file mode 100644
index 00000000..2ea1b972
--- /dev/null
+++ b/lib/src/main/java/org/team1540/base/util/robots/PIDTuningRobot.java
@@ -0,0 +1,172 @@
+package org.team1540.base.util.robots;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.jetbrains.annotations.NotNull;
+import org.jetbrains.annotations.Nullable;
+import org.team1540.base.Utilities;
+import org.team1540.base.preferencemanager.Preference;
+import org.team1540.base.preferencemanager.PreferenceManager;
+import org.team1540.base.util.SimpleCommand;
+import org.team1540.base.wrappers.ChickenTalon;
+
+/**
+ * Robot class to tune a Motion Magic PID controller.
+ *
+ * To change the motors to be tuned, change the preference values and then restart the robot code to
+ * allow the values to take effect. To disable a motor, set its motor ID to -1. Motor 1 will be
+ * configured as the master Talon and motors 2, 3, and 4 will be slaved to it in follower mode.
+ */
+public class PIDTuningRobot extends IterativeRobot {
+
+ @Preference(value = "P", persistent = false)
+ public double p;
+ @Preference(value = "I", persistent = false)
+ public double i;
+ @Preference(value = "D", persistent = false)
+ public double d;
+ @Preference(value = "F", persistent = false)
+ public double f;
+ @Preference(value = "I-Zone", persistent = false)
+ public int iZone;
+ @Preference(value = "Max I-Accum", persistent = false)
+ public int maxIAccum;
+ @Preference(value = "Max Acceleration", persistent = false)
+ public int maxAccel;
+ @Preference(value = "Max Velocity", persistent = false)
+ public int maxVel;
+ @Preference(value = "Setpoint", persistent = false)
+ public double setpoint;
+ @Preference(value = "Invert Sensor", persistent = false)
+ public boolean invertSensor;
+ @Preference(value = "Invert Output", persistent = false)
+ public boolean invertOutput;
+ @Preference(value = "Motor 1 ID", persistent = false)
+ public int motor1ID;
+ @Preference(value = "Motor 2 ID", persistent = false)
+ public int motor2ID;
+ @Preference(value = "Motor 3 ID", persistent = false)
+ public int motor3ID;
+ @Preference(value = "Motor 4 ID", persistent = false)
+ public int motor4ID;
+ @Preference(value = "Enable PID", persistent = false)
+ public boolean enablePID;
+
+ @Nullable
+ private ChickenTalon motor1;
+ @Nullable
+ private ChickenTalon motor2;
+ @Nullable
+ private ChickenTalon motor3;
+ @Nullable
+ private ChickenTalon motor4;
+
+ @NotNull
+ private Joystick joystick = new Joystick(0);
+
+ @NotNull
+ private SendableChooser controlModeChooser = new SendableChooser<>();
+
+ @Override
+ public void robotInit() {
+ System.out.println("Initializing PID Tuner Robot");
+ System.out.println(
+ "To change the motors to be tuned, change the preference values and then run the Reset command to\n"
+ + " * allow the values to take effect. To disable a motor, set its motor ID to -1. Motor 1 will be \n"
+ + " * configured as the master Talon and motors 2, 3, and 4 will be slaved to it in follower mode.");
+ PreferenceManager.getInstance().add(this);
+ Scheduler.getInstance().run(); // allow the PreferenceManager to update
+
+ controlModeChooser.addDefault("Position", ControlMode.Position);
+ controlModeChooser.addDefault("Velocity", ControlMode.Velocity);
+ controlModeChooser.addDefault("MotionMagic", ControlMode.MotionMagic);
+
+ SmartDashboard.putData("Control Mode Chooser", controlModeChooser);
+ Command reset = new SimpleCommand("Reset", () -> {
+ if (motor1ID != -1) {
+ motor1 = new ChickenTalon(motor1ID);
+ } else {
+ System.err.println("Motor 1 must be set!");
+ return;
+ }
+ if (motor2ID != -1) {
+ motor2 = new ChickenTalon(motor2ID);
+ motor2.set(ControlMode.Follower, motor1.getDeviceID());
+ }
+ if (motor3ID != -1) {
+ motor3 = new ChickenTalon(motor3ID);
+ motor3.set(ControlMode.Follower, motor1.getDeviceID());
+ }
+ if (motor4ID != -1) {
+ motor4 = new ChickenTalon(motor4ID);
+ motor4.set(ControlMode.Follower, motor1.getDeviceID());
+ }
+
+ for (ChickenTalon motor : new ChickenTalon[]{motor1, motor2, motor3, motor4}) {
+ if (motor != null) {
+ motor.configClosedloopRamp(0);
+ motor.configOpenloopRamp(0);
+ motor.configPeakOutputForward(1);
+ motor.configPeakOutputReverse(-1);
+ motor.enableCurrentLimit(false);
+ }
+ }
+ });
+ reset.setRunWhenDisabled(true);
+ reset.start();
+ SmartDashboard.putData(reset);
+
+ Command zero = new SimpleCommand("Zero Position", () -> {
+ if (motor1 != null) {
+ motor1.setSelectedSensorPosition(0);
+ }
+ });
+ zero.setRunWhenDisabled(true);
+ SmartDashboard.putData(zero);
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ if (motor1 != null) {
+ motor1.config_kP(0, p);
+ motor1.config_kI(0, i);
+ motor1.config_kD(0, d);
+ motor1.config_kF(0, f);
+ motor1.config_IntegralZone(0, iZone);
+ motor1.configMaxIntegralAccumulator(0, maxIAccum);
+ motor1.configMotionCruiseVelocity(maxVel);
+ motor1.configMotionAcceleration(maxAccel);
+ if (enablePID) {
+ motor1.set(controlModeChooser.getSelected(), setpoint);
+ } else {
+ motor1
+ .set(ControlMode.PercentOutput, Utilities.processDeadzone(joystick.getRawAxis(1), 0.1));
+ }
+ }
+ }
+
+ @Override
+ public void robotPeriodic() {
+ Scheduler.getInstance().run();
+ for (ChickenTalon motor : new ChickenTalon[]{motor1, motor2, motor3, motor4}) {
+ if (motor != null) {
+ motor.setInverted(invertOutput);
+ }
+ }
+ if (motor1 != null) {
+ motor1.setSensorPhase(invertSensor);
+ SmartDashboard.putNumber("Position", motor1.getSelectedSensorPosition());
+ SmartDashboard.putNumber("Velocity", motor1.getSelectedSensorVelocity());
+ SmartDashboard.putNumber("Throttle", motor1.getMotorOutputPercent());
+ SmartDashboard.putNumber("Current", motor1.getOutputCurrent());
+ SmartDashboard.putNumber("Error", motor1.getClosedLoopError());
+ SmartDashboard.putNumber("Target", motor1.getClosedLoopTarget(0));
+ SmartDashboard.putNumber("Integral Accumulator", motor1.getIntegralAccumulator());
+ }
+ }
+}
diff --git a/test/build.gradle b/test/build.gradle
index f10a9202..1745e1e3 100644
--- a/test/build.gradle
+++ b/test/build.gradle
@@ -6,6 +6,10 @@ plugins {
id "jaci.openrio.gradle.GradleRIO" version "2018.03.06"
}
+wpi {
+ ctreVersion = "5.3.1.0"
+}
+
dependencies {
compile project(":lib")
compile wpilib()