From 6a639544ae572dfb06af16862cdd76f7d904a71d Mon Sep 17 00:00:00 2001 From: Ryan Manley Date: Sat, 5 Mar 2022 14:21:48 -0700 Subject: [PATCH] hook fixes --- src/main/java/frc4388/robot/Constants.java | 11 +-- .../java/frc4388/robot/RobotContainer.java | 27 ++++--- src/main/java/frc4388/robot/RobotMap.java | 24 +++--- .../java/frc4388/robot/subsystems/Claws.java | 76 +++++++++++++++++++ .../frc4388/robot/subsystems/Climber.java | 17 ++--- .../java/frc4388/robot/subsystems/Hooks.java | 62 --------------- 6 files changed, 114 insertions(+), 103 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Claws.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Hooks.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b79a13d..cfc8496 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -80,7 +80,7 @@ public final class Constants { public static final int SHOULDER_ID = 1; public static final int ELBOW_ID = 3; public static final int GYRO_ID = -1; - + // TODO Update this stuff too public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm public static final double LOWER_ARM_LENGTH = 27; @@ -110,14 +110,15 @@ public final class Constants { public static final int CLIMBER_TIMEOUT_MS = 50; - /* TODO: Update Constants */ + // TODO: Update Constants + // Robot Angle public static final double ROBOT_ANGLE_ID = 0; } - public static final class HooksConstants { - public static final int LEFT_HOOK_ID = -1; - public static final int RIGHT_HOOK_ID = -1; + public static final class ClawConstants { + public static final int LEFT_CLAW_ID = 44; + public static final int RIGHT_CLAW_ID = 45; public static final double OPEN_POSITION = 0; public static final double CLOSE_POSITION = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8f7cd6a..1b15a07 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -10,8 +10,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.subsystems.Claws; import frc4388.robot.subsystems.Climber; -import frc4388.robot.subsystems.Hooks; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; @@ -29,7 +29,7 @@ public class RobotContainer { /* RobotMap */ private final RobotMap m_robotMap = new RobotMap(); - /* Subsystems */ + /* Subsystems *//* private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor, m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor, @@ -39,13 +39,13 @@ public class RobotContainer { m_robotMap.rightFrontEncoder, m_robotMap.leftBackEncoder, m_robotMap.rightBackEncoder - ); + );*/ - private final LED m_robotLED = new LED(m_robotMap.LEDController); + /*private final LED m_robotLED = new LED(m_robotMap.LEDController); private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); - - private final Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook); + */ + private final Claws m_claws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -64,12 +64,12 @@ public class RobotContainer { // getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); // moves climber in xy space with two-axis input from the operator controller - m_robotClimber.setDefaultCommand( + /*m_robotClimber.setDefaultCommand( new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftXAxis(), getOperatorController().getLeftYAxis()), m_robotClimber)); // continually sends updates to the Blinkin LED controller to keep the lights on - m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); + m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));*/ } /** @@ -83,13 +83,16 @@ public class RobotContainer { /* Operator Buttons */ // activates "Lit Mode" - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + /*new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) - .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));*/ new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whenPressed(() -> m_hooks.setOpen(true)) - .whenReleased(() -> m_hooks.setOpen(false)); + .whenPressed(() -> m_claws.setSpeed(0.5)) + .whenReleased(() -> m_claws.setSpeed(0.0)); + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + .whenPressed(() -> m_claws.setSpeed(-0.5)) + .whenReleased(() -> m_claws.setSpeed(0.0)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index a1ddadb..bfa738e 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.ClimberConstants; -import frc4388.robot.Constants.HooksConstants; +import frc4388.robot.Constants.ClawConstants; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; @@ -27,18 +27,18 @@ import frc4388.robot.Constants.SwerveDriveConstants; public class RobotMap { public RobotMap() { - configureLEDMotorControllers(); - configureSwerveMotorControllers(); + //configureLEDMotorControllers(); + //configureSwerveMotorControllers(); } /* LED Subsystem */ - public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - void configureLEDMotorControllers() { + /*void configureLEDMotorControllers() { - } + }*/ - /* Swerve Subsystem */ + /* Swerve Subsystem *//* public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); @@ -99,14 +99,14 @@ public class RobotMap { //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - } + }/* /* Climb Subsystem */ - public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO + /*public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO - public final WPI_PigeonIMU gyro = null;//new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO + public final WPI_PigeonIMU gyro = null;//new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO*/ /* Hooks Subsystem */ - public final CANSparkMax leftHook = new CANSparkMax(HooksConstants.LEFT_HOOK_ID, MotorType.kBrushless); - public final CANSparkMax rightHook = new CANSparkMax(HooksConstants.RIGHT_HOOK_ID, MotorType.kBrushless); + public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless); + public final CANSparkMax rightClaw = new CANSparkMax(ClawConstants.RIGHT_CLAW_ID, MotorType.kBrushless); } diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java new file mode 100644 index 0000000..d76c7c1 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -0,0 +1,76 @@ +package frc4388.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMaxLimitSwitch; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ClawConstants; + +public class Claws extends SubsystemBase { + private CANSparkMax m_leftClaw; + private CANSparkMax m_rightClaw; + + private SparkMaxLimitSwitch m_leftLimitSwitchF; + private SparkMaxLimitSwitch m_rightLimitSwitchF; + private SparkMaxLimitSwitch m_leftLimitSwitchR; + private SparkMaxLimitSwitch m_rightLimitSwitchR; + + private double m_leftOffset; + private double m_rightOffset; + + private boolean m_open; + + public Claws(CANSparkMax leftClaw, CANSparkMax rightClaw) { + m_leftClaw = leftClaw; + m_rightClaw = rightClaw; + + m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol + m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + + m_leftLimitSwitchF.enableLimitSwitch(true); + m_rightLimitSwitchF.enableLimitSwitch(true); + m_leftLimitSwitchR.enableLimitSwitch(true); + m_rightLimitSwitchR.enableLimitSwitch(true); + leftClaw.setInverted(true); + rightClaw.setInverted(true); + + m_open = false; + + // m_leftClaw.set(ClawConstants.CALIBRATION_SPEED); + // m_rightClaw.set(ClawConstants.CALIBRATION_SPEED); + } + + public void setSpeed(double speed){ + m_leftClaw.set(speed); + m_rightClaw.set(speed); + } + + public void setOpen(boolean open) { + if(open) { + m_leftClaw.getEncoder().setPosition(ClawConstants.OPEN_POSITION + m_leftOffset); + m_rightClaw.getEncoder().setPosition(ClawConstants.OPEN_POSITION + m_rightOffset); + } else { + m_leftClaw.getEncoder().setPosition(ClawConstants.CLOSE_POSITION + m_leftOffset); + m_rightClaw.getEncoder().setPosition(ClawConstants.CLOSE_POSITION + m_rightOffset); + } + + m_open = open; + } + + public boolean getOpen() { + return m_open; + } + + @Override + public void periodic() { + if(m_leftLimitSwitchR.isPressed()) + m_leftOffset = m_leftClaw.getEncoder().getPosition(); + + if(m_rightLimitSwitchR.isPressed()) + m_rightOffset = m_rightClaw.getEncoder().getPosition(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 2105e92..0956280 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -32,6 +32,7 @@ public class Climber extends SubsystemBase { private boolean m_groundRelative; private double m_robotAngle; private double m_robotPosition; + private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d}; @@ -162,18 +163,11 @@ public class Climber extends SubsystemBase { {Math.cos(theta) - Math.sin(theta), 0 }, {Math.sin(theta) + Math.cos(theta), 0}, {0, 0, 1} - }; - if (m_robotPosition < 45){ - - setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, m_robotAngle); + if (m_robotPosition < m_robotAngle || m_robotPosition > m_robotAngle){ + setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, rotMax, m_robotPosition); } - if( m_robotPosition > 45){ - setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, m_robotAngle); - } - - return Math.toRadians(ypr[1]); // Pitch // multiply by pie and then divide by 180 @@ -254,11 +248,10 @@ public class Climber extends SubsystemBase { setJointAngles(jointAngles); } - public void setRobotAngle(double robotAngle, double robotPosition) { + public void setRobotAngle(double robotAngle, double[][] rotMax, double robotPosition) { m_robotPosition = robotPosition; m_robotAngle = robotAngle; - m_robotAngle = 45; - + m_robotAngle = 45; //45 is placeholder } } diff --git a/src/main/java/frc4388/robot/subsystems/Hooks.java b/src/main/java/frc4388/robot/subsystems/Hooks.java deleted file mode 100644 index 6c6aebf..0000000 --- a/src/main/java/frc4388/robot/subsystems/Hooks.java +++ /dev/null @@ -1,62 +0,0 @@ -package frc4388.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkMaxLimitSwitch; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.HooksConstants; - -public class Hooks extends SubsystemBase { - private CANSparkMax m_leftHook; - private CANSparkMax m_rightHook; - - private SparkMaxLimitSwitch m_leftLimitSwitch; - private SparkMaxLimitSwitch m_rightLimitSwitch; - - private double m_leftOffset; - private double m_rightOffset; - - private boolean m_open; - - public Hooks(CANSparkMax leftHook, CANSparkMax rightHook) { - m_leftHook = leftHook; - m_rightHook = rightHook; - - m_leftLimitSwitch = m_leftHook.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_rightLimitSwitch = m_rightHook.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_leftLimitSwitch.enableLimitSwitch(true); - m_rightLimitSwitch.enableLimitSwitch(true); - - m_open = false; - - m_leftHook.set(HooksConstants.CALIBRATION_SPEED); - m_rightHook.set(HooksConstants.CALIBRATION_SPEED); - } - - public void setOpen(boolean open) { - if(open) { - m_leftHook.getEncoder().setPosition(HooksConstants.OPEN_POSITION + m_leftOffset); - m_rightHook.getEncoder().setPosition(HooksConstants.OPEN_POSITION + m_rightOffset); - } else { - m_leftHook.getEncoder().setPosition(HooksConstants.CLOSE_POSITION + m_leftOffset); - m_rightHook.getEncoder().setPosition(HooksConstants.CLOSE_POSITION + m_rightOffset); - } - - m_open = open; - } - - public boolean getOpen() { - return m_open; - } - - @Override - public void periodic() { - if(m_leftLimitSwitch.isPressed()) - m_leftOffset = m_leftHook.getEncoder().getPosition(); - - if(m_rightLimitSwitch.isPressed()) - m_rightOffset = m_rightHook.getEncoder().getPosition(); - } -}