diff --git a/build.gradle b/build.gradle index c1f07ee..7e60634 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2022.1.1" + id "edu.wpi.first.GradleRIO" version "2022.2.1" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4d42016..757383c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -77,8 +77,8 @@ public final class Constants { public static final class ClimberConstants { /* TODO: Update motor IDS */ - public static final int SHOULDER_ID = -1; - public static final int ELBOW_ID = -1; + 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 @@ -88,7 +88,7 @@ public final class Constants { 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 MOVE_SPEED = .2; // cm per second public static final double SHOULDER_RESTING_ANGLE = 0; public static final double ELBOW_RESTING_ANGLE = 0; @@ -103,7 +103,7 @@ public final class Constants { public static final Gains SHOULDER_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains ELBOW_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); - public static final int CLIMBER_TIMEOUT_MS = 100; + public static final int CLIMBER_TIMEOUT_MS = 50; } public static final class HooksConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5df7097..9b8923b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -43,9 +43,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 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 Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -59,15 +59,14 @@ public class RobotContainer { /* Default Commands */ // drives the swerve drive with a two-axis input from the driver controller - m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), - getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); + // m_robotSwerveDrive.setDefaultCommand( + // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), + // getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); // 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(), - getDriverJoystick().getRawButtonPressed(XboxController.A_BUTTON)), m_robotClimber)); + 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)); @@ -88,9 +87,9 @@ public class RobotContainer { .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)); + // 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 f3fbc4a..6de2f65 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -99,9 +99,9 @@ public class RobotMap { /* Climb Subsystem */ 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 + public final WPI_PigeonIMU gyro = null;//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); + //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 c234b99..032a85f 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -24,10 +24,11 @@ public class Climber extends SubsystemBase { private WPI_TalonFX m_elbow; private WPI_PigeonIMU m_gyro; + private boolean m_groundRelative; private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d}; - public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro) { + public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro, boolean groundRelative) { m_shoulder = shoulder; m_shoulder.configFactoryDefault(); m_shoulder.setNeutralMode(NeutralMode.Brake); @@ -36,7 +37,12 @@ public class Climber extends SubsystemBase { m_elbow.configFactoryDefault(); m_elbow.setNeutralMode(NeutralMode.Brake); - m_gyro = gyro; + setClimberGains(); + + if(groundRelative) + m_gyro = gyro; + + m_groundRelative = groundRelative; } /* getJointAngles @@ -93,6 +99,8 @@ public class Climber extends SubsystemBase { elbowAngle += Math.PI; } + angles[0] = shoulderAngle; + angles[1] = elbowAngle; return angles; } @@ -119,11 +127,14 @@ public class Climber extends SubsystemBase { } public void setJointAngles(double[] angles) { + System.out.println(angles); setJointAngles(angles[0], angles[1]); } public void setJointAngles(double shoulderAngle, double elbowAngle) { // Convert radians to ticks + System.out.println("angles: " + shoulderAngle + ", " + elbowAngle); + double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; @@ -131,14 +142,13 @@ public class Climber extends SubsystemBase { m_elbow.set(TalonFXControlMode.Position, elbowPosition); } - public void controlWithInput(double xInput, double yInput, boolean safety) { - if(!safety) - return; - + public void controlWithInput(double xInput, double yInput) { m_position[0] += xInput * ClimberConstants.MOVE_SPEED; m_position[1] += yInput * ClimberConstants.MOVE_SPEED; - double tiltAngle = getRobotTilt(); + System.out.println("x: " + m_position[0] + " y: " + m_position[1]); + + double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d; double[] jointAngles = getJointAngles(m_position[0], m_position[1], tiltAngle); setJointAngles(jointAngles);