Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Made convenience functions for tuning a CustomPIDController on SmartDashboard #197

Open
wants to merge 11 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Binary file added .gradle/5.0/fileChanges/last-build.bin
Binary file not shown.
Binary file added .gradle/5.0/fileContent/fileContent.lock
Binary file not shown.
Binary file added .gradle/5.0/fileHashes/fileHashes.bin
Binary file not shown.
Binary file added .gradle/5.0/fileHashes/fileHashes.lock
Binary file not shown.
Empty file added .gradle/5.0/gc.properties
Empty file.
Binary file added .gradle/5.0/taskHistory/taskHistory.bin
Binary file not shown.
Binary file added .gradle/5.0/taskHistory/taskHistory.lock
Binary file not shown.
Binary file added .gradle/buildOutputCleanup/buildOutputCleanup.lock
Binary file not shown.
2 changes: 2 additions & 0 deletions .gradle/buildOutputCleanup/cache.properties
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
#Sun Jan 06 15:07:52 PST 2019
gradle.version=5.0
Binary file added .gradle/buildOutputCleanup/outputFiles.bin
Binary file not shown.
Empty file added .gradle/vcs-1/gc.properties
Empty file.
17 changes: 17 additions & 0 deletions .project
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>standard</name>
<comment>Project standard created by Buildship.</comment>
<projects>
</projects>
<buildSpec>
<buildCommand>
<name>org.eclipse.buildship.core.gradleprojectbuilder</name>
<arguments>
</arguments>
</buildCommand>
</buildSpec>
<natures>
<nature>org.eclipse.buildship.core.gradleprojectnature</nature>
</natures>
</projectDescription>
2 changes: 2 additions & 0 deletions .settings/org.eclipse.buildship.core.prefs
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
connection.project.dir=
eclipse.preferences.version=1
6 changes: 6 additions & 0 deletions .wpilib/wpilib_preferences.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
{
"currentLanguage": "none",
"enableCppIntellisense": false,
"projectYear": "none",
"teamNumber": 4904
}
61 changes: 61 additions & 0 deletions build.gradle
Original file line number Diff line number Diff line change
@@ -0,0 +1,61 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2019.1.1"
}

def ROBOT_MAIN_CLASS = "frc.robot.Main"

// Define my targets (RoboRIO) and artifacts (deployable files)
// This is added by GradleRIO's backing project EmbeddedTools.
deploy {
targets {
roboRIO("roborio") {
// Team number is loaded either from the .wpilib/wpilib_preferences.json
// or from command line. If not found an exception will be thrown.
// You can use getTeamOrDefault(team) instead of getTeamNumber if you
// want to store a team number in this file.
team = frc.getTeamNumber()
}
}
artifacts {
frcJavaArtifact('frcJava') {
targets << "roborio"
// Debug can be overridden by command line, for use with VSCode
debug = frc.getDebugOrDefault(false)
}
// Built in artifact to deploy arbitrary files to the roboRIO.
fileTreeArtifact('frcStaticFileDeploy') {
// The directory below is the local directory to deploy
files = fileTree(dir: 'src/main/deploy')
// Deploy to RoboRIO target, into /home/lvuser/deploy
targets << "roborio"
directory = '/home/lvuser/deploy'
}
}
}

// Set this to true to enable desktop support.
def includeDesktopSupport = false

// Maven central needed for JUnit
repositories {
mavenCentral()
}

// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
// Also defines JUnit 4.
dependencies {
compile wpi.deps.wpilib()
compile wpi.deps.vendor.java()
nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio)
nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop)
testCompile 'junit:junit:4.12'
}

// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
// in order to make them all available at runtime. Also adding the manifest so WPILib
// knows where to look for our Robot Class.
jar {
from { configurations.compile.collect { it.isDirectory() ? it : zipTree(it) } }
manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
}
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
3 changes: 3 additions & 0 deletions build/tmp/jar/MANIFEST.MF
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
Manifest-Version: 1.0
Main-Class: frc.robot.Main

