From 23a208fbe3f058aa9f87639b14fcb348a8a35523 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 11:21:29 -0600 Subject: [PATCH] idk --- .../frc4388/robot/subsystems/Turret.java.bak | 234 ++++++++++++++++++ 1 file changed, 234 insertions(+) create mode 100644 src/main/java/frc4388/robot/subsystems/Turret.java.bak diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java.bak b/src/main/java/frc4388/robot/subsystems/Turret.java.bak new file mode 100644 index 0000000..e98d8b6 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Turret.java.bak @@ -0,0 +1,234 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.SparkMaxPIDController; + +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.commands.ShooterCommands.Shoot; +import frc4388.utility.Gains; + +public class Turret extends SubsystemBase { + + /** Creates a new Turret. */ + public BoomBoom m_boomBoomSubsystem; + public SwerveDrive m_sDriveSubsystem; + + public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, + // MotorType.kBrushless); + public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; + + public SparkMaxPIDController m_boomBoomRotatePIDController; + public RelativeEncoder m_boomBoomRotateEncoder; + + SparkMaxLimitSwitch m_boomBoomLeftLimit; + SparkMaxLimitSwitch m_boomBoomRightLimit; + + boolean leftState; +<<<<<<< HEAD + boolean rightState; + boolean recentlyPressed; +======= + boolean leftPrevState = false; + boolean hasLeftSwitchChanged = false; +>>>>>>> 87a6707cc636ec5de2749b86024b13dd7e28c908 + + long leftCurrentTime; + long leftElapsedTime; + + public Turret(CANSparkMax boomBoomRotateMotor) { + + m_boomBoomRotateMotor = boomBoomRotateMotor; + m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); + m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); + + // setTurretSoftLimits(true); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); + + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // setTurretLimitSwitches(true); + m_boomBoomRightLimit.enableLimitSwitch(true); + m_boomBoomLeftLimit.enableLimitSwitch(true); + + setTurretPIDGains(); + } + + // public void toggleLeftLimitSwitch() { + // TODO: find better way to do this, but im in a hurry + + + // if (leftSwitch.isLimitSwitchEnabled()) { + // leftSwitch.enableLimitSwitch(false); + // } else { + // leftSwitch.enableLimitSwitch(true); + // } + // } + + // public void turnOnLeftLimitSwitch() { + // SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // System.out.println("Left Switch ENABLED"); + // leftSwitch.enableLimitSwitch(true); + // } + + // public void turnOffLeftLimitSwitch() { + // SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // System.out.println("Left Switch DISABLED"); + // leftSwitch.enableLimitSwitch(false); + // } + + /** + * Set gains for turret PIDs. + */ + public void setTurretPIDGains() { + m_boomBoomRotatePIDController.setP(m_shooterTGains.kP); + m_boomBoomRotatePIDController.setI(m_shooterTGains.kI); + m_boomBoomRotatePIDController.setD(m_shooterTGains.kD); + m_boomBoomRotatePIDController.setFF(m_shooterTGains.kF); + 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()); + + // limit switch annoying time thing but actually worked first try wtf +<<<<<<< HEAD + leftState = m_boomBoomLeftLimit.isPressed(); + + hasLeftSwitchChanged = (leftState != leftPrevState); + + if (leftState && hasLeftSwitchChanged) { +======= + leftState = m_boomBoomLeftLimit.isPressed(); // * Get the state of the left limit switch (true for pressed). + + hasLeftSwitchChanged = (leftState != leftPrevState); // * Get whether the state of the left limit switch has changed, based on its previous state. + + if (leftState && hasLeftSwitchChanged) { // * If the left limit switch is pressed, and it recently changed, start the time. +>>>>>>> 87a6707cc636ec5de2749b86024b13dd7e28c908 + leftCurrentTime = System.currentTimeMillis(); + leftElapsedTime = 0; + } + +<<<<<<< HEAD + if (leftState && !hasLeftSwitchChanged) { + leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; + } + + if (leftState && (leftElapsedTime > 500)) { + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT); + } + if (!m_boomBoomRightLimit.isPressed()) recentlyPressed = false; + + if(m_boomBoomRightLimit.isPressed() && !recentlyPressed){ + recentlyPressed = true; + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); + } + SmartDashboard.putBoolean("Recently Pressed", recentlyPressed); + + leftPrevState = leftState; + // rightPrevState = rightState; +======= + if (leftState && !hasLeftSwitchChanged) { // * If the left limit switch is still pressed, but the state hasn't changed, then calculate elapsed time. + leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; + } + + if (leftState && (leftElapsedTime > 500)) { // * If the left limit switch is pressed for more than half a second, update the encoder position. + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT); + } + + leftPrevState = leftState; // * Update the state of the left limit switch. +>>>>>>> 87a6707cc636ec5de2749b86024b13dd7e28c908 + } + + /** + * Set status of turret motor soft limits. + * @param set Boolean to set soft limits to. + */ + public void setTurretSoftLimits(boolean set) { + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); + } + + /** + * Set status of turret limit switches. + * @param set Boolean to set limit switches to. + */ + public void setTurretLimitSwitches(boolean set) { + m_boomBoomRightLimit.enableLimitSwitch(set); + m_boomBoomLeftLimit.enableLimitSwitch(set); + } + + public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) { + m_boomBoomSubsystem = subsystem0; + m_sDriveSubsystem = subsystem1; + } + /** + * Move the turret with an input + * @param input from -1.0 to 1.0, positive is clockwise + */ + public void runTurretWithInput(double input) { + m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); + } + + public void runShooterRotatePID(double targetAngle) { + targetAngle = targetAngle / ShooterConstants.TURRET_DEGREES_PER_ROT; + m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition); + } + + public void resetGyroShooterRotate() { + m_boomBoomRotateEncoder.setPosition(0); + } + + /** + * Run a PID to go to the zero position. + */ + public void gotoZero() { + runShooterRotatePID(0); + } + + /** + * Run a PID to go to the midpoint position, between the two soft limits. + */ + public void gotoMidpoint() { + runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT); + } + + public double getEncoderPosition() { + return m_boomBoomRotateEncoder.getPosition(); + } + + public double getBoomBoomAngleDegrees() { + return (getEncoderPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); + } + + public double getCurrent(){ + return m_boomBoomRotateMotor.getOutputCurrent(); + } + +} \ No newline at end of file