From 457a31294f3f8a571a0386e25c03d625662e428c Mon Sep 17 00:00:00 2001 From: Jack Nelson Date: Sat, 13 Jan 2024 12:53:47 -0500 Subject: [PATCH] add encoder conversion factors for shooter and arm these need to be changed later --- .../frc/robot/subsystems/ArmSubsystem.java | 24 ++++++++++-- .../robot/subsystems/ShooterSubsystem.java | 38 ++++++++++++------- 2 files changed, 45 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 145d361..dc50f70 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -8,14 +8,23 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.CANConstants; public class ArmSubsystem extends SubsystemBase { private final CANSparkMax m_armMotorMain, m_armMotorSecondary; private final SparkPIDController m_armMainPIDController; - private RelativeEncoder m_MainEncoder; + private final RelativeEncoder m_MainEncoder, m_SecondaryEncoder; private final double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, kLoweredArmPosition; + // general drive constants + // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 + // https://sciencing.com/convert-rpm-linear-speed-8232280.html + private final double kWheelDiameter = Units.inchesToMeters(6); // meters + private final double kGearRatio = 1; // TBD + // basically converted from rotations to to radians to then meters using the wheel diameter. + // the diameter is already *2 so we don't need to multiply by 2 again. + private final double kPositionConversionRatio = (Math.PI * kWheelDiameter) / kGearRatio; /** Creates a new ArmSubsystem. */ public ArmSubsystem() { @@ -35,6 +44,10 @@ public ArmSubsystem() { m_armMainPIDController = m_armMotorMain.getPIDController(); // allow us to read the encoder m_MainEncoder = m_armMotorMain.getEncoder(); + m_SecondaryEncoder = m_armMotorSecondary.getEncoder(); + // setup the encoders + m_MainEncoder.setPositionConversionFactor(kPositionConversionRatio); + m_SecondaryEncoder.setPositionConversionFactor(kPositionConversionRatio); // PID coefficients kP = 0.1; kI = 1e-4; @@ -54,11 +67,16 @@ public ArmSubsystem() { m_armMainPIDController.setOutputRange(kMinOutput, kMaxOutput); } + public double getAverageEncoderPosition() { + // get the average encoder position + return (m_MainEncoder.getPosition() + m_SecondaryEncoder.getPosition()) / 2; + } + /* * Move arm relative to current position */ public void MoveArmRelative(double rotations) { - rotations = rotations + m_MainEncoder.getPosition(); + rotations = rotations + getAverageEncoderPosition(); // update the PID controller with current encoder position MoveArmToPosition(rotations); } @@ -81,7 +99,7 @@ public void MoveArmToPosition(double rotations) { */ public void stop() { // update the PID controller with current encoder position - MoveArmToPosition(m_MainEncoder.getPosition()); + MoveArmToPosition(getAverageEncoderPosition()); } public void zero() { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 9e4e8a4..e00bca2 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -8,6 +8,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.CANConstants; @@ -15,7 +16,15 @@ public class ShooterSubsystem extends SubsystemBase { private final CANSparkMax m_ShooterMotorMain; private final SparkPIDController m_ShooterMainPIDController; private RelativeEncoder m_ShooterMainEncoder; - private final double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, maxRPM; + private final double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, kMaxSpeed; + // general drive constants + // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 + // https://sciencing.com/convert-rpm-linear-speed-8232280.html + private final double kWheelDiameter = Units.inchesToMeters(6); // meters + private final double kGearRatio = 1; // TBD + // basically converted from rotations to to radians to then meters using the wheel diameter. + // the diameter is already *2 so we don't need to multiply by 2 again. + private final double kVelocityConversionRatio = (Math.PI * kWheelDiameter) / kGearRatio / 60; /** Creates a new ShooterSubsystem. */ public ShooterSubsystem() { @@ -30,6 +39,7 @@ public ShooterSubsystem() { // allow us to read the encoder m_ShooterMainEncoder = m_ShooterMotorMain.getEncoder(); + m_ShooterMainEncoder.setVelocityConversionFactor(kVelocityConversionRatio); // PID coefficients kP = 6e-5; kI = 0; @@ -38,7 +48,7 @@ public ShooterSubsystem() { kFF = 0.000015; kMaxOutput = 1; kMinOutput = -1; - maxRPM = 5700; + kMaxSpeed = 5; // set PID coefficients m_ShooterMainPIDController.setP(kP); @@ -50,10 +60,10 @@ public ShooterSubsystem() { } /* - * Spin shooter at a given RPM + * Spin shooter at a given Speed (M/S) */ - public void SpinShooter(double RPM) { - m_ShooterMainPIDController.setReference(RPM, CANSparkBase.ControlType.kVelocity); + public void SpinShooter(double speed) { + m_ShooterMainPIDController.setReference(speed, CANSparkBase.ControlType.kVelocity); } /* @@ -64,25 +74,25 @@ public void StopShooter() { } /* - * Spin Shooter at max RPM + * Spin Shooter at max Speed */ public void SpinShooterFull() { - SpinShooter(maxRPM); + SpinShooter(kMaxSpeed); } /* - * Check if shooter is at a given RPM + * Check if shooter is at a given Speed */ - public Boolean isAtRPMTolerance(double RPM) { - return (m_ShooterMainEncoder.getVelocity() > RPM - 100 - && m_ShooterMainEncoder.getVelocity() < RPM + 100); + public Boolean isAtRPMTolerance(double speed) { + return (m_ShooterMainEncoder.getVelocity() > speed - 0.1 + && m_ShooterMainEncoder.getVelocity() < speed + 0.1); } /* - * Check if shooter is at max RPM + * Check if shooter is at max Speed */ - public Boolean isAtMaxRPM() { - return isAtRPMTolerance(maxRPM); + public Boolean isAtMaxSpeed() { + return isAtRPMTolerance(kMaxSpeed); } @Override