Binary file added build/tmp/jniExtractDir/libcscore.dylib
Binary file not shown.
Binary file added build/tmp/jniExtractDir/libcscorejni.dylib
Binary file not shown.
Binary file added build/tmp/jniExtractDir/libntcore.dylib
Binary file not shown.
Binary file added build/tmp/jniExtractDir/libntcorejni.dylib
Binary file not shown.
Binary file not shown.
Binary file added build/tmp/jniExtractDir/libopencv_core.3.4.dylib
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file added build/tmp/jniExtractDir/libopencv_java344.dylib
Binary file not shown.
Binary file added build/tmp/jniExtractDir/libopencv_ml.3.4.dylib
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file added build/tmp/jniExtractDir/libwpiHal.dylib
Binary file not shown.
Binary file added build/tmp/jniExtractDir/libwpiHaljni.dylib
Binary file not shown.
Binary file added build/tmp/jniExtractDir/libwpiutil.dylib
Binary file not shown.
118 changes: 118 additions & 0 deletions custom/motioncontrollers/BangBangController.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException;
import org.usfirst.frc4904.standard.custom.sensors.PIDSensor;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
* A bang bang controller.
Expand All @@ -18,6 +19,7 @@ public class BangBangController extends MotionController {
protected double A;
protected double F;
protected double threshold;
protected static final String DEFAULT_SMARTDASHBOARD_PREFIX = "!!";
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If this actually works, I think it is great. I would watch out for improper nonalphanumeric character parsing in NetworkTables though.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why did we change it to this? It's pretty jenk

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This did not exist before. It is for the bang (!) bang (!) controller. We should make a shebang controller at some point.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As funny as that is it should probably be BangBang or BANGBANG


/**
* BangBang controller
Expand Down Expand Up @@ -115,6 +117,60 @@ public BangBangController(PIDSource source, double A, double F) {
this(source, A, F, Double.MIN_VALUE);
}

/**
* @return
* The current A value
*/
public double getA() {
return A;
}

/**
* @return
* The current F (feed forward) value
*/
public double getF() {
return F;
}

/**
* @return
* The current D value
*/
public double getThreshold() {
return threshold;
}

/**
* Set the value of the A constant
*
* @param the
* desired A value
*/
public void setA(double A) {
this.A = A;
}

/**
* Set the value of the F (feed forward) constant
*
* @param the
* desired F value
*/
public void setF(double F) {
this.F = F;
}

/**
* Set the value of the bang-bang threshold
*
* @param the
* desired threshold value
*/
public void setThreshold(double threshold) {
this.threshold = threshold;
}

/**
* Sets the stored error value to zero (0)
*/
Expand Down Expand Up @@ -188,4 +244,66 @@ public double get() {
public double getError() {
return error;
}

/**
* Put the PID controller to SmartDashboard for tuning.
* Puts the error, setpoint, sensor value, and output.
* This method adds a very small random value to everything
* so that graphs show properly on SmartDashboard.
*
* @param prefix
* The prefix to use when putting things on SmartDashboard.
*/
@Override
public void putToSmartDashboard(String prefix) {
double noise = (Math.random() - 0.5) * 0.0000001; // Generate very small noise centered at zero
SmartDashboard.putNumber(prefix + "_Error", getError() + noise);
SmartDashboard.putNumber(prefix + "_Setpoint", getSetpoint() + noise);
SmartDashboard.putNumber(prefix + "_Sensor", getSensorValue() + noise);
SmartDashboard.putNumber(prefix + "_Output", get() + noise);
}

/**
* Put the PID controller to SmartDashboard for tuning.
* Puts the error, setpoint, sensor value, and output.
* This method adds a very small random value to everything
* so that graphs show properly on SmartDashboard.
*/
@Override
public void putToSmartDashboard() {
putToSmartDashboard(BangBangController.DEFAULT_SMARTDASHBOARD_PREFIX);
}

/**
* Update the PID controller from SmartDashboard for tuning.
* Gets the P, I, D, and F constants from SmartDashboard,
* or puts the current ones there if there's no constants.
*
* @param prefix
* The prefix to use when putting things on SmartDashboard.
*/
@Override
public void updateFromSmartDashboard(String prefix) {
// If we're missing any constants on SmartDashboard, put the current ones
if (!(SmartDashboard.containsKey(prefix + "_A") && SmartDashboard.containsKey(prefix + "_F")
&& SmartDashboard.containsKey(prefix + "_threshold"))) {
SmartDashboard.putNumber(prefix + "_A", getA());
SmartDashboard.putNumber(prefix + "_F", getF());
SmartDashboard.putNumber(prefix + "_threshold", getThreshold());
return;
}
setA(SmartDashboard.getNumber(prefix + "_A", getA()));
setF(SmartDashboard.getNumber(prefix + "_F", getF()));
setThreshold(SmartDashboard.getNumber(prefix + "_threshold", getThreshold()));
}

/**
* Update the PID controller from SmartDashboard for tuning.
* Gets the P, I, D, and F constants from SmartDashboard,
* or puts the current ones there if there's no constants.
*/
@Override
public void updateFromSmartDashboard() {
updateFromSmartDashboard(BangBangController.DEFAULT_SMARTDASHBOARD_PREFIX);
}
}
67 changes: 67 additions & 0 deletions custom/motioncontrollers/CustomPIDController.java
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
import org.usfirst.frc4904.standard.custom.sensors.PIDSensor;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.hal.util.BoundaryException;

/**
Expand All @@ -28,6 +29,7 @@ public class CustomPIDController extends MotionController {
protected double lastErrorDerivative;
protected double derivativeTolerance;
protected double minimumNominalOutput = 0.0;
protected static final String DEFAULT_SMARTDASHBOARD_PREFIX = "PID";

/**
* An extremely basic PID controller.
Expand Down Expand Up @@ -421,12 +423,77 @@ public double get() {
}
}

/**
* Put the PID controller to SmartDashboard for tuning.
* Puts the error, setpoint, sensor value, and output.
* This method adds a very small random value to everything
* so that graphs show properly on SmartDashboard.
*
* @param prefix
* The prefix to use when putting things on SmartDashboard.
*/
@Override
public void putToSmartDashboard(String prefix) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This should be added as an abstract function to MotionController (same for the other functions).

double noise = (Math.random() - 0.5) * 0.0000001; // Generate very small noise centered at zero
SmartDashboard.putNumber(prefix + "_Error", getError() + noise);
SmartDashboard.putNumber(prefix + "_Setpoint", getSetpoint() + noise);
SmartDashboard.putNumber(prefix + "_Sensor", getSensorValue() + noise);
SmartDashboard.putNumber(prefix + "_Output", get() + noise);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also, did we decide that the mutating effects of calling get() (of the I and D) calculations are not big enough to matter?

Copy link
Contributor

@carturn carturn Mar 19, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we have decided to fix that in the future. It should not make too much of a difference, as it does calculate time deltas every time.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Cool -- and the sensor updates fast enough that each few ticks will probably have a different value.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That is on the list to be fixed. (In fact I have a list of PID changes we need to make that various programmers will be doing before SVR)

}

