From fc3a61f75c2c089ab1c426fda4b83c4986497264 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 11:21:55 -0600 Subject: [PATCH] ik --- .../frc4388/robot/subsystems/Turret.java.bak | 234 ------------------ 1 file changed, 234 deletions(-) delete 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 deleted file mode 100644 index e98d8b6..0000000 --- a/src/main/java/frc4388/robot/subsystems/Turret.java.bak +++ /dev/null @@ -1,234 +0,0 @@ -// 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