Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

calibration speed + velocity ramp only if soft limits

Browse Source
This commit is contained in:
aarav18
2022-03-19 21:03:01 -06:00
parent be850585ec
commit 6b38e89660
5 changed files with 116 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Hood.java
+20 -12
View File
@@ -35,6 +35,7 @@ public class Hood extends SubsystemBase {
public double m_fireAngle;
public double speedLimiter;
public double calibrationSpeed = 1.0;
/** Creates a new Hood. */
public Hood(CANSparkMax angleAdjusterMotor) {
@@ -63,23 +64,30 @@ public class Hood extends SubsystemBase {
SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition());
// * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output).
double currentPos = this.getEncoderPosition();
double forwardDistance = Math.abs(currentPos - ShooterConstants.HOOD_FORWARD_SOFT_LIMIT);
double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT);
if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
}
if (areSoftLimitsEnabled()) {
double currentPos = this.getEncoderPosition();
double forwardDistance = Math.abs(currentPos - ShooterConstants.HOOD_FORWARD_SOFT_LIMIT);
double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT);
if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
}
if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
}
if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) {
this.speedLimiter = 1.0;
if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
}
if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) {
this.speedLimiter = 1.0;
}
}
}
public boolean areSoftLimitsEnabled() {
return this.m_angleAdjusterMotor.isSoftLimitEnabled(SoftLimitDirection.kForward) && this.m_angleAdjusterMotor.isSoftLimitEnabled(SoftLimitDirection.kReverse);
}
/**
* Set status of hood motor soft limits.
* @param set Boolean to set soft limits to.
@@ -107,7 +115,7 @@ public class Hood extends SubsystemBase {
* @param input value from -1.0 to 1.0, postive is upward (more horizontal shootijng angle)
*/
public void runHood(double input) {
m_angleAdjusterMotor.set(input * this.speedLimiter);
m_angleAdjusterMotor.set(input * this.speedLimiter * this.calibrationSpeed);
}
/**