From 470552c575b22a7b7f0bd0addf94978cfda7e1e9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 20:01:32 -0600 Subject: [PATCH] basic climber code (DRIVE STILL NOT WORKING) --- .../java/frc4388/robot/RobotContainer.java | 36 ++++++++++++------- .../frc4388/robot/subsystems/Climber.java | 24 +++++++++++-- .../frc4388/robot/subsystems/SwerveDrive.java | 2 +- 3 files changed, 46 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 559fcaf..1e59a82 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -29,6 +29,7 @@ import java.util.regex.Matcher; import java.util.regex.Pattern; import java.util.stream.Collectors; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.diffplug.common.base.Errors; import com.pathplanner.lib.PathPlanner; @@ -114,9 +115,10 @@ public class RobotContainer { public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); - private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30); - private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(31); - public final Climber m_robotClimber = new Climber(testElbowMotor); + + private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30); + private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31); + public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor); // Controllers private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -145,7 +147,6 @@ public class RobotContainer { */ public RobotContainer() { configureButtonBindings(); - // testShoulderMotor.setNeutralMode(NeutralMode.Brake); /* Default Commands */ // Swerve Drive with Input @@ -170,9 +171,9 @@ public class RobotContainer { // m_robotBoomBoom, // m_robotTurret).withName("Storage ManageStorage defaultCommand")); - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber) - // ); + m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.runBothMotorsWithInputs(getDriverController().getLeftY() * 0.1, getDriverController().getRightY() * 0.1), m_robotClimber) + ); // Storage Management /*m_robotStorage.setDefaultCommand( new RunCommand(() -> m_robotStorage.manageStorage(), @@ -186,7 +187,6 @@ public class RobotContainer { new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - m_robotHood.setDefaultCommand( new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); // m_robotTurret.setDefaultCommand( @@ -215,11 +215,19 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kStart.value) .whenPressed(m_robotSwerveDrive::resetGyro); // Left Bumper > Shift Down - new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) - .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); - // Right Bumper > Shift Up - new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) - .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + // new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) + // .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); + // // Right Bumper > Shift Up + // new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) + // .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + + new JoystickButton(getDriverController(), XboxController.Axis.kLeftTrigger.value) + .whileHeld(new RunCommand(() -> m_robotClimber.runShoulderWithInput(0.1), m_robotClimber)) + .whenReleased(new RunCommand(() -> m_robotClimber.runShoulderWithInput(0.0))); + + new JoystickButton(getDriverController(), XboxController.Axis.kRightTrigger.value) + .whileHeld(new RunCommand(() -> m_robotClimber.runShoulderWithInput(-0.1), m_robotClimber)) + .whenReleased(new RunCommand(() -> m_robotClimber.runShoulderWithInput(0.0))); // new JoystickButton(getDriverController(), XboxController.Button.kA.value) // .whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025)); @@ -238,6 +246,8 @@ public class RobotContainer { // .whenPressed(() -> m_robotMap.leftBack.reset()) // .whenPressed(() -> m_robotMap.rightBack.reset()); + + /* Operator Buttons */ // X > Extend Intake diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 052d024..7425346 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -4,21 +4,41 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Climber extends SubsystemBase { + + WPI_TalonFX m_climberShoulder; WPI_TalonFX m_climberElbow; + /** Creates a new Climber. */ - public Climber(WPI_TalonFX climberElbow) { + public Climber(WPI_TalonFX climberShoulder, WPI_TalonFX climberElbow) { + m_climberShoulder = climberShoulder; m_climberElbow = climberElbow; + + m_climberShoulder.configFactoryDefault(); + m_climberElbow.configFactoryDefault(); + m_climberShoulder.setNeutralMode(NeutralMode.Brake); + m_climberElbow.setNeutralMode(NeutralMode.Brake); } - public void runWithInput(double input){ + public void runShoulderWithInput(double input) { + m_climberShoulder.set(input); + } + + public void runElbowWithInput(double input){ m_climberElbow.set(input); } + + public void runBothMotorsWithInputs(double shoulderInput, double elbowInput) { + m_climberShoulder.set(shoulderInput); + m_climberElbow.set(elbowInput); + } + @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index e1b8fae..9a4a5e9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -122,7 +122,7 @@ public class SwerveDrive extends SubsystemBase { double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(m_gyro.getRotation2d().getRadians() + (Math.PI/2))) + rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2))) : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 8); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( chassisSpeeds);