From ab2a3f6866f306d275c9c848aa0d73d6af166e59 Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Fri, 21 Jan 2022 17:20:31 -0700 Subject: [PATCH] Added hooks and did code review stuff Arm will no longer teleport --- src/main/java/frc4388/robot/Constants.java | 17 ++++++++-- .../java/frc4388/robot/RobotContainer.java | 11 +++++- src/main/java/frc4388/robot/RobotMap.java | 6 ++++ .../frc4388/robot/subsystems/Climber.java | 33 +++++++++++------- .../java/frc4388/robot/subsystems/Hooks.java | 34 +++++++++++++++++++ 5 files changed, 85 insertions(+), 16 deletions(-) create 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 d24efca..4d42016 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -21,6 +21,8 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { + public static final double TICKS_PER_ROTATION_FX = 2048; + public static final class SwerveDriveConstants { public static final double ROTATION_SPEED = 0.1; public static final double WHEEL_SPEED = 0.1; @@ -83,11 +85,14 @@ public final class Constants { public static final double UPPER_ARM_LENGTH = 50; // Units should be in cm public static final double LOWER_ARM_LENGTH = 50; - public static final double MAX_ARM_LENGTH = LOWER_ARM_LENGTH + UPPER_ARM_LENGTH; - public static final double MIN_ARM_LENGTH = Math.abs(LOWER_ARM_LENGTH + UPPER_ARM_LENGTH); + public static final double MAX_ARM_LENGTH = 100; + public static final double MIN_ARM_LENGTH = 5; public static final double MOVE_SPEED = 50; // cm per second + public static final double SHOULDER_RESTING_ANGLE = 0; + public static final double ELBOW_RESTING_ANGLE = 0; + // PID Constants public static final int SHOULDER_SLOT_IDX = 0; public static final int SHOULDER_PID_LOOP_IDX = 1; @@ -100,6 +105,14 @@ public final class Constants { public static final int CLIMBER_TIMEOUT_MS = 100; } + + public static final class HooksConstants { + public static final int LEFT_HOOK_ID = -1; + public static final int RIGHT_HOOK_ID = -1; + + public static final double OPEN_POSITION = 0; + public static final double CLOSE_POSITION = 0; + } /** * The OIConstants class contains the ID for the XBox controllers */ diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ef171b2..5df7097 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.Hooks; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; @@ -43,6 +44,9 @@ public class RobotContainer { 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); + + private final Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook); + /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); @@ -62,7 +66,8 @@ public class RobotContainer { // moves climber in xy space with two-axis input from the operator controller m_robotClimber.setDefaultCommand( new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftXAxis(), - getOperatorController().getLeftYAxis()), m_robotClimber)); + getOperatorController().getLeftYAxis(), + getDriverJoystick().getRawButtonPressed(XboxController.A_BUTTON)), 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)); @@ -82,6 +87,10 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + + new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + .whenPressed(() -> m_hooks.setOpen(true)) + .whenReleased(() -> m_hooks.setOpen(false)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 3aa9b0a..f3fbc4a 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -8,8 +8,10 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_PigeonIMU; +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.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; @@ -98,4 +100,8 @@ public class RobotMap { 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 = new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO + + /* Hooks Subsystem */ + public final Servo leftHook = new Servo(HooksConstants.LEFT_HOOK_ID); + public final Servo rightHook = new Servo(HooksConstants.RIGHT_HOOK_ID); } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index a55c329..c234b99 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -5,8 +5,7 @@ /* Safety Hooks -Add -Overextension when lower arm is resting check in processing +Add */ package frc4388.robot.subsystems; @@ -17,6 +16,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.WPI_PigeonIMU; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants; import frc4388.robot.Constants.ClimberConstants; public class Climber extends SubsystemBase { @@ -25,7 +25,7 @@ public class Climber extends SubsystemBase { private WPI_PigeonIMU m_gyro; - private double[] position = {ClimberConstants.MIN_ARM_LENGTH, 0.d}; + private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d}; public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro) { m_shoulder = shoulder; @@ -77,16 +77,20 @@ public class Climber extends SubsystemBase { elbowAngle = Math.PI - elbowAngle; // If the climber is resting on the robot base, rotate the upper arm in the direction of the target - if(shoulderAngle <= 0.d) { - shoulderAngle = 0.d; + if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) { + shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE; double xDiff = xTarget - ClimberConstants.LOWER_ARM_LENGTH; elbowAngle = Math.atan(-yTarget / xDiff); + if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE) + elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE; + + elbowAngle = Math.PI - elbowAngle; - if(xDiff >= 0.d) { + // Deal with negative wraparound + if(xDiff >= 0.d) elbowAngle += Math.PI; - } } return angles; @@ -120,20 +124,23 @@ public class Climber extends SubsystemBase { public void setJointAngles(double shoulderAngle, double elbowAngle) { // Convert radians to ticks - double shoulderPosition = (shoulderAngle * 1024.d) / Math.PI; - double elbowPosition = (elbowAngle * 1024.d) / Math.PI; + double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; + double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; m_shoulder.set(TalonFXControlMode.Position, shoulderPosition); m_elbow.set(TalonFXControlMode.Position, elbowPosition); } - public void controlWithInput(double xInput, double yInput) { - position[0] += xInput * ClimberConstants.MOVE_SPEED; - position[1] += yInput * ClimberConstants.MOVE_SPEED; + public void controlWithInput(double xInput, double yInput, boolean safety) { + if(!safety) + return; + + m_position[0] += xInput * ClimberConstants.MOVE_SPEED; + m_position[1] += yInput * ClimberConstants.MOVE_SPEED; double tiltAngle = getRobotTilt(); - double[] jointAngles = getJointAngles(position[0], position[1], tiltAngle); + double[] jointAngles = getJointAngles(m_position[0], m_position[1], tiltAngle); setJointAngles(jointAngles); } } diff --git a/src/main/java/frc4388/robot/subsystems/Hooks.java b/src/main/java/frc4388/robot/subsystems/Hooks.java new file mode 100644 index 0000000..555bec2 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Hooks.java @@ -0,0 +1,34 @@ +package frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.HooksConstants; + +public class Hooks extends SubsystemBase { + private Servo m_leftHook; + private Servo m_rightHook; + + private boolean m_open; + + public Hooks(Servo leftHook, Servo rightHook) { + m_leftHook = leftHook; + m_rightHook = rightHook; + + m_open = false; + setOpen(m_open); + } + + public void setOpen(boolean open) { + if(open) { + m_leftHook.setPosition(HooksConstants.OPEN_POSITION); + m_rightHook.setPosition(HooksConstants.OPEN_POSITION); + } else { + m_leftHook.setPosition(HooksConstants.CLOSE_POSITION); + m_rightHook.setPosition(HooksConstants.CLOSE_POSITION); + } + } + + public boolean getOpen() { + return m_open; + } +}