-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
4 changed files
with
150 additions
and
5 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
2 changes: 1 addition & 1 deletion
2
...lib/addons/sensors/DisplayAbleDevice.java → ...otlib/lib/addons/sensors/DisplayAble.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,145 @@ | ||
| package pibotlib.lib.pid; | ||
|
|
||
| import pibotlib.lib.addons.sensors.DisplayAble; | ||
| import pibotlib.lib.time.ElapseTimer; | ||
|
|
||
| public class PIDController implements DisplayAble { | ||
|
|
||
| final double kP; | ||
| final double kI; | ||
| final double kD; | ||
|
|
||
| final double iLimit; | ||
| double maxTolerance; | ||
| double minTolerance; | ||
| double target; | ||
| double maxTarget; | ||
| double minTarget; | ||
|
|
||
| volatile double error = 0; | ||
| volatile double output = 0; | ||
| volatile double errorSum = 0; | ||
| volatile double errorRate = 0; | ||
| volatile double lastError = 0; | ||
| volatile double lastTimeStamp = 0; | ||
| volatile double currentTime = 0; | ||
| volatile double sensorValue; | ||
|
|
||
| volatile boolean greaterThanMax = false; | ||
| volatile boolean lowerThanMin = false; | ||
| volatile boolean isOnTarget = false; | ||
|
|
||
| boolean autoKill; | ||
|
|
||
| ElapseTimer timer; | ||
|
|
||
| public PIDController(double kP, double kI, double kD, double iLimit, double maxTolerance, double minTolerance, double target){ | ||
| this.kP = kP; | ||
| this.kI = kI; | ||
| this.kD = kD; | ||
| this.iLimit = iLimit; | ||
| this.maxTolerance = maxTolerance; | ||
| this.minTolerance = minTolerance; | ||
| this.target = target; | ||
| minTarget = target - minTolerance; | ||
| maxTarget = target + maxTolerance; | ||
| timer = new ElapseTimer(); | ||
| } | ||
|
|
||
| public PIDController(double kP, double kI, double kD, double iLimit, double maxTolerance, double minTolerance, double target, boolean autoKill){ | ||
| this.kP = kP; | ||
| this.kI = kI; | ||
| this.kD = kD; | ||
| this.iLimit = iLimit; | ||
| this.maxTolerance = maxTolerance; | ||
| this.minTolerance = minTolerance; | ||
| this.target = target; | ||
| minTarget = target - minTolerance; | ||
| maxTarget = target + maxTolerance; | ||
| this.autoKill = autoKill; | ||
| timer = new ElapseTimer(); | ||
| } | ||
|
|
||
| public double getOutput(double sensorValue){ | ||
| timer.startTimer(); | ||
| currentTime = timer.getElapsedMilliseconds() - lastTimeStamp; | ||
| if(sensorValue > target){ | ||
| error = target - sensorValue; | ||
| greaterThanMax = true; | ||
| lowerThanMin = false; | ||
| } | ||
|
|
||
| if(sensorValue < target){ | ||
| error = target - sensorValue; | ||
| greaterThanMax = false; | ||
| lowerThanMin = true; | ||
| } | ||
|
|
||
| if(Math.abs(error) < iLimit && Math.round(error) != 0){ | ||
| errorSum += error * currentTime; | ||
| } | ||
|
|
||
| errorRate = (error - lastError)/currentTime; | ||
| lastTimeStamp = timer.getElapsedMilliseconds(); | ||
| lastError = error; | ||
|
|
||
| isOnTarget = maxTarget >= sensorValue && minTarget <= sensorValue; | ||
|
|
||
| if(isOnTarget && autoKill){ | ||
| error = 0; | ||
| errorSum = 0; | ||
| errorRate = 0; | ||
| timer.reset(); | ||
| timer.stopTimer(); | ||
| return 0; | ||
| } | ||
| return (kP * error) + (kI * errorSum) + (kD * errorRate);//only PI for now | ||
| } | ||
|
|
||
| public void pause(){ | ||
| error = 0; | ||
| errorSum = 0; | ||
| errorRate = 0; | ||
| timer.reset(); | ||
| timer.stopTimer(); | ||
| } | ||
|
|
||
| public void setTarget(double minTolerance, double maxTolerance, double target){ | ||
| this.target = target; | ||
| this.maxTolerance = maxTarget; | ||
| this.minTolerance = minTolerance; | ||
| this.maxTarget = target + maxTolerance; | ||
| this.minTarget = target - minTolerance; | ||
| } | ||
|
|
||
| public double getP(){return kP;} | ||
| public double getI(){return kI;} | ||
| public double getD(){return kD;} | ||
|
|
||
| public double getILimit(){return iLimit;} | ||
|
|
||
| public double getMaxTolerance(){return maxTolerance;} | ||
| public double getMinTolerance(){return minTolerance;} | ||
|
|
||
| public synchronized double getError(){return error;} | ||
| public synchronized double getErrorSum(){return errorSum;} | ||
|
|
||
| public synchronized double getLastTimeStamp(){return lastTimeStamp;} | ||
| public synchronized double getCurrentTime(){return currentTime;} | ||
|
|
||
| public synchronized double getSensorValue(){return sensorValue;} | ||
|
|
||
| public synchronized boolean isGreaterThanMax(){return greaterThanMax;} | ||
| public synchronized boolean isLowerThanMin(){return lowerThanMin;} | ||
| public synchronized boolean isOnTarget(){return isOnTarget;} | ||
|
|
||
| @Override | ||
| public String getValueToString() { | ||
| return null; | ||
| } | ||
|
|
||
| @Override | ||
| public String getName() { | ||
| return "PID Controller"; | ||
| } | ||
| } |