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

[wpilib] Fixup wouldHitLowerLimit in elevator and arm simulation classes. (#3076)

Browse Source 
Closes #3050.
This commit is contained in:
Matt
2021-01-14 00:28:00 -08:00
committed by GitHub
parent 04a90b5dd1
commit 406d055f07
6 changed files with 118 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View File

@@ -127,26 +127,42 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
m_maxHeight = maxHeightMeters;
}
/**
* Returns whether the elevator would hit the lower limit.
*
* @param elevatorHeightMeters The elevator height.
* @return Whether the elevator would hit the lower limit.
*/
public boolean wouldHitLowerLimit(double elevatorHeightMeters) {
return elevatorHeightMeters < this.m_minHeight;
}
/**
* Returns whether the elevator would hit the upper limit.
*
* @param elevatorHeightMeters The elevator height.
* @return Whether the elevator would hit the upper limit.
*/
public boolean wouldHitUpperLimit(double elevatorHeightMeters) {
return elevatorHeightMeters > this.m_maxHeight;
}
/**
* Returns whether the elevator has hit the lower limit.
*
* @param x The current elevator state.
* @return Whether the elevator has hit the lower limit.
*/
@SuppressWarnings("ParameterName")
public boolean hasHitLowerLimit(Matrix<N2, N1> x) {
return x.get(0, 0) < this.m_minHeight;
public boolean hasHitLowerLimit() {
return wouldHitLowerLimit(getPositionMeters());
}
/**
* Returns whether the elevator has hit the upper limit.
*
* @param x The current elevator state.
* @return Whether the elevator has hit the upper limit.
*/
@SuppressWarnings("ParameterName")
public boolean hasHitUpperLimit(Matrix<N2, N1> x) {
return x.get(0, 0) > this.m_maxHeight;
public boolean hasHitUpperLimit() {
return wouldHitUpperLimit(getPositionMeters());
}
/**
@@ -215,10 +231,10 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
dtSeconds);
// We check for collisions after updating x-hat.
if (hasHitLowerLimit(updatedXhat)) {
if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
return VecBuilder.fill(m_minHeight, 0);
}
if (hasHitUpperLimit(updatedXhat)) {
if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
return VecBuilder.fill(m_maxHeight, 0);
}
return updatedXhat;

38
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
View File

@@ -173,26 +173,42 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
m_simulateGravity = simulateGravity;
}
/**
* Returns whether the arm would hit the lower limit.
*
* @param currentAngleRads The current arm height.
* @return Whether the arm would hit the lower limit.
*/
public boolean wouldHitLowerLimit(double currentAngleRads) {
return currentAngleRads < this.m_minAngle;
}
/**
* Returns whether the arm would hit the upper limit.
*
* @param currentAngleRads The current arm height.
* @return Whether the arm would hit the upper limit.
*/
public boolean wouldHitUpperLimit(double currentAngleRads) {
return currentAngleRads > this.m_maxAngle;
}
/**
* Returns whether the arm has hit the lower limit.
*
* @param x The current arm state.
* @return Whether the arm has hit the lower limit.
*/
@SuppressWarnings("ParameterName")
public boolean hasHitLowerLimit(Matrix<N2, N1> x) {
return x.get(0, 0) < this.m_minAngle;
public boolean hasHitLowerLimit() {
return wouldHitLowerLimit(getAngleRads());
}
/**
* Returns whether the arm has hit the upper limit.
*
* @param x The current arm state.
* @return Whether the arm has hit the upper limit.
*/
@SuppressWarnings("ParameterName")
public boolean hasHitUpperLimit(Matrix<N2, N1> x) {
return x.get(0, 0) > this.m_maxAngle;
public boolean hasHitUpperLimit() {
return wouldHitUpperLimit(getAngleRads());
}
/**
@@ -227,7 +243,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
}
/**
* Sets the input voltage for the elevator.
* Sets the input voltage for the arm.
*
* @param volts The input voltage.
*/
@@ -288,10 +304,10 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
dtSeconds);
// We check for collision after updating xhat
if (hasHitLowerLimit(updatedXhat)) {
if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
return VecBuilder.fill(m_minAngle, 0);
}
if (hasHitUpperLimit(updatedXhat)) {
if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
return VecBuilder.fill(m_maxAngle, 0);
}
return updatedXhat;