Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge changes I8a9cf402,I8c0f8442

Browse Source 
* changes:
  Tuned test constants for VelocityPID.
  artf4127: Implemented velocity PID controller
This commit is contained in:
Brad Miller (WPI)
2015-08-13 11:15:01 -07:00
committed by Gerrit Code Review
parent 92edf35b41 cd4ebbd8a0
commit 4f8c1dff2f
43 changed files with 649 additions and 246 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
wpilibc/wpilibC++/include/Commands/PIDSubsystem.h
View File

@@ -52,6 +52,7 @@ class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource {
void SetOutputRange(float minimumOutput, float maximumOutput);
double GetSetpoint();
double GetPosition();
double GetRate();
virtual void SetAbsoluteTolerance(float absValue);
virtual void SetPercentTolerance(float percent);

36
wpilibc/wpilibC++/include/PIDController.h
View File

@@ -10,13 +10,16 @@
#include "Controller.h"
#include "LiveWindow/LiveWindow.h"
#include "PIDInterface.h"
#include "HAL/cpp/priority_mutex.h"
#include "PIDSource.h"
#include "Notifier.h"
#include "HAL/cpp/priority_mutex.h"
#include <memory>
#include <atomic>
#include <queue>
class PIDOutput;
class PIDSource;
/**
* Class implements a PID Control Loop.
@@ -53,10 +56,15 @@ class PIDController : public LiveWindowSendable,
virtual double GetSetpoint() const override;
virtual float GetError() const;
virtual float GetAvgError() const;
virtual void SetPIDSourceType(PIDSourceType pidSource);
virtual PIDSourceType GetPIDSourceType() const;
virtual void SetTolerance(float percent);
virtual void SetAbsoluteTolerance(float absValue);
virtual void SetPercentTolerance(float percentValue);
virtual void SetToleranceBuffer(unsigned buf = 1);
virtual bool OnTarget() const;
virtual void Enable() override;
@@ -67,6 +75,13 @@ class PIDController : public LiveWindowSendable,
virtual void InitTable(std::shared_ptr<ITable> table) override;
protected:
PIDSource *m_pidInput;
PIDOutput *m_pidOutput;
std::shared_ptr<ITable> m_table;
virtual void Calculate();
private:
float m_P; // factor for "proportional" control
float m_I; // factor for "integral" control
@@ -86,17 +101,20 @@ class PIDController : public LiveWindowSendable,
kPercentTolerance,
kNoTolerance
} m_toleranceType = kNoTolerance;
float m_tolerance = 0.05; // the percetage or absolute error that is considered on
// target
// the percetage or absolute error that is considered on target.
float m_tolerance = 0.05;
float m_setpoint = 0;
float m_error;
float m_result = 0;
float m_period;
mutable priority_mutex m_mutex;
// Length of buffer for averaging for tolerances.
std::atomic<unsigned> m_bufLength{1};
std::queue<double> m_buf;
double m_bufTotal = 0;
PIDSource *m_pidInput;
PIDOutput *m_pidOutput;
mutable priority_mutex m_mutex;
std::unique_ptr<Notifier> m_controlLoop;
@@ -111,8 +129,4 @@ class PIDController : public LiveWindowSendable,
virtual void UpdateTable() override;
virtual void StartLiveWindowMode() override;
virtual void StopLiveWindowMode() override;
protected:
std::shared_ptr<ITable> m_table = nullptr;
virtual void Calculate();
};

8
wpilibc/wpilibC++/include/PIDSource.h
View File

@@ -6,6 +6,8 @@
/*----------------------------------------------------------------------------*/
#pragma once
enum class PIDSourceType { kDisplacement, kRate };
/**
* PIDSource interface is a generic sensor source for the PID class.
* All sensors that can be used with the PID class will implement the PIDSource
@@ -14,6 +16,10 @@
*/
class PIDSource {
public:
enum PIDSourceParameter { kDistance, kRate, kAngle };
virtual void SetPIDSourceType(PIDSourceType pidSource);
PIDSourceType GetPIDSourceType() const;
virtual double PIDGet() const = 0;
protected:
PIDSourceType m_pidSource = PIDSourceType::kDisplacement;
};

3
wpilibc/wpilibC++/include/interfaces/Potentiometer.h
View File

@@ -16,12 +16,15 @@
class Potentiometer : public PIDSource {
public:
virtual ~Potentiometer() = default;
/**
* Common interface for getting the current value of a potentiometer.
*
* @return The current set speed. Value is between -1.0 and 1.0.
*/
virtual double Get() const = 0;
virtual void SetPIDSourceType(PIDSourceType pidSource) override;
};
#endif

6
wpilibc/wpilibC++/src/Commands/PIDSubsystem.cpp
View File

@@ -206,6 +206,12 @@ bool PIDSubsystem::OnTarget() const { return m_controller->OnTarget(); }
*/
double PIDSubsystem::GetPosition() { return ReturnPIDInput(); }
/**
* Returns the current rate
* @return the current rate
*/
double PIDSubsystem::GetRate() { return ReturnPIDInput(); }
void PIDSubsystem::PIDWrite(float output) { UsePIDOutput(output); }
double PIDSubsystem::PIDGet() const { return ReturnPIDInput(); }

21
wpilibc/wpilibC++/src/PIDSource.cpp Normal file
View File

@@ -0,0 +1,21 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. 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 $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "PIDSource.h"
/**
* Set which parameter you are using as a process control variable.
*
* @param pidSource An enum to select the parameter.
*/
void PIDSource::SetPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
PIDSourceType PIDSource::GetPIDSourceType() const {
return m_pidSource;
}

15
wpilibc/wpilibC++/src/interfaces/Potentiometer.cpp Normal file
View File

@@ -0,0 +1,15 @@
/*----------------------------------------------------------------------------*/
/* 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 $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include <interfaces/Potentiometer.h>
#include <Utility.h>
void Potentiometer::SetPIDSourceType(PIDSourceType pidSource) {
if (wpi_assert(pidSource == PIDSourceType::kDisplacement)) {
m_pidSource = pidSource;
}
}

3
wpilibc/wpilibC++Devices/include/Encoder.h
View File

@@ -83,7 +83,6 @@ class Encoder : public SensorBase,
void SetReverseDirection(bool reverseDirection);
void SetSamplesToAverage(int samplesToAverage);
int GetSamplesToAverage() const;
void SetPIDSourceParameter(PIDSourceParameter pidSource);
double PIDGet() const override;
void SetIndexSource(uint32_t channel, IndexingType type = kResetOnRisingEdge);
@@ -115,8 +114,6 @@ class Encoder : public SensorBase,
nullptr; // Counter object for 1x and 2x encoding
EncodingType m_encodingType; // Encoding type
int32_t m_encodingScale; // 1x, 2x, or 4x, per the encodingType
PIDSourceParameter
m_pidSource = kDistance; // Encoder parameter that sources a PID controller
std::shared_ptr<ITable> m_table = nullptr;
};

2
wpilibc/wpilibC++Devices/include/Gyro.h
View File

@@ -49,7 +49,6 @@ class Gyro : public SensorBase, public PIDSource, public LiveWindowSendable {
virtual double GetRate() const;
void SetSensitivity(float voltsPerDegreePerSecond);
void SetDeadband(float volts);
void SetPIDSourceParameter(PIDSourceParameter pidSource);
virtual void Reset();
void InitGyro();
@@ -70,7 +69,6 @@ class Gyro : public SensorBase, public PIDSource, public LiveWindowSendable {
float m_voltsPerDegreePerSecond;
float m_offset;
uint32_t m_center;
PIDSourceParameter m_pidSource;
std::shared_ptr<ITable> m_table = nullptr;
};

1
wpilibc/wpilibC++Devices/include/Ultrasonic.h
View File

@@ -70,6 +70,7 @@ class Ultrasonic : public SensorBase,
void SetEnabled(bool enable) { m_enabled = enable; }
double PIDGet() const override;
void SetPIDSourceType(PIDSourceType pidSource) override;
void SetDistanceUnits(DistanceUnit units);
DistanceUnit GetDistanceUnits() const;

17
wpilibc/wpilibC++Devices/src/Encoder.cpp
View File

@@ -475,17 +475,6 @@ int Encoder::GetSamplesToAverage() const {
return result;
}
/**
* Set which parameter of the encoder you are using as a process control
* variable.
*
* @param pidSource An enum to select the parameter.
*/
void Encoder::SetPIDSourceParameter(PIDSourceParameter pidSource) {
if (StatusIsFatal()) return;
m_pidSource = pidSource;
}
/**
* Implement the PIDSource interface.
*
@@ -493,10 +482,10 @@ void Encoder::SetPIDSourceParameter(PIDSourceParameter pidSource) {
*/
double Encoder::PIDGet() const {
if (StatusIsFatal()) return 0.0;
switch (m_pidSource) {
case kDistance:
switch (GetPIDSourceType()) {
case PIDSourceType::kDisplacement:
return GetDistance();
case kRate:
case PIDSourceType::kRate:
return GetRate();
default:
return 0.0;

20
wpilibc/wpilibC++Devices/src/Gyro.cpp
View File

@@ -62,7 +62,7 @@ void Gyro::InitGyro() {
SetDeadband(0.0f);
SetPIDSourceParameter(kAngle);
SetPIDSourceType(PIDSourceType::kDisplacement);
HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
LiveWindow::GetInstance().AddSensor("Gyro", m_analog->GetChannel(), this);
@@ -189,27 +189,17 @@ void Gyro::SetDeadband(float volts) {
m_analog->SetAccumulatorDeadband(deadband);
}
/**
* Sets the type of output to use with the WPILib PID class
* The gyro supports using either rate or angle for PID calculations
*/
void Gyro::SetPIDSourceParameter(PIDSourceParameter pidSource) {
if (pidSource == 0 || pidSource > 2)
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Gyro pidSource");
m_pidSource = pidSource;
}
/**
* Get the PIDOutput for the PIDSource base object. Can be set to return
* angle or rate using SetPIDSourceParameter(). Defaults to angle.
* angle or rate using SetPIDSourceType(). Defaults to angle.
*
* @return The PIDOutput (angle or rate, defaults to angle)
*/
double Gyro::PIDGet() const {
switch (m_pidSource) {
case kRate:
switch (GetPIDSourceType()) {
case PIDSourceType::kRate:
return GetRate();
case kAngle:
case PIDSourceType::kDisplacement:
return GetAngle();
default:
return 0;

107
wpilibc/wpilibC++Devices/src/PIDController.cpp
View File

@@ -127,20 +127,37 @@ void PIDController::Calculate() {
}
}
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;
if (m_pidInput->GetPIDSourceType() == 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_maximumOutput / m_P;
}
}
}
m_result = m_P * m_error + m_I * m_totalError +
m_D * (m_prevInput - input) + m_setpoint * m_F;
m_result = m_D * m_error + m_P * m_totalError + 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_prevInput = input;
if (m_result > m_maximumOutput)
@@ -152,6 +169,15 @@ void PIDController::Calculate() {
result = m_result;
pidOutput->PIDWrite(result);
// Update the buffer.
m_buf.push(m_error);
m_bufTotal += m_error;
// Remove old elements when buffer is full.
if (m_buf.size() > m_bufLength) {
m_bufTotal -= m_buf.front();
m_buf.pop();
}
}
}
}
@@ -293,6 +319,7 @@ void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) {
/**
* Set the setpoint for the PIDController
* Clears the queue for GetAvgError().
* @param setpoint the desired setpoint
*/
void PIDController::SetSetpoint(float setpoint) {
@@ -308,6 +335,9 @@ void PIDController::SetSetpoint(float setpoint) {
} else {
m_setpoint = setpoint;
}
// Clear m_buf.
m_buf = std::queue<double>();
}
if (m_table != nullptr) {
@@ -325,7 +355,7 @@ double PIDController::GetSetpoint() const {
}
/**
* Retruns the current difference of the input from the setpoint
* Returns the current difference of the input from the setpoint
* @return the current error
*/
float PIDController::GetError() const {
@@ -337,6 +367,36 @@ float PIDController::GetError() const {
return GetSetpoint() - pidInput;
}
/**
* Sets what type of input the PID controller will use
*/
void PIDController::SetPIDSourceType(PIDSourceType pidSource) {
m_pidInput->SetPIDSourceType(pidSource);
}
/**
* Returns the type of input the PID controller is using
* @return the PID controller input type
*/
PIDSourceType PIDController::GetPIDSourceType() const {
return m_pidInput->GetPIDSourceType();
}
/**
* Returns the current average of the error over the past few iterations.
* You can specify the number of iterations to average with SetToleranceBuffer()
* (defaults to 1). This is the same value that is used for OnTarget().
* @return the average error
*/
float PIDController::GetAvgError() const {
float avgError = 0;
{
std::unique_lock<priority_mutex> sync(m_mutex);
// Don't divide by zero.
if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
}
return avgError;
}
/*
* Set the percentage error which is considered tolerable for use with
* OnTarget.
@@ -376,6 +436,25 @@ void PIDController::SetAbsoluteTolerance(float absTolerance) {
}
}
/*
* Set the number of previous error samples to average for tolerancing. When
* determining whether a mechanism is on target, the user may want to use a
* rolling average of previous measurements instead of a precise position or
* velocity. This is useful for noisy sensors which return a few erroneous
* measurements when the mechanism is on target. However, the mechanism will
* not register as on target for at least the specified bufLength cycles.
* @param bufLength Number of previous cycles to average. Defaults to 1.
*/
void PIDController::SetToleranceBuffer(unsigned bufLength) {
m_bufLength = bufLength;
// Cut the buffer down to size if needed.
while (m_buf.size() > bufLength) {
m_bufTotal -= m_buf.front();
m_buf.pop();
}
}
/*
* Return true if the error is within the percentage of the total input range,
* determined by SetTolerance. This asssumes that the maximum and minimum input
@@ -386,7 +465,7 @@ void PIDController::SetAbsoluteTolerance(float absTolerance) {
* time.
*/
bool PIDController::OnTarget() const {
double error = GetError();
double error = GetAvgError();
std::unique_lock<priority_mutex> sync(m_mutex);
switch (m_toleranceType) {

6
wpilibc/wpilibC++Devices/src/Ultrasonic.cpp
View File

@@ -316,6 +316,12 @@ double Ultrasonic::PIDGet() const {
}
}
void Ultrasonic::SetPIDSourceType(PIDSourceType pidSource) {
if (wpi_assert(pidSource == PIDSourceType::kDisplacement)) {
m_pidSource = pidSource;
}
}
/**
* Set the current DistanceUnit that should be used for the PIDSource base
* object.

29
wpilibc/wpilibC++IntegrationTests/src/MotorEncoderTest.cpp
View File

@@ -122,11 +122,12 @@ TEST_P(MotorEncoderTest, ClampSpeed) {
}
/**
* Test if PID loops work
* Test if position PID loop works
*/
TEST_P(MotorEncoderTest, PIDController) {
TEST_P(MotorEncoderTest, PositionPIDController) {
Reset();
m_encoder->SetPIDSourceType(PIDSourceType::kDisplacement);
PIDController pid(0.001f, 0.0005f, 0.0f, m_encoder, m_speedController);
pid.SetAbsoluteTolerance(20.0f);
pid.SetOutputRange(-0.3f, 0.3f);
@@ -142,6 +143,30 @@ TEST_P(MotorEncoderTest, PIDController) {
EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds.";
}
/**
* Test if velocity PID loop works
*/
TEST_P(MotorEncoderTest, VelocityPIDController) {
Reset();
m_encoder->SetPIDSourceType(PIDSourceType::kRate);
PIDController pid(1e-5, 0.0f, 3e-5, 8e-5, m_encoder, m_speedController);
pid.SetAbsoluteTolerance(50.0f);
pid.SetToleranceBuffer(10);
pid.SetOutputRange(-0.3f, 0.3f);
pid.SetSetpoint(2000);
/* 10 seconds should be plenty time to get to the setpoint */
pid.Enable();
Wait(10.0);
RecordProperty("PIDError", pid.GetAvgError());
EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds. Goal was: " << 2000 << " Error was: " << pid.GetError();
pid.Disable();
}
/**
* Test resetting encoders
*/

3
wpilibc/wpilibC++Sim/include/Encoder.h
View File

@@ -53,7 +53,7 @@ public:
void SetReverseDirection(bool reverseDirection);
void SetSamplesToAverage(int samplesToAverage);
int GetSamplesToAverage() const;
void SetPIDSourceParameter(PIDSourceParameter pidSource);
void SetPIDSourceType(PIDSourceType pidSource);
double PIDGet() const override;
void UpdateTable() override;
@@ -80,7 +80,6 @@ private:
double m_distancePerPulse; // distance of travel for each encoder tick
EncodingType m_encodingType; // Encoding type
int32_t m_encodingScale; // 1x, 2x, or 4x, per the encodingType
PIDSourceParameter m_pidSource; // Encoder parameter that sources a PID controller
bool m_reverseDirection;
SimEncoder* impl;

3
wpilibc/wpilibC++Sim/include/Gyro.h
View File

@@ -38,10 +38,10 @@ public:
virtual ~Gyro() = default;
virtual float GetAngle() const;
virtual double GetRate() const;
void SetPIDSourceParameter(PIDSourceParameter pidSource);
virtual void Reset();
// PIDSource interface
void SetPIDSourceType(PIDSourceType pidSource) override;
double PIDGet() const override;
void UpdateTable() override;
@@ -55,7 +55,6 @@ private:
void InitGyro(int channel);
SimGyro* impl;
PIDSourceParameter m_pidSource;
std::shared_ptr<ITable> m_table = nullptr;
};

7
wpilibc/wpilibC++Sim/src/Encoder.cpp
View File

@@ -34,7 +34,6 @@ void Encoder::InitEncoder(int channelA, int channelB, bool reverseDirection, Enc
int32_t index = 0;
m_distancePerPulse = 1.0;
m_pidSource = kDistance;
LiveWindow::GetInstance().AddSensor("Encoder", channelA, this);
@@ -309,7 +308,7 @@ void Encoder::SetReverseDirection(bool reverseDirection)
*
* @param pidSource An enum to select the parameter.
*/
void Encoder::SetPIDSourceParameter(PIDSourceParameter pidSource)
void Encoder::SetPIDSourceType(PIDSourceType pidSource)
{
m_pidSource = pidSource;
}
@@ -323,9 +322,9 @@ double Encoder::PIDGet() const
{
switch (m_pidSource)
{
case kDistance:
case PIDSourceType::kDisplacement:
return GetDistance();
case kRate:
case PIDSourceType::kRate:
return GetRate();
default:
return 0.0;

12
wpilibc/wpilibC++Sim/src/Gyro.cpp
View File

@@ -25,7 +25,7 @@ constexpr float Gyro::kDefaultVoltsPerDegreePerSecond;
*/
void Gyro::InitGyro(int channel)
{
SetPIDSourceParameter(kAngle);
SetPIDSourceType(PIDSourceType::kDisplacement);
char buffer[50];
int n = sprintf(buffer, "analog/%d", channel);
@@ -83,10 +83,8 @@ double Gyro::GetRate() const
return impl->GetVelocity();
}
void Gyro::SetPIDSourceParameter(PIDSourceParameter pidSource)
void Gyro::SetPIDSourceType(PIDSourceType pidSource)
{
if(pidSource == 0 || pidSource > 2)
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Gyro pidSource");
m_pidSource = pidSource;
}
@@ -97,10 +95,10 @@ void Gyro::SetPIDSourceParameter(PIDSourceParameter pidSource)
*/
double Gyro::PIDGet() const
{
switch(m_pidSource){
case kRate:
switch(GetPIDSourceType()){
case PIDSourceType::kRate:
return GetRate();
case kAngle:
case PIDSourceType::kDisplacement:
return GetAngle();
default:
return 0;

51
wpilibc/wpilibC++Sim/src/PIDController.cpp
View File

@@ -148,23 +148,42 @@ void PIDController::Calculate()
}
}
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;
}
}
if (m_pidInput->GetPIDSourceType() == 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_maximumOutput / m_P;
}
}
m_result = m_P * m_error + m_I * m_totalError + m_D * (m_prevInput - input) + m_setpoint * m_F;
m_result = m_D * m_error + m_P * m_totalError + 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_prevInput = input;
if (m_result > m_maximumOutput) m_result = m_maximumOutput;

20
wpilibj/enum.txt
View File

@@ -1,20 +0,0 @@
//Going to use this pattern for enumeration classes, since they are otherwise unsupported
//The typesafe enum pattern
public class Suit {
public final int value;
protected static final int CLUBS_VAL = 0;
protected static final int DIAMONDS_VAL = 1;
protected static final int HEARTS_VAL = 2;
protected static final int SPADES_VAL = 3;
public static final Suit CLUBS = new Suit(CLUBS_VAL);
public static final Suit DIAMONDS = new Suit(DIAMONDS_VAL);
public static final Suit HEARTS = new Suit(HEARTS_VAL);
public static final Suit SPADES = new Suit(SPADES_VAL);
private Suit(int value){
this.value = value;
}
}

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

@@ -6,6 +6,7 @@
package edu.wpi.first.wpilibj;
import java.util.TimerTask;
import java.util.LinkedList;
import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
import edu.wpi.first.wpilibj.tables.ITable;
@@ -39,12 +40,15 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
// integral calc
private Tolerance m_tolerance; // the tolerance object used to check if on
// target
private int m_bufLength = 0;
private LinkedList<Double> m_buf;
private double m_bufTotal = 0.0;
private double m_setpoint = 0.0;
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;
@@ -79,7 +83,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
@Override
public boolean onTarget() {
return (Math.abs(getError()) < percentage / 100 * (m_maximumInput - m_minimumInput));
return (Math.abs(getAvgError()) < percentage / 100 * (m_maximumInput - m_minimumInput));
}
}
@@ -92,7 +96,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
@Override
public boolean onTarget() {
return Math.abs(getError()) < value;
return Math.abs(getAvgError()) < value;
}
}
@@ -153,6 +157,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
instances++;
HLUsageReporting.reportPIDController(instances);
m_tolerance = new NullTolerance();
m_buf = new LinkedList<Double>();
}
/**
@@ -256,21 +262,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) {
@@ -280,6 +304,14 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
}
pidOutput = m_pidOutput;
result = m_result;
// Update the buffer.
m_buf.push(m_error);
m_bufTotal += m_error;
// Remove old elements when the buffer is full.
if (m_buf.size() > m_bufLength) {
m_bufTotal -= m_buf.pop();
}
}
pidOutput.pidWrite(result);
@@ -427,6 +459,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
/**
* Set the setpoint for the PIDController
* Clears the queue for GetAvgError().
*$
* @param setpoint the desired setpoint
*/
@@ -443,6 +476,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
m_setpoint = setpoint;
}
m_buf.clear();
if (table != null)
table.putNumber("setpoint", m_setpoint);
}
@@ -466,6 +501,39 @@ 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();
}
/**
* Returns the current difference of the error over the past few iterations.
* You can specify the number of iterations to average with
* setToleranceBuffer() (defaults to 1). getAvgError() is used for the
* onTarget() function.
*$
* @return the current average of the error
*/
public synchronized double getAvgError() {
double avgError = 0;
// Don't divide by zero.
if (m_buf.size() != 0) avgError = m_bufTotal / m_buf.size();
return avgError;
}
/**
* Set the percentage error which is considered tolerable for use with
* OnTarget. (Input of 15.0 = 15 percent)
@@ -513,6 +581,24 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
m_tolerance = new PercentageTolerance(percentage);
}
/**
* Set the number of previous error samples to average for tolerancing. When
* determining whether a mechanism is on target, the user may want to use a
* rolling average of previous measurements instead of a precise position or
* velocity. This is useful for noisy sensors which return a few erroneous
* measurements when the mechanism is on target. However, the mechanism will
* not register as on target for at least the specified bufLength cycles.
* @param bufLength Number of previous cycles to average.
*/
public synchronized void setToleranceBuffer(int bufLength) {
m_bufLength = bufLength;
// Cut the existing buffer down to size if needed.
while (m_buf.size() > bufLength) {
m_bufTotal -= m_buf.pop();
}
}
/**
* Return true if the error is within the percentage of the total input range,
* determined by setTolerance. This assumes that the maximum and minimum input

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();

11
wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
View File

@@ -30,18 +30,15 @@ public class ADXL345_I2C extends SensorBase implements Accelerometer, LiveWindow
private static final byte kDataFormat_SelfTest = (byte) 0x80, kDataFormat_SPI = 0x40,
kDataFormat_IntInvert = 0x20, kDataFormat_FullRes = 0x08, kDataFormat_Justify = 0x04;
public static class Axes {
public static enum Axes {
kX((byte) 0x00),
kY((byte) 0x02),
kZ((byte) 0x04);
/**
* The integer value representing this enumeration
*/
public final byte value;
static final byte kX_val = 0x00;
static final byte kY_val = 0x02;
static final byte kZ_val = 0x04;
public static final Axes kX = new Axes(kX_val);
public static final Axes kY = new Axes(kY_val);
public static final Axes kZ = new Axes(kZ_val);
private Axes(byte value) {
this.value = value;

11
wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
View File

@@ -42,18 +42,15 @@ public class ADXL345_SPI extends SensorBase implements Accelerometer, LiveWindow
private static final int kDataFormat_FullRes = 0x08;
private static final int kDataFormat_Justify = 0x04;
public static class Axes {
public static enum Axes {
kX((byte) 0x00),
kY((byte) 0x02),
kZ((byte) 0x04);
/**
* The integer value representing this enumeration
*/
public final byte value;
static final byte kX_val = 0x00;
static final byte kY_val = 0x02;
static final byte kZ_val = 0x04;
public static final ADXL345_SPI.Axes kX = new ADXL345_SPI.Axes(kX_val);
public static final ADXL345_SPI.Axes kY = new ADXL345_SPI.Axes(kY_val);
public static final ADXL345_SPI.Axes kZ = new ADXL345_SPI.Axes(kZ_val);
private Axes(byte value) {
this.value = value;

15
wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
View File

@@ -26,6 +26,7 @@ public class AnalogAccelerometer extends SensorBase implements PIDSource, LiveWi
private double m_voltsPerG = 1.0;
private double m_zeroGVoltage = 2.5;
private boolean m_allocatedChannel;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/**
* Common initialization
@@ -112,6 +113,20 @@ public class AnalogAccelerometer extends SensorBase implements PIDSource, LiveWi
m_zeroGVoltage = zero;
}
/**
* {@inheritDoc}
*/
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
/**
* {@inheritDoc}
*/
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the Acceleration for the PID Source parent.
*

15
wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
View File

@@ -46,6 +46,7 @@ public class AnalogInput extends SensorBase implements PIDSource, LiveWindowSend
private int m_channel;
private static final int[] kAccumulatorChannels = {0, 1};
private long m_accumulatorOffset;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/**
* Construct an analog channel.
@@ -460,6 +461,20 @@ public class AnalogInput extends SensorBase implements PIDSource, LiveWindowSend
return value;
}
/**
* {@inheritDoc}
*/
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
/**
* {@inheritDoc}
*/
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the average voltage for use with PIDController
*

18
wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
View File

@@ -28,6 +28,7 @@ public class AnalogPotentiometer implements Potentiometer, LiveWindowSendable {
private double m_fullRange, m_offset;
private AnalogInput m_analog_input;
private boolean m_init_analog_input;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/**
* Common initialization code called by all constructors.
@@ -152,6 +153,23 @@ public class AnalogPotentiometer implements Potentiometer, LiveWindowSendable {
return (m_analog_input.getVoltage() / ControllerPower.getVoltage5V()) * m_fullRange + m_offset;
}
/**
* {@inheritDoc}
*/
public void setPIDSourceType(PIDSourceType pidSource) {
if (!pidSource.equals(PIDSourceType.kDisplacement)) {
throw new IllegalArgumentException("Only displacement PID is allowed for potentiometers.");
}
m_pidSource = pidSource;
}
/**
* {@inheritDoc}
*/
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Implement the PIDSource interface.
*

15
wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java
View File

@@ -16,6 +16,7 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, PIDInterface
CANSpeedController, LiveWindowSendable {
private MotorSafetyHelper m_safetyHelper;
private boolean isInverted = false;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
public enum TalonControlMode {
PercentVbus(0), Follower(5), Voltage(4), Position(1), Speed(2), Current(3), Disabled(15);
@@ -123,6 +124,20 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, PIDInterface
}
}
/**
* {@inheritDoc}
*/
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
/**
* {@inheritDoc}
*/
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
@Override
public double pidGet() {
return getPosition();

17
wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Counter.java
View File

@@ -67,7 +67,7 @@ public class Counter extends SensorBase implements CounterBase, LiveWindowSendab
private boolean m_allocatedDownSource;
private ByteBuffer m_counter; // /< The FPGA counter object.
private int m_index; // /< The index of this counter.
private PIDSourceParameter m_pidSource;
private PIDSourceType m_pidSource;
private double m_distancePerPulse; // distance of travel for each tick
private void initCounter(final Mode mode) {
@@ -640,7 +640,7 @@ public class Counter extends SensorBase implements CounterBase, LiveWindowSendab
*
* @param pidSource An enum to select the parameter.
*/
public void setPIDSourceParameter(PIDSourceParameter pidSource) {
public void setPIDSourceType(PIDSourceType pidSource) {
if (pidSource == null) {
throw new NullPointerException("PID Source Parameter given was null");
}
@@ -648,12 +648,19 @@ public class Counter extends SensorBase implements CounterBase, LiveWindowSendab
m_pidSource = pidSource;
}
/**
* {@inheritDoc}
*/
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
@Override
public double pidGet() {
switch (m_pidSource.value) {
case PIDSourceParameter.kDistance_val:
switch (m_pidSource) {
case kDisplacement:
return getDistance();
case PIDSourceParameter.kRate_val:
case kRate:
return getRate();
default:
return 0.0;

29
wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CounterBase.java
View File

@@ -20,27 +20,24 @@ public interface CounterBase {
/**
* The number of edges for the counterbase to increment or decrement on
*/
public static class EncodingType {
public enum EncodingType {
/**
* Count only the rising edge
*/
k1X(0),
/**
* Count both the rising and falling edge
*/
k2X(1),
/**
* Count rising and falling on both channels
*/
k4X(2);
/**
* The integer value representing this enumeration
*/
public final int value;
static final int k1X_val = 0;
static final int k2X_val = 1;
static final int k4X_val = 2;
/**
* Count only the rising edge
*/
public static final EncodingType k1X = new EncodingType(k1X_val);
/**
* Count both the rising and falling edge
*/
public static final EncodingType k2X = new EncodingType(k2X_val);
/**
* Count rising and falling on both channels
*/
public static final EncodingType k4X = new EncodingType(k4X_val);
private EncodingType(int value) {
this.value = value;

25
wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
View File

@@ -26,19 +26,10 @@ public class DoubleSolenoid extends SolenoidBase implements LiveWindowSendable {
/**
* Possible values for a DoubleSolenoid
*/
public static class Value {
public final int value;
public static final int kOff_val = 0;
public static final int kForward_val = 1;
public static final int kReverse_val = 2;
public static final Value kOff = new Value(kOff_val);
public static final Value kForward = new Value(kForward_val);
public static final Value kReverse = new Value(kReverse_val);
private Value(int value) {
this.value = value;
}
public static enum Value {
kOff,
kForward,
kReverse
}
private int m_forwardChannel; // /< The forward channel on the module to
@@ -119,14 +110,14 @@ public class DoubleSolenoid extends SolenoidBase implements LiveWindowSendable {
public void set(final Value value) {
byte rawValue = 0;
switch (value.value) {
case Value.kOff_val:
switch (value) {
case kOff:
rawValue = 0x00;
break;
case Value.kForward_val:
case kForward:
rawValue = m_forwardMask;
break;
case Value.kReverse_val:
case kReverse:
rawValue = m_reverseMask;
break;
}

56
wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Encoder.java
View File

@@ -59,7 +59,7 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
private boolean m_allocatedA;
private boolean m_allocatedB;
private boolean m_allocatedI;
private PIDSourceParameter m_pidSource;
private PIDSourceType m_pidSource;
/**
* Common initialization code for Encoders. This code allocates resources for
@@ -77,8 +77,8 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
* exactly match the spec'd count or be double (2x) the spec'd count.
*/
private void initEncoder(boolean reverseDirection) {
switch (m_encodingType.value) {
case EncodingType.k4X_val:
switch (m_encodingType) {
case k4X:
m_encodingScale = 4;
ByteBuffer status = ByteBuffer.allocateDirect(4);
// set the byte order
@@ -97,15 +97,15 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
m_counter = null;
setMaxPeriod(.5);
break;
case EncodingType.k2X_val:
case EncodingType.k1X_val:
case k2X:
case k1X:
m_encodingScale = m_encodingType == EncodingType.k1X ? 1 : 2;
m_counter = new Counter(m_encodingType, m_aSource, m_bSource, reverseDirection);
m_index = m_counter.getFPGAIndex();
break;
}
m_distancePerPulse = 1.0;
m_pidSource = PIDSourceParameter.kDistance;
m_pidSource = PIDSourceType.kDisplacement;
UsageReporting.report(tResourceType.kResourceType_Encoder, m_index, m_encodingType.value);
LiveWindow.addSensor("Encoder", m_aSource.getChannelForRouting(), this);
@@ -526,12 +526,12 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
* pulses.
*/
private double decodingScaleFactor() {
switch (m_encodingType.value) {
case EncodingType.k1X_val:
switch (m_encodingType) {
case k1X:
return 1.0;
case EncodingType.k2X_val:
case k2X:
return 0.5;
case EncodingType.k4X_val:
case k4X:
return 0.25;
default:
// This is never reached, EncodingType enum limits values
@@ -611,8 +611,8 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
* @param samplesToAverage The number of samples to average from 1 to 127.
*/
public void setSamplesToAverage(int samplesToAverage) {
switch (m_encodingType.value) {
case EncodingType.k4X_val:
switch (m_encodingType) {
case k4X:
ByteBuffer status = ByteBuffer.allocateDirect(4);
// set the byte order
status.order(ByteOrder.LITTLE_ENDIAN);
@@ -622,8 +622,8 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
}
HALUtil.checkStatus(status.asIntBuffer());
break;
case EncodingType.k1X_val:
case EncodingType.k2X_val:
case k1X:
case k2X:
m_counter.setSamplesToAverage(samplesToAverage);
}
}
@@ -637,16 +637,16 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
* 127)
*/
public int getSamplesToAverage() {
switch (m_encodingType.value) {
case EncodingType.k4X_val:
switch (m_encodingType) {
case k4X:
ByteBuffer status = ByteBuffer.allocateDirect(4);
// set the byte order
status.order(ByteOrder.LITTLE_ENDIAN);
int value = EncoderJNI.getEncoderSamplesToAverage(m_encoder, status.asIntBuffer());
HALUtil.checkStatus(status.asIntBuffer());
return value;
case EncodingType.k1X_val:
case EncodingType.k2X_val:
case k1X:
case k2X:
return m_counter.getSamplesToAverage();
}
return 1;
@@ -658,21 +658,27 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
*
* @param pidSource An enum to select the parameter.
*/
public void setPIDSourceParameter(PIDSourceParameter pidSource) {
BoundaryException.assertWithinBounds(pidSource.value, 0, 1);
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
/**
* {@inheritDoc}
*/
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Implement the PIDSource interface.
*
* @return The current value of the selected source parameter.
*/
public double pidGet() {
switch (m_pidSource.value) {
case PIDSourceParameter.kDistance_val:
switch (m_pidSource) {
case kDisplacement:
return getDistance();
case PIDSourceParameter.kRate_val:
case kRate:
return getRate();
default:
return 0.0;
@@ -745,8 +751,8 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
* Live Window code, only does anything if live window is activated.
*/
public String getSmartDashboardType() {
switch (m_encodingType.value) {
case EncodingType.k4X_val:
switch (m_encodingType) {
case k4X:
return "Quadrature Encoder";
default:
return "Encoder";

25
wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Gyro.java
View File

@@ -11,7 +11,6 @@ import edu.wpi.first.wpilibj.communication.UsageReporting;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
import edu.wpi.first.wpilibj.tables.ITable;
import edu.wpi.first.wpilibj.util.BoundaryException;
/**
* Use a rate gyro to return the robots heading relative to a starting position.
@@ -35,7 +34,7 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
int m_center;
boolean m_channelAllocated = false;
AccumulatorResult result;
private PIDSourceParameter m_pidSource;
private PIDSourceType m_pidSource;
/**
* Initialize the gyro. Calibrate the gyro by running for a number of samples
@@ -73,7 +72,7 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
setDeadband(0.0);
setPIDSourceParameter(PIDSourceParameter.kAngle);
setPIDSourceType(PIDSourceType.kDisplacement);
UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel());
LiveWindow.addSensor("Gyro", m_analog.getChannel(), this);
@@ -199,27 +198,33 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
/**
* Set which parameter of the gyro you are using as a process control
* variable. The Gyro class supports the rate and angle parameters
* variable. The Gyro class supports the rate and displacement parameters
*
* @param pidSource An enum to select the parameter.
*/
public void setPIDSourceParameter(PIDSourceParameter pidSource) {
BoundaryException.assertWithinBounds(pidSource.value, 1, 2);
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
/**
* {@inheritDoc}
*/
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the output of the gyro for use with PIDControllers. May be the angle or
* rate depending on the set PIDSourceParameter
* rate depending on the set PIDSourceType
*
* @return the output according to the gyro
*/
@Override
public double pidGet() {
switch (m_pidSource.value) {
case PIDSourceParameter.kRate_val:
switch (m_pidSource) {
case kRate:
return getRate();
case PIDSourceParameter.kAngle_val:
case kDisplacement:
return getAngle();
default:
return 0.0;

41
wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
View File

@@ -29,26 +29,15 @@ public class Ultrasonic extends SensorBase implements PIDSource, LiveWindowSenda
/**
* The units to return when PIDGet is called
*/
public static class Unit {
/**
* The integer value representing this enumeration
*/
public final int value;
static final int kInches_val = 0;
static final int kMillimeters_val = 1;
public static enum Unit {
/**
* Use inches for PIDGet
*/
public static final Unit kInches = new Unit(kInches_val);
kInches,
/**
* Use millimeters for PIDGet
*/
public static final Unit kMillimeter = new Unit(kMillimeters_val);
private Unit(int value) {
this.value = value;
}
kMillimeters
}
private static final double kPingTime = 10 * 1e-6; // /< Time (sec) for the
@@ -72,6 +61,7 @@ public class Ultrasonic extends SensorBase implements PIDSource, LiveWindowSenda
// sensing
private Unit m_units;
private static int m_instances = 0;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/**
* Background task that goes through the list of ultrasonic sensors and pings
@@ -344,16 +334,33 @@ public class Ultrasonic extends SensorBase implements PIDSource, LiveWindowSenda
return getRangeInches() * 25.4;
}
/**
* {@inheritDoc}
*/
public void setPIDSourceType(PIDSourceType pidSource) {
if (!pidSource.equals(PIDSourceType.kDisplacement)) {
throw new IllegalArgumentException("Only displacement PID is allowed for potentiometers.");
}
m_pidSource = pidSource;
}
/**
* {@inheritDoc}
*/
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the range in the current DistanceUnit for the PIDSource base object.
*
* @return The range in DistanceUnit
*/
public double pidGet() {
switch (m_units.value) {
case Unit.kInches_val:
switch (m_units) {
case kInches:
return getRangeInches();
case Unit.kMillimeters_val:
case kMillimeters:
return getRangeMM();
default:
return 0.0;

23
wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
View File

@@ -167,7 +167,8 @@ public class MotorEncoderTest extends AbstractComsSetup {
@Test
public void testPIDController() {
public void testPositionPIDController() {
me.getEncoder().setPIDSourceType(PIDSourceType.kDisplacement);
PIDController pid = new PIDController(0.003, 0.001, 0, me.getEncoder(), me.getMotor());
pid.setAbsoluteTolerance(50);
pid.setOutputRange(-0.2, 0.2);
@@ -184,6 +185,26 @@ public class MotorEncoderTest extends AbstractComsSetup {
pid.free();
}
@Test
public void testVelocityPIDController() {
me.getEncoder().setPIDSourceType(PIDSourceType.kRate);
PIDController pid =
new PIDController(1e-5, 0.0, 3e-5, 8e-5, me.getEncoder(), me.getMotor());
pid.setAbsoluteTolerance(200);
pid.setToleranceBuffer(50);
pid.setOutputRange(-0.3, 0.3);
pid.setSetpoint(2000);
pid.enable();
Timer.delay(10.0);
pid.disable();
assertTrue(
"PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getAvgError(),
pid.onTarget());
pid.free();
}
/**
* Checks to see if the encoders and counters are appropriately reset to zero

15
wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
View File

@@ -31,6 +31,7 @@ public class AnalogInput extends SensorBase implements PIDSource,
private SimFloatInput m_impl;
private int m_channel;
protected PIDSourceParameter m_pidSource = PIDSourceParameter.kDistance;
/**
* Construct an analog channel.
@@ -87,6 +88,20 @@ public class AnalogInput extends SensorBase implements PIDSource,
return m_channel;
}
/**
* {@inheritDoc}
*/
public void setPIDSourceParameter(PIDSourceParameter pidSource) {
m_pidSource = pidSource;
}
/**
* {@inheritDoc}
*/
public PIDSourceParameter getPIDSourceParameter() {
return m_pidSource;
}
/**
* Get the average value for use with PIDController
*

15
wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
View File

@@ -21,6 +21,7 @@ public class AnalogPotentiometer implements Potentiometer, LiveWindowSendable {
private double m_scale, m_offset;
private AnalogInput m_analog_input;
private boolean m_init_analog_input;
protected PIDSourceParameter m_pidSource = PIDSourceParameter.kDistance;
/**
* Common initialization code called by all constructors.
@@ -134,6 +135,20 @@ public class AnalogPotentiometer implements Potentiometer, LiveWindowSendable {
return m_analog_input.getVoltage() * m_scale + m_offset;
}
/**
* {@inheritDoc}
*/
public void setPIDSourceParameter(PIDSourceParameter pidSource) {
m_pidSource = pidSource;
}
/**
* {@inheritDoc}
*/
public PIDSourceParameter getPIDSourceParameter() {
return m_pidSource;
}
/**
* Implement the PIDSource interface.
*

7
wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Encoder.java
View File

@@ -280,6 +280,13 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
m_pidSource = pidSource;
}
/**
* {@inheritDoc}
*/
public PIDSourceParameter getPIDSourceParameter() {
return m_pidSource;
}
/**
* Implement the PIDSource interface.
*

7
wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Gyro.java
View File

@@ -118,6 +118,13 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
m_pidSource = pidSource;
}
/**
* {@inheritDoc}
*/
public PIDSourceParameter getPIDSourceParameter() {
return m_pidSource;
}
/**
* Get the angle of the gyro for use with PIDControllers
*