From 6db9636f222944a66db6b060bde2594923b684c3 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 14:13:37 -0600 Subject: [PATCH] softlims --- src/main/java/frc4388/robot/Constants.java | 4 +- src/main/java/frc4388/robot/Robot.java | 1 + .../java/frc4388/robot/RobotContainer.java | 49 ++++++++++++++++--- .../robot/subsystems/ClimberRewrite.java | 32 +++++++++--- 4 files changed, 71 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d9bf54d..7a06fd9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -194,9 +194,9 @@ public final class Constants { public static final double ELBOW_GB_RATIO = 1.d; public static final double SHOULDER_GB_RATIO = 1.d; - public static final double SHOULDER_SOFT_LIMIT_FORWARD = 0; + public static final double SHOULDER_SOFT_LIMIT_FORWARD = 53869; public static final double SHOULDER_SOFT_LIMIT_REVERSE = 0; - public static final double ELBOW_SOFT_LIMIT_FORWARD = 0; + public static final double ELBOW_SOFT_LIMIT_FORWARD = 281717; public static final double ELBOW_SOFT_LIMIT_REVERSE = 0; // PID Constants diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 759e2c9..f01d45a 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -124,6 +124,7 @@ public class Robot extends TimedRobot { }); desmosServer.start(); + m_robotContainer.m_robotVisionOdometry.setLEDs(false); // DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0}); } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5b4f7b7..5cb54c8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -112,10 +112,14 @@ public class RobotContainer { /* Subsystems */ public final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); +<<<<<<< Updated upstream private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); // Subsystems +======= + public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); +>>>>>>> Stashed changes public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor); @@ -139,7 +143,7 @@ public class RobotContainer { // Autonomous private PathPlannerTrajectory loadedPathTrajectory = null; - // private final ListeningSendableChooser autoChooser = new ListeningSendableChooser<>(this::loadPath); + private final ListeningSendableChooser autoChooser = new ListeningSendableChooser<>(this::loadPath); private final List pathPoints = new ArrayList<>(); private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault(); private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording"); @@ -199,6 +203,7 @@ public class RobotContainer { m_robotSerializer.setDefaultCommand( new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); +<<<<<<< Updated upstream // Turret Manual @@ -212,9 +217,15 @@ public class RobotContainer { .until(() -> this.currentMode.equals(CurrentMode.TURRET))); // EnableTurret() +======= + // Turret Manual + // m_robotTurret.setDefaultCommand( + // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), + // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); +>>>>>>> Stashed changes - m_robotHood.setDefaultCommand( - new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); + // m_robotHood.setDefaultCommand( + // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -268,10 +279,10 @@ public class RobotContainer { .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); //Toggles extender in and out - new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret)) .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret)); @@ -292,7 +303,7 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)); //B > Shoot with Lime - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))) .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); @@ -305,15 +316,18 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) + .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) - .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)); + .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) @@ -411,6 +425,7 @@ public class RobotContainer { * Finally, adds the existing path files to the auto chooser */ private void autoInit() { +<<<<<<< Updated upstream // try { // WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), // StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, @@ -429,6 +444,26 @@ public class RobotContainer { // .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) // .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); // SmartDashboard.putData("Auto Chooser", autoChooser); +======= + try { + WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), + StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, + StandardWatchEventKinds.ENTRY_DELETE); + // TODO: Store this and other commands as fields so they can be rescheduled. + new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }.withName("Path Watcher").schedule(); + } catch (IOException exception) { + LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception); + } + Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles()) + .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) + .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); + SmartDashboard.putData("Auto Chooser", autoChooser); +>>>>>>> Stashed changes } /** diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index 4d81b38..a9d208a 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -17,6 +17,7 @@ import com.ctre.phoenix.sensors.WPI_Pigeon2; import org.opencv.core.Point; import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants; import frc4388.robot.Constants.ClimberConstants; @@ -51,16 +52,16 @@ public class ClimberRewrite extends SubsystemBase { // shoulderStartPosition = m_shoulder.getSelectedSensorPosition(); // elbowStartPosition = m_elbow.getSelectedSensorPosition(); - m_shoulder.setSelectedSensorPosition(((ClimberConstants.SHOULDER_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO); - m_elbow.setSelectedSensorPosition(((ClimberConstants.ELBOW_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO); + // m_shoulder.setSelectedSensorPosition(((ClimberConstants.SHOULDER_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO); + // m_elbow.setSelectedSensorPosition(((ClimberConstants.ELBOW_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO); m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD); - m_elbow.configForwardSoftLimitEnable(false); - m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); - m_elbow.configReverseSoftLimitEnable(false); + m_elbow.configForwardSoftLimitEnable(true); + // m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); + // m_elbow.configReverseSoftLimitEnable(true); m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD); - m_shoulder.configForwardSoftLimitEnable(false); + m_shoulder.configForwardSoftLimitEnable(true); m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE); m_shoulder.configReverseSoftLimitEnable(false); @@ -174,6 +175,8 @@ public class ClimberRewrite extends SubsystemBase { @Override public void periodic() { + SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); + SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); // setJointAngles(jointAngles); } @@ -283,7 +286,24 @@ public class ClimberRewrite extends SubsystemBase { return getClimberPosition(jointAngles[0], jointAngles[1]); } +<<<<<<< Updated upstream public double getCurrent() { return (this.m_elbow.getSupplyCurrent() + this.m_shoulder.getSupplyCurrent()); } +======= + public void setClimberSoftLimits(boolean set){ + m_elbow.configForwardSoftLimitEnable(set); + m_shoulder.configForwardSoftLimitEnable(set); + } + + public void setEncoders(double value){ + m_elbow.setSelectedSensorPosition(value); + m_shoulder.setSelectedSensorPosition(value); + } + + public double getCurrent(){ + return (m_shoulder.getSupplyCurrent() + m_elbow.getSupplyCurrent()); + } + +>>>>>>> Stashed changes }