diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0c4485f..a16900a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -24,7 +24,9 @@ import frc4388.utility.LEDPatterns; public final class Constants { public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = -0.7; + public static final double MAX_ROT_SPEED = -0.7; + public static final double MIN_ROT_SPEED = -0.3; + public static double ROTATION_SPEED = MAX_ROT_SPEED; public static final class IDs { public static final int LEFT_FRONT_WHEEL_ID = 2; @@ -114,10 +116,10 @@ public final class Constants { public static final double TELE_TICKS_PER_SECOND = (-5); - public static final double PIVOT_FORWARD_SOFT_LIMIT = -1.0; // TODO: find actual value - public static final double PIVOT_REVERSE_SOFT_LIMIT = -1.0; // TODO: find actual value + public static final double PIVOT_FORWARD_SOFT_LIMIT = 100; // TODO: find actual value + public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; // TODO: find actual value - public static final double TELE_FORWARD_SOFT_LIMIT = 0; + public static final double TELE_FORWARD_SOFT_LIMIT = 100; public static final double TELE_REVERSE_SOFT_LIMIT = 0; public static final double kP = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ed61e0d..1444e27 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.AutoBalance; +import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.subsystems.Arm; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.commands.JoystickRecorder; @@ -128,6 +129,10 @@ public class RobotContainer { "Blue1Path.txt")) .onFalse(new InstantCommand()); + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) + .onFalse(new InstantCommand()); + // * Operator Buttons new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) @@ -135,6 +140,9 @@ public class RobotContainer { servo.setRaw(servo_open ? 1000 : 2000); servo_open = !servo_open; })); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit())); } /** diff --git a/src/main/java/frc4388/robot/commands/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/PlaybackChooser.java index af7f4ac..c2b32e4 100644 --- a/src/main/java/frc4388/robot/commands/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/PlaybackChooser.java @@ -42,7 +42,7 @@ public class PlaybackChooser { nextChooser(); Shuffleboard.getTab("Auto Chooser") - .add("Add Sequence", new InstantCommand(() -> {})) + .add("Add Sequence", new InstantCommand(() -> nextChooser())) .withPosition(4, 0); return this; } diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 3b7503e..2deaf79 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoderConfiguration; import frc4388.robot.Constants.ArmConstants; +import frc4388.robot.Constants.SwerveDriveConstants; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -23,6 +24,9 @@ public class Arm extends SubsystemBase { m_pivot = pivot; m_pivotEncoder = encoder; + tele.configFactoryDefault(); + m_pivot.configFactoryDefault(); + TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); pivotConfig.slot0.kP = ArmConstants.kP; pivotConfig.slot0.kI = ArmConstants.kI; @@ -31,19 +35,13 @@ public class Arm extends SubsystemBase { pivotConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); pivotConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; pivotConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; - pivot.configAllSettings(pivotConfig); + m_pivot.configAllSettings(pivotConfig); - pivot.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT); - pivot.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT); - pivot.configForwardSoftLimitEnable(true); - pivot.configReverseSoftLimitEnable(true); + resetTeleSoftLimit(); CANCoderConfiguration config = new CANCoderConfiguration(); config.magnetOffsetDegrees = ArmConstants.OFFSET; m_pivotEncoder.configAllSettings(config); - - tele.configFactoryDefault(); - pivot.configFactoryDefault(); } public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) { @@ -55,7 +53,7 @@ public class Arm extends SubsystemBase { } public void setTeleVel(double vel) { - m_tele.set(ControlMode.PercentOutput, vel); + m_tele.set(ControlMode.PercentOutput, -0.25 * vel); } public void armSetRotation(double rot) { @@ -115,10 +113,49 @@ public class Arm extends SubsystemBase { return true; } + boolean tele_softLimit = false; + public void resetTeleSoftLimit() { + if (!tele_softLimit) { + var tele_soft = m_tele.getSelectedSensorPosition(); + m_tele.configForwardSoftLimitThreshold(91000 - tele_soft); + m_tele.configReverseSoftLimitThreshold(tele_soft); + m_tele.configForwardSoftLimitEnable(true); + m_tele.configReverseSoftLimitEnable(true); + } else { + m_tele.configForwardSoftLimitEnable(false); + m_tele.configReverseSoftLimitEnable(false); + } + + tele_softLimit = !tele_softLimit; + } + + boolean resetable = true; + @Override public void periodic() { + double degrees = Math.abs(m_pivotEncoder.getAbsolutePosition() - 135); + if (degrees < 2 && resetable) { + var pivot_soft = m_pivot.getSelectedSensorPosition(); + var tele_soft = m_tele.getSelectedSensorPosition(); + + SmartDashboard.putNumber("start pivot", pivot_soft); + SmartDashboard.putNumber("start tele", tele_soft); + + m_pivot.configForwardSoftLimitEnable(true); + m_pivot.configReverseSoftLimitEnable(true); + SmartDashboard.putNumber("fwd err", m_pivot.configForwardSoftLimitThreshold(1200 + pivot_soft).value); + SmartDashboard.putNumber("rvs err", m_pivot.configReverseSoftLimitThreshold(pivot_soft).value); + resetable = false; + } else if (degrees > 2) { + resetable = true; + } + + double x = Math.cos(degrees); + SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED + x * (SwerveDriveConstants.MAX_ROT_SPEED - SwerveDriveConstants.MIN_ROT_SPEED); // if (m_debug) // SmartDashboard.putNumber("Arm Motor", m_tele.getSelectedSensorPosition()); - SmartDashboard.putNumber("Pivot AbsPos", m_pivotEncoder.getAbsolutePosition()); + SmartDashboard.putNumber("Pivot CANCoder", m_pivotEncoder.getAbsolutePosition()); + SmartDashboard.putNumber("Pivot IntegratedSensor", m_pivot.getSelectedSensorPosition()); + SmartDashboard.putNumber("Telescope Encoder", m_tele.getSelectedSensorPosition()); } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index ac27c3b..aa29cdf 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -83,16 +83,17 @@ public class SwerveDrive extends SubsystemBase { public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (fieldRelative) { if (rightStick.getNorm() > 0.1) { - rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1)); + rotTarget = gyro.getRotation2d(); } - double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); + double rot = rightStick.getX(); // Use the left joystick to set speed. Apply a quadratic curve and the set max speed. Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds.