Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

artf4127: Implemented velocity PID controller

Browse Source 
Change-Id: I8c0f84422f7ca0ac5c50fddb282fb3452ee1d491
This commit is contained in:
Tyler Veness
2015-07-14 20:37:52 -07:00
parent f0e3bb5164
commit d5922bb037
43 changed files with 532 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java
View File

@@ -43,8 +43,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
private double m_error = 0.0;
private double m_result = 0.0;
private double m_period = kDefaultPeriod;
PIDSource m_pidInput;
PIDOutput m_pidOutput;
protected PIDSource m_pidInput;
protected PIDOutput m_pidOutput;
java.util.Timer m_controlLoop;
private boolean m_freed = false;
private boolean m_usingPercentTolerance;
@@ -256,21 +256,39 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
}
}
if (m_I != 0) {
double potentialIGain = (m_totalError + m_error) * m_I;
if (potentialIGain < m_maximumOutput) {
if (potentialIGain > m_minimumOutput) {
m_totalError += m_error;
if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
if (m_P != 0) {
double potentialPGain = (m_totalError + m_error) * m_P;
if (potentialPGain < m_maximumOutput) {
if (potentialPGain > m_minimumOutput) {
m_totalError += m_error;
} else {
m_totalError = m_minimumOutput / m_P;
}
} else {
m_totalError = m_minimumOutput / m_I;
m_totalError = m_maximumOutput / m_P;
}
} else {
m_totalError = m_maximumOutput / m_I;
m_result = m_P * m_totalError + m_D * m_error + m_setpoint * m_F;
}
}
else {
if (m_I != 0) {
double potentialIGain = (m_totalError + m_error) * m_I;
if (potentialIGain < m_maximumOutput) {
if (potentialIGain > m_minimumOutput) {
m_totalError += m_error;
} else {
m_totalError = m_minimumOutput / m_I;
}
} else {
m_totalError = m_maximumOutput / m_I;
}
}
m_result =
m_P * m_error + m_I * m_totalError + m_D * (m_prevInput - input) + m_setpoint * m_F;
m_result =
m_P * m_error + m_I * m_totalError + m_D * (m_prevInput - input) + m_setpoint * m_F;
}
m_prevInput = input;
if (m_result > m_maximumOutput) {
@@ -466,6 +484,24 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
return getSetpoint() - m_pidInput.pidGet();
}
/**
* Sets what type of input the PID controller will use
*$
* @param pidSource the type of input
*/
void setPIDSourceType(PIDSourceType pidSource) {
m_pidInput.setPIDSourceType(pidSource);
}
/**
* Returns the type of input the PID controller is using
*$
* @return the PID controller input type
*/
PIDSourceType getPIDSourceType() {
return m_pidInput.getPIDSourceType();
}
/**
* Set the percentage error which is considered tolerable for use with
* OnTarget. (Input of 15.0 = 15 percent)

28
wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
View File

@@ -6,6 +6,7 @@
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj;
import edu.wpi.first.wpilibj.PIDSourceType;
/**
* This interface allows for PIDController to automatically read from this
@@ -14,23 +15,22 @@ package edu.wpi.first.wpilibj;
* @author dtjones
*/
public interface PIDSource {
/**
* Set which parameter of the device you are using as a process control
* variable.
*
* @param pidSource
* An enum to select the parameter.
*/
public void setPIDSourceType(PIDSourceType pidSource);
/**
* A description for the type of output value to provide to a PIDController
* Get which parameter of the device you are using as a process control
* variable.
*
* @return the currently selected PID source parameter
*/
public static class PIDSourceParameter {
public final int value;
static final int kDistance_val = 0;
static final int kRate_val = 1;
static final int kAngle_val = 2;
public static final PIDSourceParameter kDistance = new PIDSourceParameter(kDistance_val);
public static final PIDSourceParameter kRate = new PIDSourceParameter(kRate_val);
public static final PIDSourceParameter kAngle = new PIDSourceParameter(kAngle_val);
private PIDSourceParameter(int value) {
this.value = value;
}
}
public PIDSourceType getPIDSourceType();
/**
* Get the result to use in PIDController

25
wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java Normal file
View File

@@ -0,0 +1,25 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2015. 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 edu.wpi.first.wpilibj;
/**
* A description for the type of output value to provide to a PIDController
*/
public enum PIDSourceType {
kDisplacement(0),
kRate(1);
/**
* The integer value representing this enumeration
*/
public final int value;
private PIDSourceType(int value) {
this.value = value;
}
}

6
wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
View File

@@ -10,6 +10,7 @@ package edu.wpi.first.wpilibj.command;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.tables.ITable;
@@ -37,6 +38,11 @@ public abstract class PIDCommand extends Command implements Sendable {
};
/** A source which calls {@link PIDCommand#returnPIDInput()} */
private PIDSource source = new PIDSource() {
public void setPIDSourceType(PIDSourceType pidSource) {}
public PIDSourceType getPIDSourceType() {
return PIDSourceType.kDisplacement;
}
public double pidGet() {
return returnPIDInput();

6
wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
View File

@@ -10,6 +10,7 @@ package edu.wpi.first.wpilibj.command;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.tables.ITable;
@@ -39,6 +40,11 @@ public abstract class PIDSubsystem extends Subsystem implements Sendable {
};
/** A source which calls {@link PIDCommand#returnPIDInput()} */
private PIDSource source = new PIDSource() {
public void setPIDSourceType(PIDSourceType pidSource) {}
public PIDSourceType getPIDSourceType() {
return PIDSourceType.kDisplacement;
}
public double pidGet() {
return returnPIDInput();