From b290645b33d638eb0672f1509012c1541eb1cc79 Mon Sep 17 00:00:00 2001 From: Aarav Date: Thu, 2 Mar 2023 16:56:37 -0700 Subject: [PATCH] suicide --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 28 ++++++++++++++----- src/main/java/frc4388/robot/RobotMap.java | 6 ++-- .../java/frc4388/robot/subsystems/Arm.java | 16 +++++++---- 4 files changed, 36 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9d6381f..0c4485f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -103,7 +103,7 @@ public final class Constants { } public static final class GyroConstants { - public static final int ID = 14; // TODO: find the actual ID + public static final int ID = 14; } public static final class ArmConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 78b2aea..ed61e0d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,19 +7,20 @@ package frc4388.robot; +import edu.wpi.first.wpilibj.PWM; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; 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.Arm; import frc4388.robot.commands.AutoBalance; +import frc4388.robot.subsystems.Arm; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.commands.JoystickRecorder; import frc4388.robot.commands.PlaybackChooser; import frc4388.utility.controller.DeadbandedXboxController; -import frc4388.utility.controller.XboxController; +import frc4388.utility.controller.XboxController; /** * This class is where the bulk of the robot should be declared. Since @@ -33,15 +34,13 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder); - // private final LED m_robotLED = new LED(m_robotMap.LEDController); - - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro); + + public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder); /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -50,7 +49,7 @@ public class RobotContainer { /* Autos */ public SendableChooser chooser = new SendableChooser<>(); - // private Command noAuto = new InstantCommand(); + private Command noAuto = new InstantCommand(); // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive); @@ -63,6 +62,8 @@ public class RobotContainer { // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); private PlaybackChooser playbackChooser; + private PWM servo = new PWM(0); + private boolean servo_open = true; /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -76,12 +77,19 @@ public class RobotContainer { true); }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); + + m_robotArm.setDefaultCommand(new RunCommand(() -> { + m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY()); + m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY()); + }, m_robotArm) + .withName("Arm DefaultCommand")); // * Auto Commands // chooser.setDefaultOption("NoAuto", noAuto); // chooser.addOption("Blue1Path", blue1Path); // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance); + // chooser.addOption("Red1Path", red1Path); // chooser.addOption("Red1PathWithBalance", red1PathWithBalance); @@ -121,6 +129,12 @@ public class RobotContainer { .onFalse(new InstantCommand()); // * Operator Buttons + + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> { + servo.setRaw(servo_open ? 1000 : 2000); + servo_open = !servo_open; + })); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 6762ddd..362b401 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -128,9 +128,9 @@ public class RobotMap { } // arm stuff - public WPI_TalonFX pivot = new WPI_TalonFX(-1); // TODO: Add real id - public WPI_TalonFX tele = new WPI_TalonFX(-1); // TODO: Add real id - public CANCoder pivotEncoder = new CANCoder(-1); + public WPI_TalonFX pivot = new WPI_TalonFX(15); + public WPI_TalonFX tele = new WPI_TalonFX(16); + public CANCoder pivotEncoder = new CANCoder(17); public void configArmMotors() { // config factory default diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index ca7b058..3b7503e 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -33,6 +33,11 @@ public class Arm extends SubsystemBase { pivotConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; pivot.configAllSettings(pivotConfig); + pivot.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT); + pivot.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT); + pivot.configForwardSoftLimitEnable(true); + pivot.configReverseSoftLimitEnable(true); + CANCoderConfiguration config = new CANCoderConfiguration(); config.magnetOffsetDegrees = ArmConstants.OFFSET; m_pivotEncoder.configAllSettings(config); @@ -46,11 +51,11 @@ public class Arm extends SubsystemBase { } public void setRotVel(double vel) { - m_pivot.set(ControlMode.Velocity, vel); + m_pivot.set(ControlMode.PercentOutput, vel / 5); } public void setTeleVel(double vel) { - m_tele.set(ControlMode.Velocity, vel); + m_tele.set(ControlMode.PercentOutput, vel); } public void armSetRotation(double rot) { @@ -84,7 +89,7 @@ public class Arm extends SubsystemBase { } public void runPivotTele(double pivot, double tele) { - var rot = 0; + double rot = 0; if (checkLimits(tele, rot)) { armSetRotation(pivot); @@ -112,7 +117,8 @@ public class Arm extends SubsystemBase { @Override public void periodic() { - if (m_debug) - SmartDashboard.putNumber("Arm Motor", m_tele.getSelectedSensorPosition()); + // if (m_debug) + // SmartDashboard.putNumber("Arm Motor", m_tele.getSelectedSensorPosition()); + SmartDashboard.putNumber("Pivot AbsPos", m_pivotEncoder.getAbsolutePosition()); } }