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

speed ramp fixes for hood + turret

Browse Source
This commit is contained in:
aarav18
2022-03-17 13:49:13 -06:00
parent 0a79361a97
commit ba8de492b3
4 changed files with 44 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+6 -3
View File
@@ -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;
}
src/main/java/frc4388/robot/RobotContainer.java
+3 -3
View File
@@ -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)
src/main/java/frc4388/robot/subsystems/Hood.java
+28 -5
View File
@@ -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(){
src/main/java/frc4388/robot/subsystems/Turret.java
+7 -3
View File
@@ -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;
}
}
/**