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

lots of weird turret limit shtuff, NOT COMPLETE

Browse Source
This commit is contained in:
Ryan Manley
2022-03-16 21:18:10 -06:00
parent 2f23aed7f1
commit 411e06211c
6 changed files with 80 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+9 -2
View File
@@ -207,8 +207,15 @@ public final class Constants {
public static final double SHOOTER_TURRET_MIN = -TURRET_SPEED_MULTIPLIER;
//Gains for hood
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7);
public static final double TURRET_FORWARD_LIMIT = 0.0;
public static final double TURRET_REVERSE_LIMIT = -95.0; //Find
//#region test start
//#endregion test end
public static final double TURRET_FORWARD_HARD_LIMIT = 0.0;
public static final double TURRET_REVERSE_HARD_LIMIT = -85.0;
public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 2;
public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2;
//Shooter gains for actual Drum
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0);
src/main/java/frc4388/robot/Robot.java
+1 -1
View File
@@ -204,7 +204,7 @@ public class Robot extends TimedRobot {
LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
}
m_robotContainer.m_robotVisionOdometry.setLEDs(true);
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
}
@Override
src/main/java/frc4388/robot/RobotContainer.java
+4 -4
View File
@@ -166,10 +166,10 @@ public class RobotContainer {
getOperatorController().getRightTriggerAxis()),
m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
m_robotStorage.setDefaultCommand(
new ManageStorage(m_robotStorage,
m_robotBoomBoom,
m_robotTurret).withName("Storage ManageStorage defaultCommand"));
// m_robotStorage.setDefaultCommand(
// new ManageStorage(m_robotStorage,
// m_robotBoomBoom,
// m_robotTurret).withName("Storage ManageStorage defaultCommand"));
// m_robotClimber.setDefaultCommand(
// new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber)
src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java
+1 -1
View File
@@ -69,7 +69,7 @@ public class AimToCenter extends CommandBase {
if (angle == Double.NaN) {
return false;
}
return !((ShooterConstants.TURRET_REVERSE_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_LIMIT));
return !((ShooterConstants.TURRET_REVERSE_SOFT_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_SOFT_LIMIT));
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/subsystems/Extender.java
+6 -6
View File
@@ -17,18 +17,18 @@ public class Extender extends SubsystemBase {
private CANSparkMax m_extenderMotor;
private SparkMaxLimitSwitch m_inLimit;
private SparkMaxLimitSwitch m_outLimit;
// private SparkMaxLimitSwitch m_inLimit;
// private SparkMaxLimitSwitch m_outLimit;
/** Creates a new Extender. */
public Extender(CANSparkMax extenderMotor) {
m_extenderMotor = extenderMotor;
m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_inLimit.enableLimitSwitch(false);
m_outLimit.enableLimitSwitch(false);
// m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_inLimit.enableLimitSwitch(false);
// m_outLimit.enableLimitSwitch(false);
m_extenderMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ExtenderConstants.EXTENDER_FORWARD_LIMIT);
m_extenderMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ExtenderConstants.EXTENDER_REVERSE_LIMIT);
src/main/java/frc4388/robot/subsystems/Turret.java
+59 -13
View File
@@ -37,23 +37,35 @@ public class Turret extends SubsystemBase {
SparkMaxLimitSwitch m_boomBoomLeftLimit;
SparkMaxLimitSwitch m_boomBoomRightLimit;
boolean hasLeftSwitchChanged = false;
boolean hasRightSwitchChanged = false;
boolean leftPrevState = false;
boolean rightPrevState = false;
boolean leftState;
boolean rightState;
long leftCurrentTime;
long rightCurrentTime;
long leftElapsedTime;
long rightElapsedTime;
public Turret(CANSparkMax boomBoomRotateMotor) {
m_boomBoomRotateMotor = boomBoomRotateMotor;
m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController();
m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder();
m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_boomBoomLeftLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_boomBoomRightLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_boomBoomLeftLimit.enableLimitSwitch(true);
// m_boomBoomRightLimit.enableLimitSwitch(true);
setTurretLimitSwitches(true);
// SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled());
// SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled());
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_LIMIT);
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT);
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_SOFT_LIMIT);
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_SOFT_LIMIT);
setTurretSoftLimits(true);
setTurretPIDGains();
}
@@ -68,19 +80,53 @@ public class Turret extends SubsystemBase {
m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone);
m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled());
SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled());
SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition());
SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT);
SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed());
SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed());
if (m_boomBoomLeftLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_LIMIT - 2);
if (m_boomBoomRightLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_LIMIT + 2);
// limit switch annoying time thing
leftState = m_boomBoomLeftLimit.isPressed();
rightState = m_boomBoomRightLimit.isPressed();
hasLeftSwitchChanged = (leftState != leftPrevState);
hasRightSwitchChanged = (rightState != rightPrevState);
if (leftState && hasLeftSwitchChanged) {
leftCurrentTime = System.currentTimeMillis();
leftElapsedTime = 0;
}
if (rightState && hasRightSwitchChanged) {
rightCurrentTime = System.currentTimeMillis();
rightElapsedTime = 0;
}
if (leftState && !hasLeftSwitchChanged) {
leftElapsedTime = System.currentTimeMillis() - leftCurrentTime;
}
if (rightState && !hasRightSwitchChanged) {
rightElapsedTime = System.currentTimeMillis() - rightCurrentTime;
}
if (leftState && (leftElapsedTime > 500)) {
m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// -95/*ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - 2*/);
}
if (rightState && (rightElapsedTime > 500)) {
m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/);
}
leftPrevState = leftState;
rightPrevState = rightState;
}
/**
@@ -110,7 +156,7 @@ public class Turret extends SubsystemBase {
* @param input from -1.0 to 1.0, positive is clockwise
*/
public void runTurretWithInput(double input) {
m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER);
m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * 0.5);
}
public void runShooterRotatePID(double targetAngle) {