/**
* Put the PID controller to SmartDashboard for tuning.
* Puts the error, setpoint, sensor value, and output.
* This method adds a very small random value to everything
* so that graphs show properly on SmartDashboard.
*/
@Override
public void putToSmartDashboard() {
putToSmartDashboard(CustomPIDController.DEFAULT_SMARTDASHBOARD_PREFIX);
}

/**
* Update the PID controller from SmartDashboard for tuning.
* Gets the P, I, D, and F constants from SmartDashboard,
* or puts the current ones there if there's no constants.
*
* @param prefix
* The prefix to use when putting things on SmartDashboard.
*/
@Override
public void updateFromSmartDashboard(String prefix) {
// If we're missing any constants on SmartDashboard, put the current ones
if (!(SmartDashboard.containsKey(prefix + "_P") && SmartDashboard.containsKey(prefix + "_I")
&& SmartDashboard.containsKey(prefix + "_D") && SmartDashboard.containsKey(prefix + "_F"))) {
SmartDashboard.putNumber(prefix + "_P", getP());
SmartDashboard.putNumber(prefix + "_I", getI());
SmartDashboard.putNumber(prefix + "_D", getD());
SmartDashboard.putNumber(prefix + "_F", getF());
return;
}
setPIDF(SmartDashboard.getNumber(prefix + "_P", getP()),
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do you want to use the two-argument version with a default value? We've done this in the past while testing (with default 0).

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this is a good way of doing it, it will default to the previous P/I/D/F value.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That is what I'm doing...?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah yes -- I have... room to improve.

SmartDashboard.getNumber(prefix + "_I", getI()),
SmartDashboard.getNumber(prefix + "_D", getD()),
SmartDashboard.getNumber(prefix + "_F", getF()));
}

