diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a16900a..83ac014 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -24,8 +24,8 @@ import frc4388.utility.LEDPatterns; public final class Constants { public static final class SwerveDriveConstants { - public static final double MAX_ROT_SPEED = -0.7; - public static final double MIN_ROT_SPEED = -0.3; + public static final double MAX_ROT_SPEED = 1.5; + public static final double MIN_ROT_SPEED = 0.8; public static double ROTATION_SPEED = MAX_ROT_SPEED; public static final class IDs { @@ -65,7 +65,7 @@ public final class Constants { public static final class Conversions { public static final int CANCODER_TICKS_PER_ROTATION = 4096; - public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 5.8; + public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8; public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8; public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index ac78650..91a0851 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -10,10 +10,16 @@ package frc4388.robot; import java.lang.System; import java.lang.reflect.Array; import java.util.Arrays; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; +import com.ctre.phoenix.motorcontrol.can.MotControllerJNI; + import java.io.File; import java.io.IOException; import java.io.PrintWriter; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -84,13 +90,27 @@ public class Robot extends TimedRobot { * You can use it to reset any subsystem information you want to clear when * the robot is disabled. */ + boolean dis = false; @Override public void disabledInit() { + // m_robotContainer.m_robotClaw.setClaw(false); m_robotTime.endMatchTime(); + m_robotContainer.m_robotClaw.disable(); + + m_handle = m_robotContainer.m_robotMap.leftBackWheel.getHandle(); } + long m_handle = 0; @Override public void disabledPeriodic() { + // if (dis) { + + // MotControllerJNI.Set_4(m_handle, ControlMode.PercentOutput.value, 1, 1, DemandType.Neutral.value); + // // m_robotContainer.m_robotMap.leftBackSteer.set(ControlMode.PercentOutput, 1, DemandType.Neutral, 0); + // // m_robotContainer.m_robotSwerveDrive.driveWithInput(new Translation2d(.5, 0), new Translation2d(), true); + // } + + // System.out.println("hi from disabled"); } /** @@ -118,6 +138,9 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { m_robotContainer.m_robotSwerveDrive.resetGyro(); + dis = true; + + m_robotContainer.m_robotClaw.enable(); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1c5f7e4..920f6cf 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -67,8 +67,6 @@ 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. */ @@ -117,8 +115,8 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); - // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(), m_robotSwerveDrive)); // // .onFalse() new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) @@ -138,16 +136,12 @@ public class RobotContainer { .onFalse(new InstantCommand()); // * Operator Buttons - - // TODO: use the claw subsystem - // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand(() -> { - // servo.setRaw(servo_open ? 1000 : 2000); - // servo_open = !servo_open; - // })); - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> System.out.println("Claw Button"))); .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm)); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4bf5968..3dd4ea2 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -116,6 +116,11 @@ public class RobotMap { rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); // set neutral mode + leftFrontWheel.setNeutralMode(NeutralMode.Brake); + rightFrontWheel.setNeutralMode(NeutralMode.Brake); + leftBackWheel.setNeutralMode(NeutralMode.Brake); + rightBackWheel.setNeutralMode(NeutralMode.Brake); + leftFrontSteer.setNeutralMode(NeutralMode.Brake); rightFrontSteer.setNeutralMode(NeutralMode.Brake); leftBackSteer.setNeutralMode(NeutralMode.Brake); diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 2deaf79..fe8975e 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -10,6 +10,8 @@ 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.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { @@ -42,6 +44,32 @@ public class Arm extends SubsystemBase { CANCoderConfiguration config = new CANCoderConfiguration(); config.magnetOffsetDegrees = ArmConstants.OFFSET; m_pivotEncoder.configAllSettings(config); + // m_pivot.configFactoryDefault(); + // m_pivot.clearStickyFaults(); + // m_pi + + SmartDashboard.putNumber("kP Pivot", 0); + SmartDashboard.putNumber("kI Pivot", 0); + SmartDashboard.putNumber("kD Pivot", 0); + + SmartDashboard.putData("Set PID", new InstantCommand(() -> { + TalonFXConfiguration pivotConfig_ = new TalonFXConfiguration(); + pivotConfig_.slot0.kP = SmartDashboard.getNumber("kP Pivot", 0); + pivotConfig_.slot0.kI = SmartDashboard.getNumber("kI Pivot", 0); + pivotConfig_.slot0.kD = SmartDashboard.getNumber("kD Pivot", 0); + + // pivotConfig_.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); + // pivotConfig_.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; + // pivotConfig_.configSelectedFeedbackFilter();// FeedbackDevice.RemoteSensor0; + m_pivot.configAllSettings(pivotConfig_); + m_pivot.configRemoteFeedbackFilter(encoder.getDeviceID(), RemoteSensorSource.CANCoder, 0); + m_pivot.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); + // m_pivot.selectProfileSlot(0, 0); + })); + + SmartDashboard.putData("set pos 1", new RunCommand(() -> m_pivot.set(ControlMode.Position, 225), this)); + SmartDashboard.putData("set pos 2", new RunCommand(() -> m_pivot.set(ControlMode.Position, 135), this)); + SmartDashboard.putData("Kill Self", new InstantCommand(() -> {}, this)); } public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) { @@ -49,11 +77,11 @@ public class Arm extends SubsystemBase { } public void setRotVel(double vel) { - m_pivot.set(ControlMode.PercentOutput, vel / 5); + m_pivot.set(ControlMode.PercentOutput, .4 * vel); } public void setTeleVel(double vel) { - m_tele.set(ControlMode.PercentOutput, -0.25 * vel); + m_tele.set(ControlMode.PercentOutput, -0.5 * vel); } public void armSetRotation(double rot) { @@ -129,7 +157,8 @@ public class Arm extends SubsystemBase { tele_softLimit = !tele_softLimit; } - boolean resetable = true; + boolean resetable = true; + boolean tele_reset = true; @Override public void periodic() { @@ -141,8 +170,8 @@ public class Arm extends SubsystemBase { SmartDashboard.putNumber("start pivot", pivot_soft); SmartDashboard.putNumber("start tele", tele_soft); - m_pivot.configForwardSoftLimitEnable(true); - m_pivot.configReverseSoftLimitEnable(true); + m_pivot.configForwardSoftLimitEnable(soft_limits); + m_pivot.configReverseSoftLimitEnable(soft_limits); SmartDashboard.putNumber("fwd err", m_pivot.configForwardSoftLimitThreshold(1200 + pivot_soft).value); SmartDashboard.putNumber("rvs err", m_pivot.configReverseSoftLimitThreshold(pivot_soft).value); resetable = false; @@ -150,12 +179,41 @@ public class Arm extends SubsystemBase { resetable = true; } + if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) { + var tele_soft = m_tele.getSelectedSensorPosition(); + m_tele.configForwardSoftLimitThreshold(91000 - tele_soft); + m_tele.configReverseSoftLimitThreshold(1000 - tele_soft); + m_tele.configForwardSoftLimitEnable(true); + m_tele.configReverseSoftLimitEnable(true); + tele_reset = false; + } else if (m_tele.isFwdLimitSwitchClosed() == 0) { + tele_reset = true; + } + + SmartDashboard.putBoolean("reverse", m_tele.isFwdLimitSwitchClosed() == 1); + double x = Math.cos(degrees); - SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED + x * (SwerveDriveConstants.MAX_ROT_SPEED - SwerveDriveConstants.MIN_ROT_SPEED); + // 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 CANCoder", m_pivotEncoder.getAbsolutePosition()); SmartDashboard.putNumber("Pivot IntegratedSensor", m_pivot.getSelectedSensorPosition()); SmartDashboard.putNumber("Telescope Encoder", m_tele.getSelectedSensorPosition()); } + + boolean soft_limits = true; + public void killSoftLimits() { + 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(!soft_limits); + m_pivot.configReverseSoftLimitEnable(!soft_limits); + SmartDashboard.putNumber("fwd err", m_pivot.configForwardSoftLimitThreshold(1200 + pivot_soft).value); + SmartDashboard.putNumber("rvs err", m_pivot.configReverseSoftLimitThreshold(pivot_soft).value); + + soft_limits = !soft_limits; + } } diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java index c2e6b87..8daa6b5 100644 --- a/src/main/java/frc4388/robot/subsystems/Claw.java +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -1,10 +1,12 @@ package frc4388.robot.subsystems; +import edu.wpi.first.hal.PWMJNI; import edu.wpi.first.wpilibj.PWM; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Claw extends SubsystemBase { private PWM m_clawMotor; - private boolean m_open = false; + private boolean m_open = false; + private boolean m_disabled = false; // Opens claw public Claw(PWM m_clawMotor) { @@ -13,16 +15,37 @@ public class Claw extends SubsystemBase { } public void setClaw(boolean open) { + if (m_disabled) return; // Open claw + // m_clawMotor.setRaw(150); m_open = open; - m_clawMotor.setRaw(open ? 0 : 2000); + System.out.println("setClaw()"); + // m_clawMotor.setPosition(0.5); + // m_clawMotor.setRaw(0); + // m_clawMotor.setRaw(m_open ? 0 : 255); + // m_clawMotor.setSpeed(m_open ? -1 : 1); + PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), m_open ? -1 : 1); + // PWMJNI.setPWMDisabled(0); + System.out.println("Claw Pos: " + m_clawMotor.getRaw()); } public void toggle() { + System.out.println("toggle()"); setClaw(!m_open); } public boolean isClawOpen() { return m_open; } + + public void disable() { + m_disabled = true; + // PWMJNI.setPWMRaw(m_clawMotor.getHandle(), PWMJNI.getPWMRaw(m_clawMotor.getHandle())); + PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), 0.5); + // PWMJNI.setPWMDisabled(m_clawMotor.getHandle()); + } + + public void enable() { + m_disabled = false; + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index aa29cdf..fc216a6 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -82,19 +82,21 @@ public class SwerveDrive extends SubsystemBase { public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (fieldRelative) { + + double rot = 0; if (rightStick.getNorm() > 0.1) { rotTarget = gyro.getRotation2d(); + rot = rightStick.getX(); + } else { + 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. + // Use the left joystick to set speed. Apply a cubic 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 * cubedSpeed.getX(), cubedSpeed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); - + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); @@ -235,8 +237,16 @@ public class SwerveDrive extends SubsystemBase { * Shifts gear from high to low, or vice versa. * @param shift true to shift to high, false to shift to low */ - public void highSpeed(boolean shift) { - this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + // public void highSpeed(boolean shift) { + // this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + // } + + public void toggleGear() { + if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) { + this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + } else { + this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + } } }