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..fbdf90c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -10,15 +10,22 @@ 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; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc4388.utility.DeferredBlock; import frc4388.utility.RobotTime; import frc4388.robot.subsystems.Location; @@ -38,9 +45,6 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - private Location location = new Location(); - - /** * This function is run when the robot is first started up and should be * used for any initialization code. @@ -65,18 +69,13 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); - - final Tag pos = location.getPosRot(); - if (pos != null) { - SmartDashboard.putNumber("x position", pos.x); - } - - //ystem.out.print(apriltagPos[0]); } /** @@ -90,7 +89,12 @@ public class Robot extends TimedRobot { } @Override - public void disabledPeriodic() { + public void disabledPeriodic() {} + + @Override + public void disabledExit() { + DeferredBlock.execute(); + super.disabledExit(); } /** @@ -134,13 +138,11 @@ public class Robot extends TimedRobot { * This function is called periodically during operator control. */ @Override - public void teleopPeriodic() { - } + public void teleopPeriodic() {} /** * This function is called periodically during test mode. */ @Override - public void testPeriodic() { - } + public void testPeriodic() {} } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1444e27..1b6fb4c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,7 +7,10 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; + import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -17,8 +20,10 @@ import frc4388.robot.Constants.*; import frc4388.robot.commands.AutoBalance; import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.subsystems.Arm; +import frc4388.robot.subsystems.Claw; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.commands.JoystickRecorder; +import frc4388.robot.commands.PivotCommand; import frc4388.robot.commands.PlaybackChooser; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.XboxController; @@ -43,6 +48,8 @@ public class RobotContainer { public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder); + public final Claw m_robotClaw = new Claw(m_robotMap.servo); + /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -63,8 +70,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. */ @@ -113,8 +118,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) @@ -134,15 +139,29 @@ public class RobotContainer { .onFalse(new InstantCommand()); // * Operator Buttons + // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + // // .onTrue(new InstantCommand(() -> System.out.println("Claw Button"))); + // .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> { - servo.setRaw(servo_open ? 1000 : 2000); - servo_open = !servo_open; - })); + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit())); + .onTrue(new PivotCommand(m_robotArm, 135)); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + .onTrue(new PivotCommand(m_robotArm, 210)); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_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)); + + // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 362b401..3dd4ea2 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -12,6 +12,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_Pigeon2; +import edu.wpi.first.wpilibj.PWM; import frc4388.robot.Constants.ArmConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; @@ -115,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); @@ -160,4 +166,8 @@ public class RobotMap { tele.configForwardSoftLimitEnable(false); tele.configReverseSoftLimitEnable(false); } + + // claw stuff (WHAT IS A SOAP ENGINEER) + PWM servo = new PWM(0); + } diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java index 2da529c..cace0a8 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -14,9 +14,9 @@ public class AutoBalance extends PelvicInflammatoryDisease { RobotGyro gyro; SwerveDrive drive; - /** Creates a new AutoBalanceTF2. */ + /** Creates a new AutoBalance. */ public AutoBalance(RobotGyro gyro, SwerveDrive drive) { - super(0.6, 0, 0, 0); + super(0.6, 0, 0, 0, 0); this.gyro = gyro; this.drive = drive; diff --git a/src/main/java/frc4388/robot/commands/LimeAlign.java b/src/main/java/frc4388/robot/commands/LimeAlign.java new file mode 100644 index 0000000..c811a3e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/LimeAlign.java @@ -0,0 +1,34 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; + +public class LimeAlign extends CommandBase { + public LimeAlign(SwerveDrive drive) { + addRequirements(drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java index fb6a871..6c54024 100644 --- a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java +++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java @@ -4,20 +4,24 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.utility.Gains; public abstract class PelvicInflammatoryDisease extends CommandBase { - protected Gains gains; - private double output = 0; + protected Gains gains; + private double output = 0; + private double tolerance = 0; /** Creates a new PelvicInflammatoryDisease. */ - public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf) { - gains = new Gains(kp, ki, kd, kf, 0); + public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf, double tolerance) { + gains = new Gains(kp, ki, kd, kf, 0); + this.tolerance = tolerance; } - public PelvicInflammatoryDisease(Gains gains) { - this.gains = gains; + public PelvicInflammatoryDisease(Gains gains, double tolerance) { + this.gains = gains; + this.tolerance = tolerance; } /** produces the error from the setpoint */ @@ -48,11 +52,9 @@ public abstract class PelvicInflammatoryDisease extends CommandBase { runWithOutput(output); } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - // Returns true when the command should end. @Override - public boolean isFinished() { return false; } + public boolean isFinished() { + return Math.abs(getError()) < tolerance; + } } diff --git a/src/main/java/frc4388/robot/commands/PivotCommand.java b/src/main/java/frc4388/robot/commands/PivotCommand.java new file mode 100644 index 0000000..a84690d --- /dev/null +++ b/src/main/java/frc4388/robot/commands/PivotCommand.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.subsystems.Arm; + +public class PivotCommand extends PelvicInflammatoryDisease { + private final Arm arm; + private final double target; + + /** Creates a new ArmCommand. */ + public PivotCommand(Arm arm, double target) { + super(6, 1.5, 0, 0, 0.015); + this.arm = arm; + this.target = target; + addRequirements(arm); + } + + @Override + public double getError() { + return (target - arm.getArmRotation()) / 360; + } + + @Override + public void runWithOutput(double output) { + SmartDashboard.putNumber("pivot output", output); + arm.setRotVel(output); + } +} diff --git a/src/main/java/frc4388/robot/commands/TeleCommand.java b/src/main/java/frc4388/robot/commands/TeleCommand.java new file mode 100644 index 0000000..02c8c27 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/TeleCommand.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import frc4388.robot.Constants.ArmConstants; +import frc4388.robot.subsystems.Arm; + +public class TeleCommand extends PelvicInflammatoryDisease { + private final Arm arm; + private final double target; + + /** Creates a new ArmCommand. */ + public TeleCommand(Arm arm, double target) { + super(0.6, 0, 0, 0, 0); + this.arm = arm; + this.target = target; + addRequirements(arm); + } + + @Override + public double getError() { + return (arm.getArmLength() - target) / + (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT); + } + + @Override + public void runWithOutput(double output) { + arm.setTeleVel(output); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 015793e..06fab10 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -5,18 +5,22 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; +import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; 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 frc4388.utility.DeferredBlock; 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 { private WPI_TalonFX m_tele; - private WPI_TalonFX m_pivot; + public WPI_TalonFX m_pivot; private CANCoder m_pivotEncoder; private boolean m_debug; @@ -27,22 +31,12 @@ public class Arm extends SubsystemBase { m_pivotEncoder = encoder; m_tele.configFactoryDefault(); - // m_tele.configReverseLimitSwitchSource(null, null); m_tele.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); m_tele.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); m_pivot.configFactoryDefault(); - TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); - pivotConfig.slot0.kP = ArmConstants.kP; - pivotConfig.slot0.kI = ArmConstants.kI; - pivotConfig.slot0.kD = ArmConstants.kD; - - pivotConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); - pivotConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; - pivotConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; - m_pivot.configAllSettings(pivotConfig); - - resetTeleSoftLimit(); + // * Example of deferred code + new DeferredBlock(() -> resetTeleSoftLimit()); CANCoderConfiguration config = new CANCoderConfiguration(); config.magnetOffsetDegrees = ArmConstants.OFFSET; @@ -54,11 +48,21 @@ public class Arm extends SubsystemBase { } public void setRotVel(double vel) { - m_pivot.set(ControlMode.PercentOutput, vel / 5); + var degrees = Math.abs(getArmRotation()) - 135; + SmartDashboard.putNumber("arm degrees", degrees); + SmartDashboard.putNumber("arm rot vel", vel); + + if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) { + m_pivot.set(ControlMode.PercentOutput, 0); + } else if (degrees > 90 && vel > 0) { + m_pivot.set(ControlMode.PercentOutput, .15 * vel); + } else { + m_pivot.set(ControlMode.PercentOutput, .3 * 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) { @@ -82,21 +86,21 @@ public class Arm extends SubsystemBase { } public double getArmLength() { - return (m_tele.getSelectedSensorPosition() - ArmConstants.TELE_FORWARD_SOFT_LIMIT) / - (ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT); + return m_tele.getSelectedSensorPosition(); } public double getArmRotation() { - return (m_pivotEncoder.getAbsolutePosition() - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) / - (ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT); + return m_pivotEncoder.getAbsolutePosition(); } public void runPivotTele(double pivot, double tele) { - double rot = 0; + double abs_pivot = Math.toRadians(getArmRotation() - 135); + double abs_tele = (getArmLength() - ArmConstants.TELE_REVERSE_SOFT_LIMIT) / + (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT); - if (checkLimits(tele, rot)) { - armSetRotation(pivot); - armSetLength(tele); + if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) { + setRotVel(pivot); + setTeleVel(tele); } } @@ -112,10 +116,7 @@ public class Arm extends SubsystemBase { var y = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta); var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x)); - if (y < minHeight) - return false; - - return true; + return y < minHeight; } boolean tele_softLimit = false; @@ -134,33 +135,36 @@ public class Arm extends SubsystemBase { tele_softLimit = !tele_softLimit; } - boolean resetable = true; + boolean resetable = true; + boolean tele_reset = 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; + + 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; } - double x = Math.cos(Math.toRadians(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 CANCoder", m_pivotEncoder.getAbsolutePosition()); - SmartDashboard.putNumber("Pivot IntegratedSensor", m_pivot.getSelectedSensorPosition()); - SmartDashboard.putNumber("Telescope Encoder", m_tele.getSelectedSensorPosition()); + // double x = Math.cos(Math.toRadians(degrees)); + } + + boolean soft_limits = true; + public void killSoftLimits() { + resetTeleSoftLimit(); + var pivot_soft = m_pivot.getSelectedSensorPosition(); + var tele_soft = m_tele.getSelectedSensorPosition(); + + m_pivot.configForwardSoftLimitEnable(!soft_limits); + m_pivot.configReverseSoftLimitEnable(!soft_limits); + + 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 37793b1..7d6d12c 100644 --- a/src/main/java/frc4388/robot/subsystems/Claw.java +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -1,9 +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 { +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) { @@ -14,10 +17,15 @@ public class Claw { public void setClaw(boolean open) { // Open claw m_open = open; - m_clawMotor.setRaw(open ? 0 : 2000); + m_clawMotor.setRaw(m_open ? 1000 : 2000); } + public void toggle() { + setClaw(!m_open); + } + public boolean isClawOpen() { return m_open; } -} \ 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..fcaadb9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -35,10 +35,6 @@ public class SwerveDrive extends SubsystemBase { private RobotGyro gyro; - // private SwerveDriveOdometry odometry; - - private SwerveDrivePoseEstimator poseEstimator; - public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default public Rotation2d rotTarget = new Rotation2d(); @@ -53,48 +49,26 @@ public class SwerveDrive extends SubsystemBase { this.gyro = gyro; - // this.odometry = new SwerveDriveOdometry( - // kinematics, - // gyro.getRotation2d(), - // new SwerveModulePosition[] { - // leftFront.getPosition(), - // rightFront.getPosition(), - // leftBack.getPosition(), - // rightBack.getPosition() - // }, - // getOdometry() - // ); - - this.poseEstimator = new SwerveDrivePoseEstimator( - kinematics, - gyro.getRotation2d(), - new SwerveModulePosition[] { - leftFront.getPosition(), - rightFront.getPosition(), - leftBack.getPosition(), - rightBack.getPosition() - }, - new Pose2d(0,0, new Rotation2d(0)) - ); - this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } 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); @@ -121,82 +95,8 @@ public class SwerveDrive extends SubsystemBase { public void resetGyro() { gyro.reset(); - // setOdometry(getOdometry()); rotTarget = new Rotation2d(0); } - - /** - * Updates the odometry of the SwerveDrive. - */ - // public void updateOdometry() { - // odometry.update( - // gyro.getRotation2d(), - // new SwerveModulePosition[] { - // leftFront.getPosition(), - // rightFront.getPosition(), - // leftBack.getPosition(), - // rightBack.getPosition() - // } - // ); - // } - - public void updatePoseEstimator() { - poseEstimator.update( - gyro.getRotation2d(), - new SwerveModulePosition[] { - leftFront.getPosition(), - rightFront.getPosition(), - leftBack.getPosition(), - rightBack.getPosition() - } - ); - } - - /** - * Gets the odometry of the SwerveDrive. - * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta). - */ - // public Pose2d getOdometry() { - // return odometry.getPoseMeters(); - // } - - public Pose2d getPoseEstimator() { - return poseEstimator.getEstimatedPosition(); - } - - /** - * Sets the odometry of the SwerveDrive. - * @param pose Pose to set the odometry to. - */ - // public void setOdometry(Pose2d pose) { - // odometry.resetPosition( - // gyro.getRotation2d(), - // new SwerveModulePosition[] { - // leftFront.getPosition(), - // rightFront.getPosition(), - // leftBack.getPosition(), - // rightBack.getPosition() - // }, - // pose - // ); - // } - - public void setPoseEstimator(Pose2d pose) { - poseEstimator.resetPosition( - gyro.getRotation2d(), - new SwerveModulePosition[] { - leftFront.getPosition(), - rightFront.getPosition(), - leftBack.getPosition(), - rightBack.getPosition() - }, - pose - ); - } - - public void resetPoseEstimator() { - setPoseEstimator(new Pose2d()); - } public void stopModules() { for (SwerveModule module : this.modules) { @@ -204,14 +104,6 @@ public class SwerveDrive extends SubsystemBase { } } - /** - * Resets the odometry of the SwerveDrive to 0. - * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions. - */ - // public void resetOdometry() { - // setOdometry(new Pose2d()); - // } - public SwerveDriveKinematics getKinematics() { return this.kinematics; } @@ -219,24 +111,14 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - - // updateOdometry(); - updatePoseEstimator(); - - // SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX())); - // SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY())); - // SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees()); - - // SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); - // SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees()); } - /** - * 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 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; + } } } diff --git a/src/main/java/frc4388/utility/DeferredBlock.java b/src/main/java/frc4388/utility/DeferredBlock.java new file mode 100644 index 0000000..20d3c57 --- /dev/null +++ b/src/main/java/frc4388/utility/DeferredBlock.java @@ -0,0 +1,23 @@ +package frc4388.utility; + +import java.util.ArrayList; + +public class DeferredBlock { + private static ArrayList m_blocks = new ArrayList<>(); + private static boolean m_hasRun = false; + + public DeferredBlock(Runnable block) { + m_blocks.add(block); + } + + public static void execute() { + if (m_hasRun) return; + + for (Runnable block : m_blocks) { + block.run(); + } + + m_blocks.clear(); // for garbage collection + m_hasRun = true; + } +}