From a67e3ebd4bcd54eaa878ec568a471eb4751941e5 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Wed, 16 Mar 2022 12:05:34 -0600 Subject: [PATCH] storage and turret shtuff --- src/main/deploy/ShooterData.csv | 4 ++-- src/main/java/frc4388/robot/Constants.java | 8 ++++---- .../frc4388/robot/subsystems/Storage.java | 7 +++++-- .../java/frc4388/robot/subsystems/Turret.java | 20 ++++++++++++------- 4 files changed, 24 insertions(+), 15 deletions(-) diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index 52e1e46..c11dc0f 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -8,5 +8,5 @@ Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Duration (s), 186 ,-76.24 ,10000 ,0, 207 ,-104.07 ,11000 ,0, 227 ,-105.32 ,11500 ,0, -255.5 ,-105.8 ,13500 ,0, -999 ,-105.8 ,13500 ,0, \ No newline at end of file +255.5 ,-105.8 ,13500 ,0, +999 ,-105.8 ,13500 ,0, \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6ecf795..2f43844 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -203,12 +203,12 @@ public final class Constants { // ID public static final int TURRET_MOTOR_CAN_ID = 19; //Gains for turret - public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, 0.6); - public static final double SHOOTER_TURRET_MIN = -0.6; + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, TURRET_SPEED_MULTIPLIER); + 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 = 17.0; // TODO: find - public static final double TURRET_REVERSE_LIMIT = -105.0; // TODO: find + public static final double TURRET_FORWARD_LIMIT = 0.0; + public static final double TURRET_REVERSE_LIMIT = -95.0; //Find //Shooter gains for actual Drum public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); // TODO: tune values diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 5823c96..adcda74 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj.DigitalInput; import com.revrobotics.ColorSensorV3; +import edu.wpi.first.wpilibj.util.Color; public class Storage extends SubsystemBase { public CANSparkMax m_storageMotor; @@ -49,11 +50,13 @@ public class Storage extends SubsystemBase { } public boolean getRed(){ - return (m_colorSensor.getRed() >= 200); + // return (m_colorSensor.getRed() >= 200 && m_colorSensor.getBlue() < 100 && m_colorSensor.getGreen() < 100); + return (m_colorSensor.getColor() == Color.kRed); } public boolean getBlue(){ - return (m_colorSensor.getBlue() >= 200); + // return (m_colorSensor.getBlue() >= 200 && m_colorSensor.getRed() < 100 && m_colorSensor.getGreen() < 100); + return (m_colorSensor.getColor() == Color.kBlue); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 403b8c4..d6aae1a 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -18,6 +18,7 @@ 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 { @@ -38,6 +39,8 @@ public class Turret extends SubsystemBase { SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController(); public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); + SparkMaxLimitSwitch m_boomBoomLeftLimit; + SparkMaxLimitSwitch m_boomBoomRightLimit; // Variables public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument @@ -47,12 +50,10 @@ public class Turret extends SubsystemBase { m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); - // m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_boomBoomRightLimit.enableLimitSwitch(true); - // m_boomBoomLeftLimit.enableLimitSwitch(true); - // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); - // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit.enableLimitSwitch(true); + m_boomBoomLeftLimit.enableLimitSwitch(true); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_LIMIT); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT); @@ -73,6 +74,8 @@ public class Turret extends SubsystemBase { // This method will be called once per scheduler run SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); + if (m_boomBoomLeftLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_LIMIT - 2); + if (m_boomBoomRightLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_LIMIT + 2); } /** @@ -88,7 +91,10 @@ public class Turret extends SubsystemBase { 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); }