/**
* Update the PID controller from SmartDashboard for tuning.
* Gets the P, I, D, and F constants from SmartDashboard,
* or puts the current ones there if there's no constants.
*/
@Override
public void updateFromSmartDashboard() {
updateFromSmartDashboard(CustomPIDController.DEFAULT_SMARTDASHBOARD_PREFIX);
}

public boolean derivativeOnTarget() {
return derivativeTolerance == 0 || Math.abs(lastErrorDerivative) < derivativeTolerance;
}

@Override
public boolean onTarget() {
return super.onTarget() && derivativeOnTarget();

}
}
43 changes: 43 additions & 0 deletions custom/motioncontrollers/MotionController.java
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ public abstract class MotionController {
protected Exception sensorException;
private volatile boolean justReset;
private final Object lock = new Object();
protected static final String DEFAULT_SMARTDASHBOARD_PREFIX = "MC";

/**
* A MotionController modifies an output using a sensor
Expand Down Expand Up @@ -397,4 +398,46 @@ public void run() {
}
}
}

/**
* Put the controller constants to SmartDashboard for tuning.
* as well as the error, setpoint, sensor value, and output.
* This method adds a very small random value to everything
* so that graphs show properly on SmartDashboard.
*
* @param prefix
* The prefix to use when putting things on SmartDashboard.
*/
public abstract void putToSmartDashboard(String prefix);

/**
* Put the controller constants to SmartDashboard for tuning.
* as well as the error, setpoint, sensor value, and output.
* This method adds a very small random value to everything
* so that graphs show properly on SmartDashboard.
*
* Uses the default prefix {@value #DEFAULT_SMARTDASHBOARD_PREFIX}
*/
public void putToSmartDashboard() {
putToSmartDashboard(MotionController.DEFAULT_SMARTDASHBOARD_PREFIX);
}

/**
* Update the controller constants from SmartDashboard for tuning.
* Gets new constants from SmartDashboard, defaulting to the current ones.
*
* @param prefix
* The prefix to use when putting things on SmartDashboard.
*/
public abstract void updateFromSmartDashboard(String prefix);

/**
* Update the controller constants from SmartDashboard for tuning.
* Gets new constants from SmartDashboard, defaulting to the current ones.
*
* Uses the default prefix {@value #DEFAULT_SMARTDASHBOARD_PREFIX}
*/
public void updateFromSmartDashboard() {
updateFromSmartDashboard(MotionController.DEFAULT_SMARTDASHBOARD_PREFIX);
}
}
5 changes: 5 additions & 0 deletions gradle/wrapper/gradle-wrapper.properties
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
distributionBase=GRADLE_USER_HOME
distributionPath=permwrapper/dists
distributionUrl=https\://services.gradle.org/distributions/gradle-5.0-bin.zip
zipStoreBase=GRADLE_USER_HOME
zipStorePath=permwrapper/dists
Loading