From ba8de492b3b4216ce097c26df1f41299f53f2574 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 13:49:13 -0600 Subject: [PATCH] speed ramp fixes for hood + turret --- src/main/java/frc4388/robot/Constants.java | 9 +++-- .../java/frc4388/robot/RobotContainer.java | 6 ++-- .../java/frc4388/robot/subsystems/Hood.java | 33 ++++++++++++++++--- .../java/frc4388/robot/subsystems/Turret.java | 10 ++++-- 4 files changed, 44 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index ff182de..5376fd2 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -4,6 +4,8 @@ package frc4388.robot; +import java.security.PublicKey; + import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; @@ -215,7 +217,7 @@ public final class Constants { public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 5; public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2; - public static final double TURRET_HARD_LIMIT_TOLERANCE = 20.0; + public static final double TURRET_SOFT_LIMIT_TOLERANCE = 20.0; //Shooter gains for actual Drum public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); @@ -223,8 +225,9 @@ public final class Constants { public static final int SHOOTER_ANGLE_ADJUST_ID = 20; public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find - public static final double HOOD_FORWARD_LIMIT = 0.0; // TODO: find - public static final double HOOD_REVERSE_LIMIT = -150; // TODO: find + public static final double HOOD_FORWARD_SOFT_LIMIT = 0.0; // TODO: find + public static final double HOOD_REVERSE_SOFT_LIMIT = -150; // TODO: find + public static final double HOOD_SOFT_LIMIT_TOLERANCE = 20.0; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 80aae83..b028826 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -63,7 +63,7 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; -import frc4388.robot.commands.ButtonBoxCommands.TurretManual; +// import frc4388.robot.commands.ButtonBoxCommands.TurretManual; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.AimToCenter; import frc4388.robot.commands.ShooterCommands.Shoot; @@ -353,8 +353,8 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true))) // .whenReleased(new RunCommand(() -> RunMiddleSwitch.setManual(false))); - new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - .whileHeld(new TurretManual(m_robotTurret)); + // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + // .whileHeld(new TurretManual(m_robotTurret)); // control turret manual mode // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 904eb26..0652f4b 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -34,6 +34,7 @@ public class Hood extends SubsystemBase { public double m_fireAngle; + public double speedLimiter; /** Creates a new Hood. */ public Hood(CANSparkMax angleAdjusterMotor) { @@ -48,9 +49,11 @@ public class Hood extends SubsystemBase { // m_hoodUpLimitSwitch.enableLimitSwitch(true); // m_hoodDownLimitSwitch.enableLimitSwitch(true); - m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_LIMIT); - m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_LIMIT); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_SOFT_LIMIT); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_SOFT_LIMIT); setHoodSoftLimits(true); + + this.speedLimiter = 1.0; } @@ -58,6 +61,26 @@ public class Hood extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run 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 * 0.05); + } + + if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (reverseDistance * 0.05); + } + + if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) { + this.speedLimiter = 1.0; + } + + double hoodCurrent = m_angleAdjusterMotor.getOutputCurrent(); + } /** @@ -87,7 +110,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); + m_angleAdjusterMotor.set(input * this.speedLimiter); } /** @@ -101,8 +124,8 @@ public class Hood extends SubsystemBase { m_angleEncoder.setPosition(0); } - public double getAnglePosition(){ - return 0.0;//m_angleEncoder.getPosition(); + public double getEncoderPosition(){ + return m_angleEncoder.getPosition(); } public double getAnglePositionDegrees(){ diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 17f05f2..de1c9eb 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -146,18 +146,22 @@ public class Turret extends SubsystemBase { leftPrevState = leftState; // * Update the state of the left limit switch. - // * speed limiting near hard limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). + // * 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.TURRET_FORWARD_SOFT_LIMIT); double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); - if (forwardDistance < ShooterConstants.TURRET_HARD_LIMIT_TOLERANCE) { + if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { this.speedLimiter = 0.2 + (forwardDistance * 0.05); } - if (reverseDistance < ShooterConstants.TURRET_HARD_LIMIT_TOLERANCE) { + if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { this.speedLimiter = 0.2 + (reverseDistance * 0.05); } + + if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) { + this.speedLimiter = 1.0; + } } /**