From 320f036c20db902b81e2cd0195be091da5bd728f Mon Sep 17 00:00:00 2001 From: Luca Barreith Date: Sat, 20 Jan 2024 15:17:28 -0800 Subject: [PATCH 01/51] Linked PID variables to SmartDashboard Added controllerID1 and controllerID2 constants end now sets motor speed to 0 Added setPointTolerance constant isFinished is now true when error is within setPointTolerance --- .../frc/robot/commands/ClimberCommand.java | 51 ++++++++++++ .../frc/robot/constants/RobotConstants.java | 9 +- .../frc/robot/subsystems/climber/Climber.java | 83 +++++++++++++++++-- 3 files changed, 137 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ClimberCommand.java diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java new file mode 100644 index 00000000..3b592d4a --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberCommand.java @@ -0,0 +1,51 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConstants; +import frc.robot.subsystems.climber.Climber; + +public class ClimberCommand extends Command { + private final Climber m_subsystem; + + private double setPoint; + + /** + * Creates a new ClimberCommand. + * + * @param subsystem The subsystem used by this command. + */ + public ClimberCommand(Climber subsystem, double setPoint) { + m_subsystem = subsystem; + this.setPoint = setPoint; + + addRequirements(subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + SmartDashboard.putNumber("Set Rotations", setPoint); + } + + // 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) { + m_subsystem.setMotorSpeed(0); + } + + // Returns true when the command should end. + // If absolute value of error is less than or equal to tolerance, returns true + @Override + public boolean isFinished() { + double error = SmartDashboard.getNumber("SetPoint", 0) - m_subsystem.getEncoderPosition(); + return Math.abs(error) <= RobotConstants.ClimberConstants.setPointTolerance; + } + +} diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 9609912c..1914e904 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -4,4 +4,11 @@ * Software/hardware constants (e.g. CAN IDs, gear ratios, field measurements, etc.). For software * configs @see RobotConfig */ -public final class RobotConstants {} +public final class RobotConstants { + public static final class ClimberConstants { + public static final int controllerID1 = -1; + public static final int controllerID2 = -1; + + public static final double setPointTolerance = 0.01; + } +} diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 42b842e0..77d6624e 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,18 +1,91 @@ package frc.robot.subsystems.climber; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.RobotConstants.ClimberConstants; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; public class Climber extends SubsystemBase { - /** Creates a new ExampleSubsystem. */ - public Climber() {} + + private CANSparkMax leaderController; + private CANSparkMax followerController; + private SparkPIDController m_pidController; + private RelativeEncoder m_encoder; + public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput; + + public Climber() { + leaderController = new CANSparkMax(ClimberConstants.controllerID1, MotorType.kBrushless); + followerController = new CANSparkMax(ClimberConstants.controllerID2, MotorType.kBrushless); + + followerController.follow(leaderController); + + m_pidController = leaderController.getPIDController(); + m_encoder = leaderController.getEncoder(); + + kP = 0.1; + kI = 1e-4; + kD = 1; + kIz = 0; + kFF = 0; + kMaxOutput = 1; + kMinOutput = -1; + + m_pidController.setP(kP); + m_pidController.setI(kI); + m_pidController.setD(kD); + m_pidController.setIZone(kIz); + m_pidController.setFF(kFF); + m_pidController.setOutputRange(kMinOutput, kMaxOutput); + + SmartDashboard.putNumber("P Gain", kP); + SmartDashboard.putNumber("I Gain", kI); + SmartDashboard.putNumber("D Gain", kD); + SmartDashboard.putNumber("I Zone", kIz); + SmartDashboard.putNumber("Feed Forward", kFF); + SmartDashboard.putNumber("Max Output", kMaxOutput); + SmartDashboard.putNumber("Min Output", kMinOutput); + SmartDashboard.putNumber("Set Rotations", 0); + + } @Override public void periodic() { // This method will be called once per scheduler run + double p = SmartDashboard.getNumber("P Gain", 0); + double i = SmartDashboard.getNumber("I Gain", 0); + double d = SmartDashboard.getNumber("D Gain", 0); + double iz = SmartDashboard.getNumber("I Zone", 0); + double ff = SmartDashboard.getNumber("Feed Forward", 0); + double max = SmartDashboard.getNumber("Max Output", 0); + double min = SmartDashboard.getNumber("Min Output", 0); + double rotations = SmartDashboard.getNumber("Set Rotations", 0); + + if((p != kP)) { m_pidController.setP(p); kP = p; } + if((i != kI)) { m_pidController.setI(i); kI = i; } + if((d != kD)) { m_pidController.setD(d); kD = d; } + if((iz != kIz)) { m_pidController.setIZone(iz); kIz = iz; } + if((ff != kFF)) { m_pidController.setFF(ff); kFF = ff; } + if((max != kMaxOutput) || (min != kMinOutput)) { + m_pidController.setOutputRange(min, max); + kMinOutput = min; kMaxOutput = max; + } + + m_pidController.setReference(rotations, CANSparkMax.ControlType.kPosition); + + SmartDashboard.putNumber("SetPoint", rotations); + SmartDashboard.putNumber("ProcessVariable", m_encoder.getPosition()); } - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation + public void setMotorSpeed(double speed) { + leaderController.set(speed); + } + + public double getEncoderPosition (){ + return m_encoder.getPosition(); } + } From cce6961492cdb92630bf4ae30a5cfa72f3decf72 Mon Sep 17 00:00:00 2001 From: Luca Barreith Date: Sat, 20 Jan 2024 15:45:10 -0800 Subject: [PATCH 02/51] Changed all SmartDashboard keys to constants --- .../frc/robot/commands/ClimberCommand.java | 2 +- .../frc/robot/constants/RobotConstants.java | 18 +++++++++++++++--- .../frc/robot/subsystems/climber/Climber.java | 14 +++++++------- 3 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java index 3b592d4a..845d1ffa 100644 --- a/src/main/java/frc/robot/commands/ClimberCommand.java +++ b/src/main/java/frc/robot/commands/ClimberCommand.java @@ -45,7 +45,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { double error = SmartDashboard.getNumber("SetPoint", 0) - m_subsystem.getEncoderPosition(); - return Math.abs(error) <= RobotConstants.ClimberConstants.setPointTolerance; + return Math.abs(error) <= RobotConstants.ClimberConstants.kSetPointTolerance; } } diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 1914e904..1c16d556 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -6,9 +6,21 @@ */ public final class RobotConstants { public static final class ClimberConstants { - public static final int controllerID1 = -1; - public static final int controllerID2 = -1; + public static final int kControllerID1 = -1; + public static final int kControllerID2 = -1; - public static final double setPointTolerance = 0.01; + public static final double kSetPointTolerance = 0.01; + + public static final String + kPGainKey = "Climber P Gain", + kIGainKey = "Climber I Gain", + kDGainKey = "Climber D Gain", + kIZoneKey = "Climber I Zone", + kFeedForwardKey = "Climber Feed Forward", + kMaxOutputKey = "Climber Max Output", + kMinOutputKey = "Climber Min Output", + kSetRotationKey = "Set Rotations", + kSetPointKey = "Setpoint", + kProcessVariableKey = "Process Variable"; } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 77d6624e..4e540544 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -18,8 +18,8 @@ public class Climber extends SubsystemBase { public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput; public Climber() { - leaderController = new CANSparkMax(ClimberConstants.controllerID1, MotorType.kBrushless); - followerController = new CANSparkMax(ClimberConstants.controllerID2, MotorType.kBrushless); + leaderController = new CANSparkMax(ClimberConstants.kControllerID1, MotorType.kBrushless); + followerController = new CANSparkMax(ClimberConstants.kControllerID2, MotorType.kBrushless); followerController.follow(leaderController); @@ -55,11 +55,11 @@ public Climber() { @Override public void periodic() { // This method will be called once per scheduler run - double p = SmartDashboard.getNumber("P Gain", 0); - double i = SmartDashboard.getNumber("I Gain", 0); - double d = SmartDashboard.getNumber("D Gain", 0); - double iz = SmartDashboard.getNumber("I Zone", 0); - double ff = SmartDashboard.getNumber("Feed Forward", 0); + double p = SmartDashboard.getNumber("Climber P Gain", 0); + double i = SmartDashboard.getNumber("Climber I Gain", 0); + double d = SmartDashboard.getNumber("Climber D Gain", 0); + double iz = SmartDashboard.getNumber("Climber I Zone", 0); + double ff = SmartDashboard.getNumber("Climber Feed Forward", 0); double max = SmartDashboard.getNumber("Max Output", 0); double min = SmartDashboard.getNumber("Min Output", 0); double rotations = SmartDashboard.getNumber("Set Rotations", 0); From 81a1b27f9ded09ecb4cdfd50481379823d75d7f0 Mon Sep 17 00:00:00 2001 From: WashEyesWithTide <145725240+WashEyesWithTide@users.noreply.github.com> Date: Tue, 23 Jan 2024 16:43:11 -0800 Subject: [PATCH 03/51] Added motor objects for climber Added motors and PID controller objects. --- .../frc/robot/commands/ClimberCommand.java | 5 +- .../frc/robot/constants/RobotConstants.java | 25 +++++---- .../frc/robot/subsystems/climber/Climber.java | 52 ++++++++++++------- 3 files changed, 47 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java index 845d1ffa..55396149 100644 --- a/src/main/java/frc/robot/commands/ClimberCommand.java +++ b/src/main/java/frc/robot/commands/ClimberCommand.java @@ -30,9 +30,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { - - } + public void execute() {} // Called once the command ends or is interrupted. @Override @@ -47,5 +45,4 @@ public boolean isFinished() { double error = SmartDashboard.getNumber("SetPoint", 0) - m_subsystem.getEncoderPosition(); return Math.abs(error) <= RobotConstants.ClimberConstants.kSetPointTolerance; } - } diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 1c16d556..dfdef7df 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -6,21 +6,20 @@ */ public final class RobotConstants { public static final class ClimberConstants { - public static final int kControllerID1 = -1; - public static final int kControllerID2 = -1; + public static final int CLIMBER_CONTROLLER_ID1 = -1; + public static final int CLIMBER_CONTROLLER_ID2 = -1; public static final double kSetPointTolerance = 0.01; - public static final String - kPGainKey = "Climber P Gain", - kIGainKey = "Climber I Gain", - kDGainKey = "Climber D Gain", - kIZoneKey = "Climber I Zone", - kFeedForwardKey = "Climber Feed Forward", - kMaxOutputKey = "Climber Max Output", - kMinOutputKey = "Climber Min Output", - kSetRotationKey = "Set Rotations", - kSetPointKey = "Setpoint", - kProcessVariableKey = "Process Variable"; + public static final String kPGainKey = "Climber P Gain", + kIGainKey = "Climber I Gain", + kDGainKey = "Climber D Gain", + kIZoneKey = "Climber I Zone", + kFeedForwardKey = "Climber Feed Forward", + kMaxOutputKey = "Climber Max Output", + kMinOutputKey = "Climber Min Output", + kSetRotationKey = "Set Rotations", + kSetPointKey = "Setpoint", + kProcessVariableKey = "Process Variable"; } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 4e540544..bc60be7a 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,13 +1,12 @@ package frc.robot.subsystems.climber; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.RobotConstants.ClimberConstants; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.RobotConstants.ClimberConstants; public class Climber extends SubsystemBase { @@ -18,8 +17,10 @@ public class Climber extends SubsystemBase { public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput; public Climber() { - leaderController = new CANSparkMax(ClimberConstants.kControllerID1, MotorType.kBrushless); - followerController = new CANSparkMax(ClimberConstants.kControllerID2, MotorType.kBrushless); + leaderController = + new CANSparkMax(ClimberConstants.CLIMBER_CONTROLLER_ID1, MotorType.kBrushless); + followerController = + new CANSparkMax(ClimberConstants.CLIMBER_CONTROLLER_ID2, MotorType.kBrushless); followerController.follow(leaderController); @@ -34,6 +35,7 @@ public Climber() { kMaxOutput = 1; kMinOutput = -1; + // set PID coefficients m_pidController.setP(kP); m_pidController.setI(kI); m_pidController.setD(kD); @@ -49,7 +51,6 @@ public Climber() { SmartDashboard.putNumber("Max Output", kMaxOutput); SmartDashboard.putNumber("Min Output", kMinOutput); SmartDashboard.putNumber("Set Rotations", 0); - } @Override @@ -64,14 +65,30 @@ public void periodic() { double min = SmartDashboard.getNumber("Min Output", 0); double rotations = SmartDashboard.getNumber("Set Rotations", 0); - if((p != kP)) { m_pidController.setP(p); kP = p; } - if((i != kI)) { m_pidController.setI(i); kI = i; } - if((d != kD)) { m_pidController.setD(d); kD = d; } - if((iz != kIz)) { m_pidController.setIZone(iz); kIz = iz; } - if((ff != kFF)) { m_pidController.setFF(ff); kFF = ff; } - if((max != kMaxOutput) || (min != kMinOutput)) { + if ((p != kP)) { + m_pidController.setP(p); + kP = p; + } + if ((i != kI)) { + m_pidController.setI(i); + kI = i; + } + if ((d != kD)) { + m_pidController.setD(d); + kD = d; + } + if ((iz != kIz)) { + m_pidController.setIZone(iz); + kIz = iz; + } + if ((ff != kFF)) { + m_pidController.setFF(ff); + kFF = ff; + } + if ((max != kMaxOutput) || (min != kMinOutput)) { m_pidController.setOutputRange(min, max); - kMinOutput = min; kMaxOutput = max; + kMinOutput = min; + kMaxOutput = max; } m_pidController.setReference(rotations, CANSparkMax.ControlType.kPosition); @@ -84,8 +101,7 @@ public void setMotorSpeed(double speed) { leaderController.set(speed); } - public double getEncoderPosition (){ + public double getEncoderPosition() { return m_encoder.getPosition(); } - } From b621640a542fc76fa022e6169d8afaae2d66ab71 Mon Sep 17 00:00:00 2001 From: continuumsrc <23npthompson@gmail.com> Date: Wed, 14 Feb 2024 16:28:35 -0800 Subject: [PATCH 04/51] Setup basic climber code. --- .../frc/robot/constants/RobotConstants.java | 18 ++-- .../frc/robot/subsystems/climber/Climber.java | 86 +++++-------------- 2 files changed, 31 insertions(+), 73 deletions(-) diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index dfdef7df..496f455d 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -11,15 +11,13 @@ public static final class ClimberConstants { public static final double kSetPointTolerance = 0.01; - public static final String kPGainKey = "Climber P Gain", - kIGainKey = "Climber I Gain", - kDGainKey = "Climber D Gain", - kIZoneKey = "Climber I Zone", - kFeedForwardKey = "Climber Feed Forward", - kMaxOutputKey = "Climber Max Output", - kMinOutputKey = "Climber Min Output", - kSetRotationKey = "Set Rotations", - kSetPointKey = "Setpoint", - kProcessVariableKey = "Process Variable"; + public static final double kClimberP = 0; + public static final double kClimberI = 0; + public static final double kClimberD = 0; + public static final double kClimberMotorRadius = 0; + public static final double kClimberIZone = 0; + public static final double kClimberFeedForward = 0; + public static final double kClimberMaxOutput = 0; + public static final double kClimberMinOutput = 0; } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index bc60be7a..c33e08bf 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -4,6 +4,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.RobotConstants.ClimberConstants; @@ -14,7 +15,6 @@ public class Climber extends SubsystemBase { private CANSparkMax followerController; private SparkPIDController m_pidController; private RelativeEncoder m_encoder; - public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput; public Climber() { leaderController = @@ -27,81 +27,41 @@ public Climber() { m_pidController = leaderController.getPIDController(); m_encoder = leaderController.getEncoder(); - kP = 0.1; - kI = 1e-4; - kD = 1; - kIz = 0; - kFF = 0; - kMaxOutput = 1; - kMinOutput = -1; - // set PID coefficients - m_pidController.setP(kP); - m_pidController.setI(kI); - m_pidController.setD(kD); - m_pidController.setIZone(kIz); - m_pidController.setFF(kFF); - m_pidController.setOutputRange(kMinOutput, kMaxOutput); + m_pidController.setP(ClimberConstants.kClimberP); + m_pidController.setI(ClimberConstants.kClimberI); + m_pidController.setD(ClimberConstants.kClimberP); + m_pidController.setIZone(ClimberConstants.kClimberIZone); + m_pidController.setFF(ClimberConstants.kClimberFeedForward); + m_pidController.setOutputRange(ClimberConstants.kClimberMinOutput, ClimberConstants.kClimberMaxOutput); - SmartDashboard.putNumber("P Gain", kP); - SmartDashboard.putNumber("I Gain", kI); - SmartDashboard.putNumber("D Gain", kD); - SmartDashboard.putNumber("I Zone", kIz); - SmartDashboard.putNumber("Feed Forward", kFF); - SmartDashboard.putNumber("Max Output", kMaxOutput); - SmartDashboard.putNumber("Min Output", kMinOutput); - SmartDashboard.putNumber("Set Rotations", 0); + SmartDashboard.putNumber("Setpoint", 0); } @Override public void periodic() { // This method will be called once per scheduler run - double p = SmartDashboard.getNumber("Climber P Gain", 0); - double i = SmartDashboard.getNumber("Climber I Gain", 0); - double d = SmartDashboard.getNumber("Climber D Gain", 0); - double iz = SmartDashboard.getNumber("Climber I Zone", 0); - double ff = SmartDashboard.getNumber("Climber Feed Forward", 0); - double max = SmartDashboard.getNumber("Max Output", 0); - double min = SmartDashboard.getNumber("Min Output", 0); - double rotations = SmartDashboard.getNumber("Set Rotations", 0); + setSetpoint(); + } - if ((p != kP)) { - m_pidController.setP(p); - kP = p; - } - if ((i != kI)) { - m_pidController.setI(i); - kI = i; - } - if ((d != kD)) { - m_pidController.setD(d); - kD = d; - } - if ((iz != kIz)) { - m_pidController.setIZone(iz); - kIz = iz; - } - if ((ff != kFF)) { - m_pidController.setFF(ff); - kFF = ff; - } - if ((max != kMaxOutput) || (min != kMinOutput)) { - m_pidController.setOutputRange(min, max); - kMinOutput = min; - kMaxOutput = max; - } + public double getEncoderPosition() { + return m_encoder.getPosition(); + } - m_pidController.setReference(rotations, CANSparkMax.ControlType.kPosition); + public void setSetpoint() { + double setpoint = SmartDashboard.getNumber("Setpoint", 0); + m_pidController.setReference(metersToRotations(setpoint), CANSparkMax.ControlType.kPosition); + } - SmartDashboard.putNumber("SetPoint", rotations); - SmartDashboard.putNumber("ProcessVariable", m_encoder.getPosition()); + public double rotationsToMeters(double rotations) { + return 2*Math.PI*ClimberConstants.kClimberMotorRadius*rotations; } - public void setMotorSpeed(double speed) { - leaderController.set(speed); + public double metersToRotations(double meters) { + return meters/(2*Math.PI*ClimberConstants.kClimberMotorRadius); } - public double getEncoderPosition() { - return m_encoder.getPosition(); + public void setMotorSpeed(double speed) { + throw new UnsupportedOperationException("Unimplemented method 'setMotorSpeed'"); } } From 846dc10a234f938079ed6cc3bbd0732b545de85a Mon Sep 17 00:00:00 2001 From: continuumsrc <23npthompson@gmail.com> Date: Thu, 15 Feb 2024 11:34:16 -0800 Subject: [PATCH 05/51] Fixed climber commands. --- src/main/java/frc/robot/commands/ClimberCommand.java | 4 ++-- src/main/java/frc/robot/subsystems/climber/Climber.java | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java index 55396149..6fd606dd 100644 --- a/src/main/java/frc/robot/commands/ClimberCommand.java +++ b/src/main/java/frc/robot/commands/ClimberCommand.java @@ -25,7 +25,7 @@ public ClimberCommand(Climber subsystem, double setPoint) { // Called when the command is initially scheduled. @Override public void initialize() { - SmartDashboard.putNumber("Set Rotations", setPoint); + SmartDashboard.putNumber("Setpoint", setPoint); } // Called every time the scheduler runs while the command is scheduled. @@ -42,7 +42,7 @@ public void end(boolean interrupted) { // If absolute value of error is less than or equal to tolerance, returns true @Override public boolean isFinished() { - double error = SmartDashboard.getNumber("SetPoint", 0) - m_subsystem.getEncoderPosition(); + double error = Climber.metersToRotations(SmartDashboard.getNumber("Setpoint", 0)) - m_subsystem.getEncoderPosition(); return Math.abs(error) <= RobotConstants.ClimberConstants.kSetPointTolerance; } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index c33e08bf..33de3ae9 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -53,15 +53,15 @@ public void setSetpoint() { m_pidController.setReference(metersToRotations(setpoint), CANSparkMax.ControlType.kPosition); } - public double rotationsToMeters(double rotations) { + public static double rotationsToMeters(double rotations) { return 2*Math.PI*ClimberConstants.kClimberMotorRadius*rotations; } - public double metersToRotations(double meters) { + public static double metersToRotations(double meters) { return meters/(2*Math.PI*ClimberConstants.kClimberMotorRadius); } public void setMotorSpeed(double speed) { - throw new UnsupportedOperationException("Unimplemented method 'setMotorSpeed'"); + leaderController.set(speed); } } From 238dfd4e9a69af2027f5c522409ba7fbdd25f2b3 Mon Sep 17 00:00:00 2001 From: TurtleMeds Date: Fri, 16 Feb 2024 17:34:58 -0800 Subject: [PATCH 06/51] stopped using ShuffleBoard for climber setpoint and added motor radius to Constants --- .../frc/robot/commands/ClimberCommand.java | 19 ++++++------------- .../java/frc/robot/constants/RobotConfig.java | 6 +++++- .../frc/robot/constants/RobotConstants.java | 2 +- .../frc/robot/subsystems/climber/Climber.java | 11 ++--------- 4 files changed, 14 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java index 6fd606dd..92e45897 100644 --- a/src/main/java/frc/robot/commands/ClimberCommand.java +++ b/src/main/java/frc/robot/commands/ClimberCommand.java @@ -1,36 +1,29 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConstants; +import frc.robot.constants.RobotConfig.ClimberConfig; import frc.robot.subsystems.climber.Climber; public class ClimberCommand extends Command { private final Climber m_subsystem; - private double setPoint; - /** * Creates a new ClimberCommand. * * @param subsystem The subsystem used by this command. */ - public ClimberCommand(Climber subsystem, double setPoint) { + public ClimberCommand(Climber subsystem) { m_subsystem = subsystem; - this.setPoint = setPoint; addRequirements(subsystem); } - // Called when the command is initially scheduled. - @Override - public void initialize() { - SmartDashboard.putNumber("Setpoint", setPoint); - } - // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + m_subsystem.setSetpoint(ClimberConfig.setpoint); + } // Called once the command ends or is interrupted. @Override @@ -42,7 +35,7 @@ public void end(boolean interrupted) { // If absolute value of error is less than or equal to tolerance, returns true @Override public boolean isFinished() { - double error = Climber.metersToRotations(SmartDashboard.getNumber("Setpoint", 0)) - m_subsystem.getEncoderPosition(); + double error = Climber.metersToRotations(ClimberConfig.setpoint) - m_subsystem.getEncoderPosition(); return Math.abs(error) <= RobotConstants.ClimberConstants.kSetPointTolerance; } } diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index b2230504..91ef4efc 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -4,4 +4,8 @@ * Software config settings (e.g. max speed, PID values). For hardware constants @see * RobotConstants" */ -public class RobotConfig {} +public class RobotConfig { + public static final class ClimberConfig { + public static double setpoint = 0; + } +} diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 496f455d..3c1a0d42 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -14,7 +14,7 @@ public static final class ClimberConstants { public static final double kClimberP = 0; public static final double kClimberI = 0; public static final double kClimberD = 0; - public static final double kClimberMotorRadius = 0; + public static final double kClimberMotorRadius = 0.003175; public static final double kClimberIZone = 0; public static final double kClimberFeedForward = 0; public static final double kClimberMaxOutput = 0; diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 33de3ae9..71fbd50a 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -5,7 +5,6 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.RobotConstants.ClimberConstants; @@ -34,22 +33,16 @@ public Climber() { m_pidController.setIZone(ClimberConstants.kClimberIZone); m_pidController.setFF(ClimberConstants.kClimberFeedForward); m_pidController.setOutputRange(ClimberConstants.kClimberMinOutput, ClimberConstants.kClimberMaxOutput); - - SmartDashboard.putNumber("Setpoint", 0); } @Override - public void periodic() { - // This method will be called once per scheduler run - setSetpoint(); - } + public void periodic() {} public double getEncoderPosition() { return m_encoder.getPosition(); } - public void setSetpoint() { - double setpoint = SmartDashboard.getNumber("Setpoint", 0); + public void setSetpoint(double setpoint) { m_pidController.setReference(metersToRotations(setpoint), CANSparkMax.ControlType.kPosition); } From 53c7871a9f9359d4c9ee2cdbb036da3952d2238f Mon Sep 17 00:00:00 2001 From: TurtleMeds Date: Sat, 17 Feb 2024 12:47:13 -0800 Subject: [PATCH 07/51] Applied Spotless --- src/main/java/frc/robot/commands/ClimberCommand.java | 5 +++-- src/main/java/frc/robot/constants/RobotConfig.java | 6 +++--- src/main/java/frc/robot/subsystems/climber/Climber.java | 8 ++++---- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java index 92e45897..fcbee844 100644 --- a/src/main/java/frc/robot/commands/ClimberCommand.java +++ b/src/main/java/frc/robot/commands/ClimberCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.RobotConstants; import frc.robot.constants.RobotConfig.ClimberConfig; +import frc.robot.constants.RobotConstants; import frc.robot.subsystems.climber.Climber; public class ClimberCommand extends Command { @@ -35,7 +35,8 @@ public void end(boolean interrupted) { // If absolute value of error is less than or equal to tolerance, returns true @Override public boolean isFinished() { - double error = Climber.metersToRotations(ClimberConfig.setpoint) - m_subsystem.getEncoderPosition(); + double error = + Climber.metersToRotations(ClimberConfig.setpoint) - m_subsystem.getEncoderPosition(); return Math.abs(error) <= RobotConstants.ClimberConstants.kSetPointTolerance; } } diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 91ef4efc..955f0990 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -5,7 +5,7 @@ * RobotConstants" */ public class RobotConfig { - public static final class ClimberConfig { - public static double setpoint = 0; - } + public static final class ClimberConfig { + public static double setpoint = 0; + } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 71fbd50a..1e60afa0 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -4,7 +4,6 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; - import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.RobotConstants.ClimberConstants; @@ -32,7 +31,8 @@ public Climber() { m_pidController.setD(ClimberConstants.kClimberP); m_pidController.setIZone(ClimberConstants.kClimberIZone); m_pidController.setFF(ClimberConstants.kClimberFeedForward); - m_pidController.setOutputRange(ClimberConstants.kClimberMinOutput, ClimberConstants.kClimberMaxOutput); + m_pidController.setOutputRange( + ClimberConstants.kClimberMinOutput, ClimberConstants.kClimberMaxOutput); } @Override @@ -47,11 +47,11 @@ public void setSetpoint(double setpoint) { } public static double rotationsToMeters(double rotations) { - return 2*Math.PI*ClimberConstants.kClimberMotorRadius*rotations; + return 2 * Math.PI * ClimberConstants.kClimberMotorRadius * rotations; } public static double metersToRotations(double meters) { - return meters/(2*Math.PI*ClimberConstants.kClimberMotorRadius); + return meters / (2 * Math.PI * ClimberConstants.kClimberMotorRadius); } public void setMotorSpeed(double speed) { From bed764566042ec3e35fe0ad58e11c95708149ae5 Mon Sep 17 00:00:00 2001 From: iris Date: Thu, 29 Feb 2024 18:09:58 -0800 Subject: [PATCH 08/51] shooter fixes --- src/main/java/frc/robot/RobotContainer.java | 154 ++++++---- .../frc/robot/commands/intake/RunIntake.java | 46 +++ .../robot/commands/shooter/ActuateShield.java | 6 +- .../java/frc/robot/commands/shooter/Aim.java | 5 +- .../frc/robot/commands/shooter/Shoot.java | 27 +- .../java/frc/robot/constants/RobotConfig.java | 83 ++++-- .../frc/robot/constants/RobotConstants.java | 75 +++-- .../robot/subsystems/drive/Drivetrain.java | 264 ++++++++++++------ .../frc/robot/subsystems/intake/Intake.java | 52 +++- .../frc/robot/subsystems/shooter/Shooter.java | 176 +++++------- src/main/java/frc/utils/Vector.java | 12 +- 11 files changed, 574 insertions(+), 326 deletions(-) create mode 100644 src/main/java/frc/robot/commands/intake/RunIntake.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4141afe0..75e7e5bd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,107 +1,157 @@ -// 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 frc.robot; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.BasicDriveCommand; -import frc.robot.commands.VisionTurnCommand; -import frc.robot.commands.shooter.*; -import frc.robot.constants.RobotConfig.*; -import frc.robot.constants.RobotConstants.*; +import frc.robot.commands.intake.RunIntake; +import frc.robot.commands.shooter.ActuateShield; +import frc.robot.commands.shooter.Aim; +import frc.robot.commands.shooter.Shoot; +import frc.robot.constants.RobotConfig; +import frc.robot.constants.RobotConfig.FieldElement; +import frc.robot.constants.RobotConstants.Bindings; import frc.robot.constants.RobotConstants.DriveConstants.OIConstants; import frc.robot.subsystems.drive.Drivetrain; +import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.vision.Vision; import frc.utils.Vector; public class RobotContainer { private Joystick m_operatorController; - private XboxController m_driverController; private POVButton m_autoAim; private POVButton m_speakerAim; private Vision m_vision; - private Drivetrain m_robotDrive; private Shooter m_shooter; + private Intake m_intake; + private Drivetrain m_robotDrive; + + // The driver's controller + private XboxController m_driverController; + private SendableChooser autoChooser; + + private Vector leftInputVec; + private Vector rightInputVec; public RobotContainer() { - m_driverController = new XboxController(OIConstants.kDriverControllerPort); - m_operatorController = new Joystick(OIConstants.kOperatorControllerPort); m_shooter = new Shooter(); m_vision = new Vision(); - m_robotDrive = new Drivetrain(m_vision); + m_intake = new Intake(); + m_robotDrive = new Drivetrain(); + + m_driverController = new XboxController(OIConstants.kDriverControllerPort); + m_operatorController = new Joystick(OIConstants.kOperatorJoystickPort); + + autoChooser = AutoBuilder.buildAutoChooser(); + leftInputVec = new Vector(); + rightInputVec = new Vector(); + configureBindings(); + registerCommands(); - m_shooter.setDefaultCommand( - new RunCommand(() -> m_shooter.runFlywheel(ShooterConfig.kMaxFlywheelRPM), m_shooter)); + /*m_shooter.setDefaultCommand( + new RunCommand(() -> m_shooter.runFlywheel(ShooterConfig.kDefaultFlywheelRPM), m_shooter));*/ } private void configureBindings() { // angle on 8-directional button m_autoAim = new POVButton(m_operatorController, 0); m_speakerAim = new POVButton(m_operatorController, 90); - m_robotDrive.setDefaultCommand( // The left stick controls translation of the robot. // Turning is controlled by the X axis of the right stick. new RunCommand( - () -> - m_robotDrive.drive( - new Vector( - MathUtil.applyDeadband( - -m_driverController.getLeftY(), OIConstants.kDriveDeadband), - MathUtil.applyDeadband( - -m_driverController.getLeftX(), OIConstants.kDriveDeadband)), - new Vector( - MathUtil.applyDeadband( - -m_driverController.getRightX(), OIConstants.kDriveDeadband), - MathUtil.applyDeadband( - -m_driverController.getRightY(), OIConstants.kDriveDeadband)), - m_driverController.getRightBumper(), - m_driverController.getAButton()), + () -> { + // update the values of leftInputVec and rightInputVec to the values of the controller + // I'm avoiding re-instantiting Vectors to save memory + updateInput(); + m_robotDrive.drive( + leftInputVec, + rightInputVec, + m_driverController.getRightBumper(), + m_driverController.getAButton()); + }, m_robotDrive)); + new Trigger(() -> triggerPressed()) + .whileTrue(new BasicDriveCommand(m_robotDrive, m_driverController)); + + // RunIntake constructor boolean is whether or not the intake should run reversed. + new Trigger(this::getIntakeButton).whileTrue(new RunIntake(m_intake, true)); + new Trigger(this::getReverseIntakeButton).whileTrue(new RunIntake(m_intake, false)); // just shoot on trigger new Trigger(() -> m_operatorController.getRawButton(Bindings.kShoot)) - .onTrue(new Shoot(m_shooter)); - // aim amp + .whileTrue(new Shoot(m_intake, false)); + new Trigger(() -> m_operatorController.getRawButton(Bindings.kShootReverse)) + .whileTrue(new Shoot(m_intake, true)); new Trigger(() -> m_operatorController.getRawButton(Bindings.kAimAmp)) - .onTrue(new Aim(m_shooter, FieldElement.AMP)); + .whileTrue(new Aim(m_shooter, FieldElement.AMP)); - m_speakerAim.onTrue(new Aim(m_shooter, FieldElement.SPEAKER)); + m_speakerAim.whileTrue(new Aim(m_shooter, FieldElement.SPEAKER)); m_autoAim.whileTrue(new Aim(m_shooter, m_vision)); - // stow shooter - new Trigger(() -> m_operatorController.getRawButton(Bindings.kStowShooter)) - .onTrue(new StowShooter(m_shooter)); - - // triggers for manual adjust up and down, both assigned to different buttons - new Trigger(() -> m_operatorController.getRawButton(Bindings.kManualAdjustDown)) - .onTrue(new ManualAdjust(m_shooter, AdjustType.down)); - new Trigger(() -> m_operatorController.getRawButton(Bindings.kManualAdjustUp)) - .onTrue(new ManualAdjust(m_shooter, AdjustType.up)); - // triggers for extending and retracting shield manually - new Trigger(() -> m_operatorController.getRawButton(Bindings.kRetractShield)) - .onTrue(new ActuateShield(m_shooter, false)); + // don't extend shield new Trigger(() -> m_operatorController.getRawButton(Bindings.kExtendShield)) + .onTrue(new ActuateShield(m_shooter, false)); + // extend shield + new Trigger(() -> m_operatorController.getRawButton(Bindings.kRetractShield)) .onTrue(new ActuateShield(m_shooter, true)); - new Trigger(() -> triggerPressed()) - .whileTrue(new BasicDriveCommand(m_robotDrive, m_driverController)); + SmartDashboard.putData("Auto Chooser", autoChooser); + } + + private void updateInput() { + leftInputVec.setX( + MathUtil.applyDeadband(-m_driverController.getLeftY(), OIConstants.kDriveDeadband)); + leftInputVec.setY( + MathUtil.applyDeadband(-m_driverController.getLeftX(), OIConstants.kDriveDeadband)); + rightInputVec.setX( + MathUtil.applyDeadband(-m_driverController.getRightX(), OIConstants.kDriveDeadband)); + rightInputVec.setY( + MathUtil.applyDeadband(-m_driverController.getRightY(), OIConstants.kDriveDeadband)); + } + + // TODO: fill in placeholder commands with actual functionality + private void registerCommands() { + NamedCommands.registerCommand("intakeFromFloor", doNothing()); + NamedCommands.registerCommand("scoreAmp", doNothing()); + NamedCommands.registerCommand("aimAndScoreSpeaker", doNothing()); + } + + private Command doNothing() { + return Commands.none(); + } + + /** + * Returns true if the intake is pressed; False otherwise. + * + * @see RobotConfig.IntakeConfig.Bindings.kIntakeNote + */ + public boolean getIntakeButton() { + return m_operatorController.getRawButton(RobotConfig.IntakeConfig.Bindings.kIntakeNoteButtonID); + } - new Trigger(() -> m_driverController.getBButton()) - .onTrue(new VisionTurnCommand(m_vision, m_robotDrive, m_driverController)); + /** + * Returns true if the reverse intake button is pressed; False otherwise. + * + * @see RobotConfig.IntakeConfig.Bindings.kReverseIntakeButtonID + */ + public boolean getReverseIntakeButton() { + return m_operatorController.getRawButton( + RobotConfig.IntakeConfig.Bindings.kReverseIntakeButtonID); } public boolean triggerPressed() { @@ -113,6 +163,6 @@ public boolean triggerPressed() { } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return autoChooser.getSelected(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/intake/RunIntake.java b/src/main/java/frc/robot/commands/intake/RunIntake.java new file mode 100644 index 00000000..e61a3cdb --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/RunIntake.java @@ -0,0 +1,46 @@ +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConfig; +import frc.robot.subsystems.intake.Intake; + +public class RunIntake extends Command { + private final Intake m_intake; + private boolean m_reversed; + + /** + * Creates a new RunIntake command, which runs the roller motor on the intake subsystem to intake + * a note + * + * @param intake The subsystem used by this command. + */ + public RunIntake(Intake intake, boolean reversed) { + m_intake = intake; + m_reversed = reversed; + + addRequirements(intake); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + int multiplier = m_reversed ? -1 : 1; + m_intake.run(RobotConfig.IntakeConfig.kDefaultSpeed * multiplier); + } + + // 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) { + m_intake.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/shooter/ActuateShield.java b/src/main/java/frc/robot/commands/shooter/ActuateShield.java index d4496d70..28cc0cb6 100644 --- a/src/main/java/frc/robot/commands/shooter/ActuateShield.java +++ b/src/main/java/frc/robot/commands/shooter/ActuateShield.java @@ -32,10 +32,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { // if we want the shield to be out, return true if that is the status - if (m_shieldState) { - return m_shooter.getShieldStatus(); - } else { - return !m_shooter.getShieldStatus(); - } + return m_shooter.getShieldStatus(m_shieldState); } } diff --git a/src/main/java/frc/robot/commands/shooter/Aim.java b/src/main/java/frc/robot/commands/shooter/Aim.java index 61365203..d1cd0bca 100644 --- a/src/main/java/frc/robot/commands/shooter/Aim.java +++ b/src/main/java/frc/robot/commands/shooter/Aim.java @@ -1,7 +1,6 @@ package frc.robot.commands.shooter; import edu.wpi.first.units.Angle; -import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; import edu.wpi.first.units.Velocity; @@ -39,7 +38,7 @@ public void initialize() { if (m_vision.getHasTarget()) { double desiredAngle = Units.Degrees.of(m_vision.getBestTarget().getPitch()).in(Units.Radians); - Measure> desiredVelocity = + Measure> desiredVelocity = m_shooter.calculateVelocity( m_vision.getDistToTarget() * Math.atan(desiredAngle), Units.Radians.of(desiredAngle)); @@ -78,6 +77,6 @@ public double getVelocity(double elementHeight) { public boolean isFinished() { return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()) && ((m_type == FieldElement.AMP || m_type == FieldElement.TRAP) - && m_shooter.getShieldStatus()); // check if shield is extended + && m_shooter.getShieldStatus(true)); // check if shield is extended } } diff --git a/src/main/java/frc/robot/commands/shooter/Shoot.java b/src/main/java/frc/robot/commands/shooter/Shoot.java index 4da90f4f..31c763ff 100644 --- a/src/main/java/frc/robot/commands/shooter/Shoot.java +++ b/src/main/java/frc/robot/commands/shooter/Shoot.java @@ -1,33 +1,26 @@ package frc.robot.commands.shooter; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.RobotConfig; -import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.intake.Intake; public class Shoot extends Command { - private final Shooter m_shooter; - private double timer; + private final Intake m_intake; + private boolean m_reverse; - public Shoot(Shooter shooter) { - m_shooter = shooter; + public Shoot(Intake intake, boolean reverse) { + m_reverse = reverse; + m_intake = intake; - addRequirements(m_shooter); + addRequirements(m_intake); } @Override public void initialize() { - timer = Timer.getFPGATimestamp(); - m_shooter.startFeedNote(); + m_intake.startFeedNote(m_reverse); } @Override public void end(boolean interrupted) { - m_shooter.stopFeedNote(); + m_intake.stopFeedNote(); } - - @Override - public boolean isFinished() { - return Timer.getFPGATimestamp() - timer > RobotConfig.ShooterConfig.kReleaseTime; - } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 70ce9044..371da1e9 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -1,9 +1,14 @@ package frc.robot.constants; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.units.Angle; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; import edu.wpi.first.units.Velocity; +import frc.robot.constants.RobotConstants.DriveConstants; +import frc.robot.constants.RobotConstants.DriveConstants.SwerveModuleConstants; /** * Software config settings (e.g. max speed, PID values). For hardware constants @see @@ -32,13 +37,13 @@ public static final class ShooterConfig { public static final double kAngleControlMaxOutput = 0; // top Flywheel controller PID coefficients - public static final double kTopFlywheelP = 0; + public static final double kTopFlywheelP = 0.2; public static final double kTopFlywheelI = 0; - public static final double kTopFlywheelD = 0; + public static final double kTopFlywheelD = 0.001; public static final double kTopFlywheelFF = 0; - public static final double kTopFlywheelIZone = 0; - public static final double kTopFlywheelMinOutput = 0; - public static final double kTopFlywheelMaxOutput = 0; + public static final double kTopFlywheelIZone = 0.0001; + public static final double kTopFlywheelMinOutput = -1; + public static final double kTopFlywheelMaxOutput = 1; // Smart dashboard Angle Controller keys public static final String kAngleControlPGainKey = "Angle Controller P Gain"; @@ -59,37 +64,44 @@ public static final class ShooterConfig { public static final String kTopFlywheelMaxOutputKey = "Top Flywheel Maximum output"; // Roller Default Speed - public static final double kRollerDefaultSpeed = 0; + public static final double kRollerDefaultSpeed = -0.4; // Flywheel default speed - public static final double kFlywheelDefaultRPM = 0; - // Shield Extended position - public static final double kShieldExtendedRotations = 124.140855612; + public static final double kFlywheelDefaultRPM = 1000; // Shield Retracted position - public static final double kShieldRetractedRotations = 0; + public static final double kShieldRetractedPosition = 15; // Timeout time (in seconds) public static final double kRunIntakeTimeoutTime = 0; - public static final double kShieldExtendedPosition = 10; // TODO get correct value + public static final double kShieldExtendedPosition = 85; // TODO get correct value // Speaker height public static final double SpeakerHeight = 1.9812; public static final double AmpHeight = .46; public static final double ShooterHeight = 0.28575; - public static final double TrapHeight = -1; + public static final double TrapHeight = 1; public static final double SpeakerBillLength = 0.6604; public static final int shootButton = 1; - public static final double kMaxFlywheelRPM = 1000; + public static final double kDefaultFlywheelRPM = 1000; public static final double kShooterStowAngle = 0; - public static final long kReleaseTime = 500; + public static final long kReleaseTime = 5000; + public static final long kShieldTime = 2; // seconds + public static final double kShieldDefaultSpeed = 0.3; public static final Measure> kFlywheelError = Units.RPM.of(1); - public static final Measure kAngleError = Units.Radians.of(0.5*Math.PI/180); - public static final Measure kSpeakerAngle = Units.Radians.of(75*Math.PI/180); - public static final Measure kAmpAngle = Units.Radians.of(109*Math.PI/180); - public static final Measure kTrapAngle = Units.Radians.of(105*Math.PI/180); - public static final Measure kAdjustAmountDegrees = Units.Radians.of(0.5*Math.PI/180); + public static final Measure kAngleError = Units.Radians.of(0.5 * Math.PI / 180); + public static final Measure kSpeakerAngle = Units.Radians.of(75 * Math.PI / 180); + public static final Measure kAmpAngle = Units.Radians.of(109 * Math.PI / 180); + public static final Measure kTrapAngle = Units.Radians.of(105 * Math.PI / 180); + public static final Measure kAdjustAmountDegrees = Units.Radians.of(0.5 * Math.PI / 180); + + public static final double kShieldExtendedRotations = 124.140855612; + public static final double kShieldRetractedRotations = 5; + + // TODO placeholders + public static final double ampVelocity = 2500; // rpm + public static final double trapVelocity = 2000; // rpm } public static class DriveConfig { @@ -117,6 +129,23 @@ public static class TurnConfig { public static final double maxIntegral = 8; } + public static final String kSlewRateTranslationMagOutput = "translation magnitude output"; + public static final String kSlewRateTranslationDirRadOutput = "translation dir rad"; + + public static final HolonomicPathFollowerConfig kPathFollowerConfig = + new HolonomicPathFollowerConfig( + new PIDConstants( + SwerveModuleConstants.kDrivingP, + SwerveModuleConstants.kDrivingI, + SwerveModuleConstants.kDrivingD), + new PIDConstants( + SwerveModuleConstants.kTurningP, + SwerveModuleConstants.kTurningI, + SwerveModuleConstants.kTurningD), + SwerveModuleConstants.kMaxModuleSpeed.in(Units.MetersPerSecond), + DriveConstants.kWheelBaseRadius.in(Units.Meters), + new ReplanningConfig()); + // 4.45 m/s max speed public static final double kMaxSpeedBase = 4.8; public static final double kMaxSpeedScaleFactor = 0.9; @@ -134,7 +163,17 @@ public static class TurnConfig { // scaling factor for the alternative turning mode public static final int altTurnSmoothing = 20; public static final double HIGH_DIRECTION_SLEW_RATE = 500; - public static final double MIN_ANGLE_SLEW_RATE = 0.45; - public static final double MAX_ANGLE_SLEW_RATE = 0.85; + public static final double MIN_ANGLE_SLEW_RATE = 0.45 * Math.PI; + public static final double MAX_ANGLE_SLEW_RATE = 0.85 * Math.PI; + } + + public static final class IntakeConfig { + // In percentage output + public static final double kDefaultSpeed = 1; + + public static final class Bindings { + public static final int kIntakeNoteButtonID = 2; + public static final int kReverseIntakeButtonID = 8; + } } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index b9255bf8..40d038d6 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -6,7 +6,11 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.units.*; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.Velocity; /** * Software/hardware constants (e.g. CAN IDs, gear ratios, field measurements, etc.). For software @@ -15,14 +19,15 @@ public final class RobotConstants { public final class Bindings { - public static final int kAimAmp = 3; + public static final int kAimAmp = 4; public static final int kShoot = 1; + public static final int kShootReverse = 13; public static final int kAimTrap = 2; - public static final int kStowShooter = 15; + public static final int kStowShooter = 14; public static final int kToggleFlywheel = 5; public static final int kManualAngleSlider = 3; public static final int kRetractShield = 16; - public static final int kExtendShield = 17; + public static final int kExtendShield = 15; public static final int kManualAdjustDown = 18; public static final int kManualAdjustUp = 19; } @@ -46,13 +51,12 @@ public static final class NeoMotorConstants { } public final class ShooterConstants { - public static final int kRollerMotorLeftId = -1; - public static final int kRollerMotorRightId = -1; + public static final int kRollerMotorLeftId = 1; + public static final int kTopFlywheelMotorId = 2; + public static final int kBottomFlywheelMotorId = 3; + public static final int kShieldMotorId = 4; public static final int kAngleMotorLeaderId = -1; public static final int kAngleMotorFollowerId = -1; - public static final int kTopFlywheelMotorId = -1; - public static final int kBottomFlywheelMotorId = -1; - public static final int kShieldMotorId = -1; public static final double FlywheelDiameter = 0.0762; public static final double ShooterLength = 0.4064; public static final double Gravity = 9.81; @@ -61,46 +65,48 @@ public final class ShooterConstants { } public static final class DriveConstants { - // placeholder CAN IDs, fix these later - - public static final double kFrontLeftChassisAngularOffset = 0.0; - public static final double kFrontRightChassisAngularOffset = 0.0; - public static final double kBackLeftChassisAngularOffset = 0.0; - public static final double kBackRightChassisAngularOffset = 0.0; + public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2; + public static final double kFrontRightChassisAngularOffset = 0; + public static final double kBackLeftChassisAngularOffset = Math.PI; + public static final double kBackRightChassisAngularOffset = Math.PI / 2; public static final double kDriveDeadband = 0.06; - public static final int kFrontLeftDrivingCanId = 6; - public static final int kFrontLeftTurningCanId = 5; + public static final int kFrontLeftDrivingCanId = 2; + public static final int kFrontLeftTurningCanId = 1; - public static final int kFrontRightDrivingCanId = 8; - public static final int kFrontRightTurningCanId = 7; + public static final int kFrontRightDrivingCanId = 6; + public static final int kFrontRightTurningCanId = 5; public static final int kRearLeftDrivingCanId = 4; public static final int kRearLeftTurningCanId = 3; - public static final int kRearRightDrivingCanId = 2; - public static final int kRearRightTurningCanId = 1; + public static final int kRearRightDrivingCanId = 8; + public static final int kRearRightTurningCanId = 7; - public static final int kGyroId = 15; + public static final int kGyroId = 9; // Chassis configuration - public static final double kTrackWidth = 26.5 * 2.54e-2; + public static final Measure kTrackWidth = Units.Inches.of(22.5); // Distance between centers of right and left wheels on robot - public static final double kWheelBase = 26.5 * 2.54e-2; + public static final Measure kWheelBase = Units.Inches.of(22.5); + + public static final Measure kWheelBaseRadius = Units.Meters.of(0.404); + // Distance between front and back wheels on robot public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2, kTrackWidth / 2), - new Translation2d(kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + new Translation2d(kWheelBase.in(Units.Meters) / 2, kTrackWidth.in(Units.Meters) / 2), + new Translation2d(kWheelBase.in(Units.Meters) / 2, -kTrackWidth.in(Units.Meters) / 2), + new Translation2d(-kWheelBase.in(Units.Meters) / 2, kTrackWidth.in(Units.Meters) / 2), + new Translation2d(-kWheelBase.in(Units.Meters) / 2, -kTrackWidth.in(Units.Meters) / 2)); public static final class OIConstants { public static final int kDriverControllerPort = 0; - public static final int kOperatorControllerPort = 1; + public static final int kOperatorJoystickPort = 1; + public static final double kDriveDeadband = 0.06; public static final double kMagnitudeDeadband = 0.06; public static final double kDirectionSlewRate = 10; // radians per second @@ -115,6 +121,8 @@ public static final class SwerveModuleConstants { // robot that drives faster). public static final int kDrivingMotorPinionTeeth = 14; + public static final Measure> kMaxModuleSpeed = Units.MetersPerSecond.of(1); + // Invert the turning encoder, since the output shaft rotates in the opposite direction of // the steering motor in the MAXSwerve Module. public static final boolean kTurningEncoderInverted = true; @@ -164,4 +172,11 @@ public static final class SwerveModuleConstants { public static final int kTurningMotorCurrentLimit = 20; // amps } } -} + + public static final class IntakeConstants { + public static final int kLineBreakSensor = 0; + + // Roller motor ID + public static final int kMotorID = 10; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index 54d0fe48..4c90e6d9 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -1,22 +1,25 @@ package frc.robot.subsystems.drive; import com.ctre.phoenix6.hardware.Pigeon2; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.*; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.RobotConfig.*; -import frc.robot.constants.RobotConstants.*; +import frc.robot.constants.RobotConfig; +import frc.robot.constants.RobotConfig.DriveConfig; +import frc.robot.constants.RobotConstants.DriveConstants; import frc.robot.constants.RobotConstants.DriveConstants.OIConstants; -import frc.robot.subsystems.vision.Vision; import frc.utils.SwerveUtils; import frc.utils.Vector; @@ -28,8 +31,6 @@ public class Drivetrain extends SubsystemBase { private final MAXSwerveModule m_rearLeft; private final MAXSwerveModule m_rearRight; - private final SwerveModulePosition[] SwerveModulePositions; - private Pigeon2 m_gyro; private final PowerDistribution m_powerDistribution; @@ -47,17 +48,22 @@ public class Drivetrain extends SubsystemBase { private SlewRateLimiter m_magLimiter; private SlewRateLimiter m_rotLimiter; + private Vector spdCommanded; private Timer m_timer; - private double m_prevTime; + private double m_prevSlewRateTime; + + private MutableMeasure m_heading; - private Vision m_vision; + // Odometry class for tracking robot pose + SwerveDriveOdometry m_odometry; + private Pose2d m_pose; + private ChassisSpeeds m_relativeSpeeds; - // Pose estimator class for tracking robot pose - SwerveDrivePoseEstimator m_swervePoseEstimator; + private SwerveModulePosition[] m_swerveModulePositions; - /** constructs a new Drivatrain object */ - public Drivetrain(Vision vision) { + /** constructs a new Drivetrain object */ + public Drivetrain() { m_frontLeft = new MAXSwerveModule( DriveConstants.kFrontLeftDrivingCanId, @@ -82,19 +88,22 @@ public Drivetrain(Vision vision) { DriveConstants.kRearRightTurningCanId, DriveConstants.kBackRightChassisAngularOffset); - SwerveModulePositions = + // TODO: initialize this to where we place the robot on the field, will get from auto chosen + // from Smart Dashboard + m_pose = new Pose2d(); + + m_swerveModulePositions = new SwerveModulePosition[] { m_frontLeft.getPosition(), m_frontRight.getPosition(), m_rearLeft.getPosition(), - m_rearRight.getPosition(), + m_rearRight.getPosition() }; - m_currentRotationRadians = 0; - - m_vision = vision; - m_gyro = new Pigeon2(DriveConstants.kGyroId); + m_gyro.reset(); + + m_heading = MutableMeasure.ofBaseUnits(m_gyro.getAngle(), Units.Degrees); m_timer = new Timer(); @@ -102,42 +111,60 @@ public Drivetrain(Vision vision) { m_magLimiter = new SlewRateLimiter(OIConstants.kMagnitudeSlewRate); m_rotLimiter = new SlewRateLimiter(OIConstants.kRotationalSlewRate); + spdCommanded = new Vector(); m_timer.start(); - m_prevTime = m_timer.get(); + m_prevSlewRateTime = m_timer.get(); - m_swervePoseEstimator = - new SwerveDrivePoseEstimator( + m_odometry = + new SwerveDriveOdometry( DriveConstants.kDriveKinematics, Rotation2d.fromRadians(-getGyroAngle().in(Units.Radians)), - SwerveModulePositions, - new Pose2d()); + m_swerveModulePositions, + m_pose); + + configureAutoBuilder(); m_powerDistribution.clearStickyFaults(); SmartDashboard.putNumber("driveVelocity", 0); } - /** runs the periodic functionality of the drivetrain * */ + /** configures the pathplanner AutoBuilder */ + private void configureAutoBuilder() { + AutoBuilder.configureHolonomic( + this::getPose, + this::resetOdometry, + this::getSpeeds, + this::driveChassisSpeeds, + RobotConfig.DriveConfig.kPathFollowerConfig, + this::allianceCheck, + this); + } + + /** + * returns the current speed of the drivetrain + * + * @return the current speed of the drivetrain + */ + public ChassisSpeeds getSpeeds() { + return m_relativeSpeeds; + } + + /** stops the drivetrain's movement */ + public void stop() { + move(Vector.Origin, 0); + } + + /** runs the periodic functionality of the drivetrain */ @Override public void periodic() { - // Update the pose estimator in the periodic block - m_swervePoseEstimator.update( - Rotation2d.fromRadians(-getGyroAngle().in(Units.Radians)), SwerveModulePositions); - - // Update the pose estimator with data from the vision pose estimator - Pose2d visPose = m_vision.getEstimatedPose2d(); - if (visPose != null) { - // var pipelineResult = m_vision.getCam().getLatestResult(); - // var resultTimestamp = pipelineResult.getTimestampSeconds(); - - /*m_swervePoseEstimator.addVisionMeasurement( - m_vision.getEstimatedPose2d(), Timer.getFPGATimestamp());*/ - } - + m_odometry.update(m_gyro.getRotation2d(), m_swerveModulePositions); double ang = getGyroAngle().in(Units.Radians); SmartDashboard.putNumber("delta heading", ang - m_prevAngleRadians); m_prevAngleRadians = ang; + m_relativeSpeeds = getRobotRelativeSpeeds(); + m_pose = m_odometry.getPoseMeters(); SmartDashboard.putNumber("heading", ang - m_headingOffsetRadians); @@ -148,11 +175,18 @@ public void periodic() { /** * Resets the pose estimator to the specified pose. * - * @param pose The pose to which to set the pose estimator. + * @param pose The pose to which to set the estimator. */ - public void resetPoseEstimator(Pose2d pose) { - m_swervePoseEstimator.resetPosition( - Rotation2d.fromRadians(-getGyroAngle().in(Units.Radians)), SwerveModulePositions, pose); + public void resetOdometry(Pose2d pose) { + m_odometry.resetPosition( + Rotation2d.fromRadians(-getGyroAngle().in(Units.Radians)), + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition(), + }, + pose); } /** @@ -175,6 +209,18 @@ public void drive(Vector spdVec, Vector rotVec, boolean altDrive, boolean center } } + /** + * moves the divetrain based on the given ChassisSpeeds + * + * @param spds the target speeds of the drivetrain chassis + */ + public void driveChassisSpeeds(ChassisSpeeds spds) { + Vector spd = new Vector(spds.vxMetersPerSecond, spds.vyMetersPerSecond); + spdCommanded = spd; + double angVel = spds.omegaRadiansPerSecond; + move(spd, angVel); + } + /** * moves the drivetrain using the main turning mode * @@ -182,7 +228,7 @@ public void drive(Vector spdVec, Vector rotVec, boolean altDrive, boolean center * @param ySpeed the proportion of the robot's max velocity to move in the y direction * @param xRot the speed to rotate with (-1, 1) */ - private void mainDrive(Vector spdVec, double xRot) { + public void mainDrive(Vector spdVec, double xRot) { double rot = xRot * DriveConfig.kMaxAngularSpeed; move(spdVec, rot); } @@ -193,8 +239,8 @@ private void mainDrive(Vector spdVec, double xRot) { * @see Measure * @return the angle of the robot gyro */ - private Measure getGyroAngle() { - return Units.Degrees.of(m_gyro.getAngle()); + public Measure getGyroAngle() { + return m_heading.mut_replace(m_gyro.getAngle(), Units.Degrees); } /** @@ -205,7 +251,7 @@ private Measure getGyroAngle() { * @param xRot the x component of the direction vector to point towards * @param yRot the y component of the direction vector to point towards */ - private void altDrive(Vector spdVec, Vector rotVec) { + public void altDrive(Vector spdVec, Vector rotVec) { double rot = 0; m_rightAngGoalRadians = rotVec.angle(); if (rotVec.squaredMag() > 0) { @@ -217,6 +263,27 @@ private void altDrive(Vector spdVec, Vector rotVec) { move(spdVec, rot); } + /** + * returns the current speed of the robot from it's reference frame + * + * @return the current speed of the robot from it's reference frame + */ + public ChassisSpeeds getRobotRelativeSpeeds() { + return DriveConstants.kDriveKinematics.toChassisSpeeds( + new SwerveModuleState[] { + m_frontLeft.getState(), + m_frontRight.getState(), + m_rearLeft.getState(), + m_rearRight.getState() + }); + } + + /** + * applies smoothing to the turning input of altDrive + * + * @param stickAng the given angle of the driver turning stick + * @return the commanded rotation based on the rotation input + */ private double altTurnSmooth(double stickAng) { return Math.tanh( ((getGyroAngle().in(Units.Radians) + stickAng + Math.PI) % (2 * Math.PI) - Math.PI) @@ -224,6 +291,16 @@ private double altTurnSmooth(double stickAng) { * DriveConfig.kMaxAngularSpeed; } + /** + * returns the current position of the robot on the field + * + * @return the current position of the robot on the field + */ + private Pose2d getPose() { + Pose2d pose = m_odometry.getPoseMeters(); + return pose; + } + /** * moves the drivetrain using the given values * @@ -231,7 +308,7 @@ private double altTurnSmooth(double stickAng) { * @param ySpeed the proportion of the robot's max velocity to move in the y direction * @param rot the angular velocity to rotate the drivetrain in radians/s */ - private void move(Vector spdVec, double rot) { + public void move(Vector spdVec, double rot) { move(spdVec, rot, true); } @@ -244,23 +321,28 @@ private void move(Vector spdVec, double rot) { * @param rateLimit whether or not to use slew rate limiting */ private void move(Vector spdVec, double rot, boolean rateLimit) { - Vector spdCommanded = spdVec; m_currentRotationRadians = rot; + spdCommanded.setX(spdVec.x()); + spdCommanded.setY(spdVec.y()); + if (rateLimit) { - spdVec = limitDirectionSlewRate(spdVec); + limitDirectionSlewRate(spdCommanded); m_currentRotationRadians = m_rotLimiter.calculate(rot); + SmartDashboard.putNumber(DriveConfig.kSlewRateTranslationMagOutput, spdCommanded.mag()); + SmartDashboard.putNumber(DriveConfig.kSlewRateTranslationDirRadOutput, spdCommanded.angle()); } // Adjust input based on max speed - Vector spdDelivered = spdCommanded.copy().mult(DriveConfig.kMaxSpeedMetersPerSecond); + spdCommanded.mult(DriveConfig.kMaxSpeedMetersPerSecond); + double rotDelivered = m_currentRotationRadians * DriveConfig.kMaxAngularSpeed; var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( ChassisSpeeds.fromFieldRelativeSpeeds( - spdDelivered.x(), - spdDelivered.y(), + spdCommanded.x(), + spdCommanded.y(), rotDelivered, Rotation2d.fromDegrees(-m_gyro.getAngle()))); SwerveDriveKinematics.desaturateWheelSpeeds( @@ -271,60 +353,74 @@ private void move(Vector spdVec, double rot, boolean rateLimit) { m_rearRight.setDesiredState(swerveModuleStates[3]); } - private Vector limitDirectionSlewRate(Vector spdVec) { + /** + * applies slewrate limiting to the given control vector + * + * @param spdVec the vector which represents the commanded speed of the drivetrain + * @return the slew rate limited Vector for controlling the drivetrain + */ + private void limitDirectionSlewRate(Vector spdVec) { // Convert XY to polar for rate limiting double inputTranslationDir = spdVec.angle(); double inputTranslationMag = spdVec.mag(); // Calculate the direction slew rate based on an estimate of the lateral acceleration double directionSlewRate; - if (m_currentTranslationMag > 1e-4) { + // if very close to zero but not exactly zero, there is no in division by zero due to floating + // point precision errors + if (m_currentTranslationMag != 0) { + // set lower rate of change/slew rate for higher translation speeds directionSlewRate = Math.abs(OIConstants.kDirectionSlewRate / m_currentTranslationMag); } else { - directionSlewRate = - DriveConfig - .HIGH_DIRECTION_SLEW_RATE; // some high number that means the slew rate is effectively - // instantaneous + directionSlewRate = DriveConfig.HIGH_DIRECTION_SLEW_RATE; } double currentTime = m_timer.get(); - double elapsedTime = currentTime - m_prevTime; + double elapsedTime = currentTime - m_prevSlewRateTime; double angleDif = SwerveUtils.AngleDifference(inputTranslationDir, m_currentTranslationDirRadians); - m_prevTime = currentTime; - if (angleDif < DriveConfig.MIN_ANGLE_SLEW_RATE) { m_currentTranslationDirRadians = SwerveUtils.StepTowardsCircular( m_currentTranslationDirRadians, inputTranslationDir, directionSlewRate * elapsedTime); m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); - return (new Vector(m_currentTranslationMag, 0)).rot(m_currentTranslationDirRadians); - } - - if (angleDif > DriveConfig.MAX_ANGLE_SLEW_RATE) { - if (SwerveUtils.approxEqual( - m_currentTranslationMag, - 0)) { // some small number to avoid floating-point errors with equality checking - // keep currentTranslationDir unchanged + SmartDashboard.putNumber("translation magnitude output", inputTranslationMag); + } else if (angleDif > DriveConfig.MAX_ANGLE_SLEW_RATE) { + if (m_currentTranslationMag > 1e-4) { m_currentTranslationMag = m_magLimiter.calculate(0.0); - return (new Vector(m_currentTranslationMag, 0)).rot(m_currentTranslationDirRadians); + } else { + m_currentTranslationDirRadians = + SwerveUtils.WrapAngle(m_currentTranslationDirRadians + Math.PI); + m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); } - + } else { m_currentTranslationDirRadians = - SwerveUtils.WrapAngle(m_currentTranslationDirRadians + Math.PI); - m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); - return (new Vector(m_currentTranslationMag, 0)).rot(m_currentTranslationDirRadians); - } + SwerveUtils.StepTowardsCircular( + m_currentTranslationDirRadians, inputTranslationDir, directionSlewRate * elapsedTime); - m_currentTranslationDirRadians = - SwerveUtils.StepTowardsCircular( - m_currentTranslationDirRadians, inputTranslationDir, directionSlewRate * elapsedTime); + m_currentTranslationMag = m_magLimiter.calculate(0.0); + + m_prevSlewRateTime = currentTime; + } - m_currentTranslationMag = m_magLimiter.calculate(0.0); + spdVec.setX(m_currentTranslationMag); + spdVec.setY(0); + spdVec.rot(m_currentTranslationDirRadians); + } - return (new Vector(m_currentTranslationMag, 0)).rot(m_currentTranslationDirRadians); + /** + * checks whether pathplanner paths should be flipped based on the current alliance + * + * @return whether pathplanner paths should be flipped + */ + private boolean allianceCheck() { + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; } /** Zeroes the heading of the robot. */ @@ -332,10 +428,4 @@ public void zeroHeading() { m_headingOffsetRadians = getGyroAngle().in(Units.Radians); m_gyro.reset(); } - - /** run periodically when being simulated, required but not used in this implementation */ - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 4bb924bf..dd6de4f6 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,18 +1,60 @@ package frc.robot.subsystems.intake; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.RobotConfig; +import frc.robot.constants.RobotConstants.IntakeConstants; +import frc.robot.constants.RobotConstants.ShooterConstants; public class Intake extends SubsystemBase { + private CANSparkMax m_shooterRollerMotor; + private final CANSparkMax m_intakeRollerMotor; // Intake roller motor + private final DigitalInput m_linebreak; + /** Creates a new ExampleSubsystem. */ - public Intake() {} + public Intake() { + // Roller + m_shooterRollerMotor = + new CANSparkMax(ShooterConstants.kRollerMotorLeftId, MotorType.kBrushless); + m_intakeRollerMotor = new CANSparkMax(IntakeConstants.kMotorID, MotorType.kBrushless); + // TODO maybe use to terminate intake command + m_linebreak = new DigitalInput(IntakeConstants.kLineBreakSensor); + } @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putBoolean("Intake/linebreak sensor", m_linebreak.get()); } - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation + // runs the rollers + public void startFeedNote(boolean reverse) { + if (reverse) { + m_shooterRollerMotor.set(-RobotConfig.ShooterConfig.kRollerDefaultSpeed); + } else { + m_shooterRollerMotor.set(RobotConfig.ShooterConfig.kRollerDefaultSpeed); + } + } + + // stops the rollers + public void stopFeedNote() { + m_shooterRollerMotor.stopMotor(); + } + + /** + * Runs the intake at given speed + * + * @param motorOutput Motor speed from -1.0 to 1.0 as a percentage + */ + public void run(double motorOutput) { + m_intakeRollerMotor.set(motorOutput); + } + + /** Stops the roller motor */ + public void stop() { + m_intakeRollerMotor.stopMotor(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index a29e1f7e..72479210 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,13 +1,11 @@ package frc.robot.subsystems.shooter; import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; import edu.wpi.first.units.*; -import edu.wpi.first.units.Measure; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -21,15 +19,8 @@ */ public class Shooter extends SubsystemBase { /** 1. create motor and pid controller objects */ - private CANSparkMax m_rollerMotor; - - private CANSparkMax m_angleMotorLeader; - private CANSparkMax m_angleMotorFollower; - private SparkPIDController m_anglePIDController; - - private RelativeEncoder m_angleEncoder; - private CANSparkMax m_topFlywheelMotor; + private CANSparkMax m_bottomFlywheelMotor; private RelativeEncoder m_topFlywheelEncoder; private RelativeEncoder m_bottomFlywheelEncoder; @@ -39,44 +30,26 @@ public class Shooter extends SubsystemBase { private RelativeEncoder m_shieldEncoder; private MutableMeasure> m_shooterSpeed; - private MutableMeasure m_shooterAngle; - private MutableMeasure m_targetAngle; private MutableMeasure m_shieldPosition; - private MutableMeasure> m_targetVelocity; + private MutableMeasure> m_targetVelocity; + private MutableMeasure m_shooterAngle; + + private CANSparkMax m_angleMotorLeader; + private CANSparkMax m_angleMotorFollower; + private SparkPIDController m_anglePIDController; public Shooter() { - // Roller - m_rollerMotor = new CANSparkMax(ShooterConstants.kRollerMotorLeftId, MotorType.kBrushless); // Flywheel - m_topFlywheelMotor = new CANSparkMax(ShooterConstants.kTopFlywheelMotorId, MotorType.kBrushed); + m_topFlywheelMotor = + new CANSparkMax(ShooterConstants.kTopFlywheelMotorId, MotorType.kBrushless); m_topFlywheelEncoder = m_topFlywheelMotor.getEncoder(); m_topFlywheelPIDController = m_topFlywheelMotor.getPIDController(); m_bottomFlywheelMotor = - new CANSparkMax(ShooterConstants.kBottomFlywheelMotorId, MotorType.kBrushed); + new CANSparkMax(ShooterConstants.kBottomFlywheelMotorId, MotorType.kBrushless); m_bottomFlywheelMotor.follow(m_topFlywheelMotor, true); m_bottomFlywheelEncoder = m_bottomFlywheelMotor.getEncoder(); - // Angle - m_angleMotorLeader = - new CANSparkMax(ShooterConstants.kAngleMotorLeaderId, MotorType.kBrushless); - m_angleMotorFollower = - new CANSparkMax(ShooterConstants.kAngleMotorFollowerId, MotorType.kBrushless); - // sets follower motor to run inversely to the leader - m_angleMotorFollower.follow(m_angleMotorLeader, true); - - m_anglePIDController = m_angleMotorLeader.getPIDController(); - m_anglePIDController.setP(RobotConfig.ShooterConfig.kAngleControlP); - m_anglePIDController.setI(RobotConfig.ShooterConfig.kAngleControlI); - m_anglePIDController.setD(RobotConfig.ShooterConfig.kAngleControlD); - m_anglePIDController.setFF(RobotConfig.ShooterConfig.kAngleControlD); - m_anglePIDController.setIZone(RobotConfig.ShooterConfig.kAngleControlIZone); - m_anglePIDController.setOutputRange( - RobotConfig.ShooterConfig.kAngleControlMinOutput, - RobotConfig.ShooterConfig.kAngleControlMaxOutput); - m_anglePIDController.setIZone(ShooterConfig.kAngleControlIZone); - m_angleEncoder = m_angleMotorLeader.getEncoder(); - // shield m_shieldController = new CANSparkMax(ShooterConstants.kShieldMotorId, MotorType.kBrushless); m_shieldEncoder = m_shieldController.getEncoder(); @@ -94,12 +67,39 @@ public Shooter() { RobotConfig.ShooterConfig.kTopFlywheelMaxOutput); SmartDashboard.putNumber("Flywheel RPM", m_topFlywheelEncoder.getVelocity()); - SmartDashboard.putNumber("Angle Degrees", m_angleEncoder.getPosition()); SmartDashboard.putNumber("Speaker Angle", ShooterConfig.kSpeakerAngle.magnitude()); SmartDashboard.putNumber("Amp Angle", ShooterConfig.kAmpAngle.magnitude()); SmartDashboard.putNumber("Trap Angle", ShooterConfig.kTrapAngle.magnitude()); + SmartDashboard.putNumber("flywheel p", m_topFlywheelPIDController.getP()); + SmartDashboard.putNumber("flywheel i", m_topFlywheelPIDController.getI()); + SmartDashboard.putNumber("flywheel d", m_topFlywheelPIDController.getD()); + + m_targetVelocity = MutableMeasure.zero(Units.RPM); + + + // Angle + m_angleMotorLeader = + new CANSparkMax(ShooterConstants.kAngleMotorLeaderId, MotorType.kBrushless); + m_angleMotorFollower = + new CANSparkMax(ShooterConstants.kAngleMotorFollowerId, MotorType.kBrushless); + // sets follower motor to run inversely to the leader + m_angleMotorFollower.follow(m_angleMotorLeader, true); + + m_anglePIDController = m_angleMotorLeader.getPIDController(); + m_anglePIDController.setP(RobotConfig.ShooterConfig.kAngleControlP); + m_anglePIDController.setI(RobotConfig.ShooterConfig.kAngleControlI); + m_anglePIDController.setD(RobotConfig.ShooterConfig.kAngleControlD); + m_anglePIDController.setFF(RobotConfig.ShooterConfig.kAngleControlD); + m_anglePIDController.setIZone(RobotConfig.ShooterConfig.kAngleControlIZone); + m_anglePIDController.setOutputRange( + RobotConfig.ShooterConfig.kAngleControlMinOutput, + RobotConfig.ShooterConfig.kAngleControlMaxOutput); + m_anglePIDController.setIZone(ShooterConfig.kAngleControlIZone); + + m_shooterAngle = MutableMeasure.zero(Units.Degrees); + if (DriverStation.isTest()) { putAngleOnSmartDashboard(); } @@ -130,58 +130,46 @@ public void putAngleOnSmartDashboard() { @Override public void periodic() { + SmartDashboard.putNumber("shield rots", m_shieldController.getEncoder().getPosition()); if (DriverStation.isTest()) { testPeriodic(); } } void testPeriodic() { - double pAngleController = - SmartDashboard.getNumber(RobotConfig.ShooterConfig.kAngleControlPGainKey, 0); - double iAngleController = - SmartDashboard.getNumber(RobotConfig.ShooterConfig.kAngleControlIGainKey, 0); - double dAngleController = - SmartDashboard.getNumber(RobotConfig.ShooterConfig.kAngleControlDGainKey, 0); - double izAngleController = - SmartDashboard.getNumber(RobotConfig.ShooterConfig.kAngleControlIZoneKey, 0); - double ffAngleController = - SmartDashboard.getNumber(RobotConfig.ShooterConfig.kAngleControlFFGainKey, 0); - + SmartDashboard.putNumber("Shooter/top flywheel output", m_topFlywheelMotor.getAppliedOutput()); + SmartDashboard.putNumber( + "Shooter/bottom flywheel output", m_bottomFlywheelMotor.getAppliedOutput()); double flywheelRPM = - SmartDashboard.getNumber("Flywheel RPM", m_topFlywheelEncoder.getVelocity()); - double angleDegrees = - SmartDashboard.getNumber("Flywheel RPM", m_topFlywheelEncoder.getVelocity()); + SmartDashboard.getNumber("Shooter/Flywheel RPM", m_topFlywheelEncoder.getVelocity()); - // checks PID values against Smart dashboard and applies them to the PID if needed - if (m_anglePIDController.getP() != pAngleController) { - m_anglePIDController.setP(pAngleController); - } - if (m_anglePIDController.getI() != iAngleController) { - m_anglePIDController.setI(iAngleController); - } - if (m_anglePIDController.getD() != dAngleController) { - m_anglePIDController.setD(dAngleController); + double pval = SmartDashboard.getNumber("flywheel p", 0.1); + if (pval != m_topFlywheelPIDController.getP()) { + m_topFlywheelPIDController.setP(pval); } - if (m_anglePIDController.getIZone() != izAngleController) { - m_anglePIDController.setIZone(izAngleController); + double ival = SmartDashboard.getNumber("flywheel i", 0.0); + if (pval != m_topFlywheelPIDController.getI()) { + m_topFlywheelPIDController.setP(ival); } - if (m_anglePIDController.getFF() != ffAngleController) { - m_anglePIDController.setIZone(ffAngleController); + + double dval = SmartDashboard.getNumber("flywheel d", 0.0); + if (pval != m_topFlywheelPIDController.getD()) { + m_topFlywheelPIDController.setP(dval); } if (m_topFlywheelEncoder.getVelocity() != flywheelRPM) { flywheelRPM = m_topFlywheelEncoder.getVelocity(); } - - if (m_angleEncoder.getPosition() != degreesToRotations(angleDegrees)) { - angleDegrees = rotationsToDegrees(m_angleEncoder.getPosition()); - } } // sets the target angle the shooter should be at public void setAngle(Measure targetAngle) { - m_anglePIDController.setReference(targetAngle.in(Units.Rotations), ControlType.kPosition); + m_anglePIDController.setReference(targetAngle.in(Units.Rotations), CANSparkBase.ControlType.kPosition); + } + + public Measure getCurrentAngle() { + return m_shooterAngle.mut_replace(m_angleMotorLeader.getEncoder().getPosition(), Units.Revolutions); } public void stopAngleMotor() { @@ -198,18 +186,9 @@ public double rotationsToDegrees(double rotations) { return angle; } - // runs the rollers - public void startFeedNote() { - m_rollerMotor.set(RobotConfig.ShooterConfig.kRollerDefaultSpeed); - } - - public void setShieldPosition(double position) { - m_shieldController.getEncoder().setPosition(position); - } - - // stops the rollers - public void stopFeedNote() { - m_rollerMotor.stopMotor(); + public void setShield(boolean forward) { + double multiplier = forward ? 1 : -1; + m_shieldController.set(ShooterConfig.kShieldDefaultSpeed * multiplier); } // runs the flywheel at a speed in rotations per minute @@ -223,7 +202,6 @@ public void stopFlywheel() { } public void zeroEncoders() { - m_angleEncoder.setPosition(0); m_topFlywheelEncoder.setPosition(0); m_bottomFlywheelEncoder.setPosition(0); m_shieldEncoder.setPosition(0); @@ -233,19 +211,11 @@ public Measure> getCurrentRPM() { return m_shooterSpeed.mut_replace(m_topFlywheelEncoder.getVelocity(), Units.RPM); } - public Measure getCurrentAngle() { - return m_shooterAngle.mut_replace(m_angleEncoder.getPosition(), Units.Revolutions); - } - - public Measure calculateAngle(double targetX, double targetY) { - return m_targetAngle.mut_replace(Math.atan2(targetY, targetX), Units.Degrees); - } - - public Measure> calculateVelocity(double targetY, Measure targetAngle) { + public Measure> calculateVelocity(double targetY, Measure targetAngle) { return m_targetVelocity.mut_replace( - Math.sqrt(2 * ShooterConstants.Gravity * targetY) - / (Math.sin(targetAngle.in(Units.Degrees))), - Units.MetersPerSecond); + convertToRPM(Math.sqrt(2 * ShooterConstants.Gravity * targetY) + / (Math.sin(targetAngle.in(Units.Degrees)))), + Units.RPM); } public double convertToRPM(double velocity) { @@ -256,11 +226,11 @@ public double convertToRPM(double velocity) { } // returns true if extended - public boolean getShieldStatus() { - if (Math.abs(m_shieldEncoder.getPosition()) < ShooterConfig.kShieldExtendedPosition) { - return false; + public boolean getShieldStatus(boolean extend) { + if (extend) { + return Math.abs(m_shieldEncoder.getPosition()) > ShooterConfig.kShieldExtendedPosition; } else { - return true; + return Math.abs(m_shieldEncoder.getPosition()) < ShooterConfig.kShieldRetractedPosition; } } @@ -268,12 +238,16 @@ public Measure getShieldPosition() { return m_shieldPosition.mut_replace(m_shieldEncoder.getPosition(), Units.Rotations); } + public void setShieldPosition(double position) { + m_shieldController.getEncoder().setPosition(position); + } + public void stopShieldMotor() { m_shieldController.stopMotor(); } public boolean isAtAngleSetpoint(double setpoint) { - return Math.abs(m_angleEncoder.getPosition() - setpoint) + return Math.abs(m_angleMotorLeader.getEncoder().getPosition() - setpoint) < ShooterConfig.kAngleError.magnitude(); } @@ -281,4 +255,4 @@ public boolean isAtFlywheelSetpoint(double setpoint) { return Math.abs(m_topFlywheelEncoder.getPosition() - setpoint) < ShooterConfig.kFlywheelError.magnitude(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/utils/Vector.java b/src/main/java/frc/utils/Vector.java index d7de1ff8..c1ed0d7a 100644 --- a/src/main/java/frc/utils/Vector.java +++ b/src/main/java/frc/utils/Vector.java @@ -5,6 +5,8 @@ // with operations for manipulating them public class Vector { + public static final Vector Origin = new Vector(0, 0); + // the array of values for the location of the Point, // from lowest dimension to highest, // ie. x-value is vals[0], y-value is vals[1], etc. @@ -423,9 +425,11 @@ public Vector getPerpendicular() { * @return this Vector */ public Vector rot(double theta) { - Vector newXLoc = new Vector(Math.cos(theta), Math.sin(theta)); - Vector newYLoc = newXLoc.getPerpendicular(); - return matrixTransform(newXLoc, newYLoc); + double prevX = x(); + double prevY = y(); + setX(Math.cos(theta) * prevX - Math.sin(theta) * prevY); + setY(Math.cos(theta) * prevY + Math.sin(theta) * prevX); + return this; } /** @@ -555,4 +559,4 @@ public boolean isWithinBounds(Vector boundsMin, Vector boundsMax) { return minX <= x() && x() <= maxX && minY <= y() && y() <= maxY; } -} +} \ No newline at end of file From 69a985625138dc3a7cdef24e9a90c800a059509d Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Fri, 1 Mar 2024 07:49:48 -0800 Subject: [PATCH 09/51] integrated pivot --- src/main/java/frc/robot/RobotContainer.java | 9 ++-- .../robot/commands/shooter/ActuateShield.java | 14 +---- .../java/frc/robot/commands/shooter/Aim.java | 17 +++---- .../frc/robot/commands/shooter/Shoot.java | 14 ++--- .../java/frc/robot/constants/RobotConfig.java | 26 ++++++---- .../frc/robot/constants/RobotConstants.java | 28 ++++++---- .../frc/robot/subsystems/intake/Intake.java | 50 ++++++++++++++++-- .../frc/robot/subsystems/shooter/Shooter.java | 51 ++++++++----------- 8 files changed, 119 insertions(+), 90 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4141afe0..112b46f6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,12 +13,12 @@ import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.BasicDriveCommand; -import frc.robot.commands.VisionTurnCommand; import frc.robot.commands.shooter.*; import frc.robot.constants.RobotConfig.*; import frc.robot.constants.RobotConstants.*; import frc.robot.constants.RobotConstants.DriveConstants.OIConstants; import frc.robot.subsystems.drive.Drivetrain; +import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.vision.Vision; import frc.utils.Vector; @@ -33,6 +33,7 @@ public class RobotContainer { private Vision m_vision; private Drivetrain m_robotDrive; private Shooter m_shooter; + private Intake m_intake; public RobotContainer() { m_driverController = new XboxController(OIConstants.kDriverControllerPort); @@ -40,6 +41,7 @@ public RobotContainer() { m_shooter = new Shooter(); m_vision = new Vision(); m_robotDrive = new Drivetrain(m_vision); + m_intake = new Intake(); configureBindings(); m_shooter.setDefaultCommand( @@ -73,7 +75,7 @@ private void configureBindings() { // just shoot on trigger new Trigger(() -> m_operatorController.getRawButton(Bindings.kShoot)) - .onTrue(new Shoot(m_shooter)); + .onTrue(new Shoot(m_intake)); // aim amp new Trigger(() -> m_operatorController.getRawButton(Bindings.kAimAmp)) .onTrue(new Aim(m_shooter, FieldElement.AMP)); @@ -99,9 +101,6 @@ private void configureBindings() { new Trigger(() -> triggerPressed()) .whileTrue(new BasicDriveCommand(m_robotDrive, m_driverController)); - - new Trigger(() -> m_driverController.getBButton()) - .onTrue(new VisionTurnCommand(m_vision, m_robotDrive, m_driverController)); } public boolean triggerPressed() { diff --git a/src/main/java/frc/robot/commands/shooter/ActuateShield.java b/src/main/java/frc/robot/commands/shooter/ActuateShield.java index d4496d70..c6250c8d 100644 --- a/src/main/java/frc/robot/commands/shooter/ActuateShield.java +++ b/src/main/java/frc/robot/commands/shooter/ActuateShield.java @@ -1,7 +1,6 @@ package frc.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.RobotConfig.ShooterConfig; import frc.robot.subsystems.shooter.Shooter; public class ActuateShield extends Command { @@ -17,11 +16,7 @@ public ActuateShield(Shooter shooter, boolean extend) { @Override public void initialize() { - if (m_shieldState) { - m_shooter.setShieldPosition(ShooterConfig.kShieldExtendedRotations); - } else { - m_shooter.setShieldPosition(ShooterConfig.kShieldRetractedRotations); - } + m_shooter.setShield(m_shieldState); } @Override @@ -31,11 +26,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - // if we want the shield to be out, return true if that is the status - if (m_shieldState) { - return m_shooter.getShieldStatus(); - } else { - return !m_shooter.getShieldStatus(); - } + return m_shooter.getShieldStatus(m_shieldState); } } diff --git a/src/main/java/frc/robot/commands/shooter/Aim.java b/src/main/java/frc/robot/commands/shooter/Aim.java index 61365203..dabbcec6 100644 --- a/src/main/java/frc/robot/commands/shooter/Aim.java +++ b/src/main/java/frc/robot/commands/shooter/Aim.java @@ -50,34 +50,29 @@ public void initialize() { switch (m_type) { case AMP: desiredAngle = ShooterConfig.kAmpAngle; - desiredVelocity = getVelocity(ShooterConfig.AmpHeight); - m_shooter.setShieldPosition(ShooterConfig.kShieldExtendedRotations); + desiredVelocity = ShooterConfig.kDefaultAmpVelocity; break; case SPEAKER: desiredAngle = ShooterConfig.kSpeakerAngle; - desiredVelocity = getVelocity(ShooterConfig.SpeakerHeight); + desiredVelocity = ShooterConfig.kDefaultSpeakerVelocity; break; case TRAP: desiredAngle = ShooterConfig.kTrapAngle; - m_shooter.setShieldPosition(ShooterConfig.kShieldRetractedRotations); - desiredVelocity = getVelocity(ShooterConfig.TrapHeight); + desiredVelocity = ShooterConfig.kDefaultTrapVelocity; break; default: desiredAngle = Units.Degrees.of(0); + desiredVelocity = 0; break; } + m_shooter.setAngle(desiredAngle); m_shooter.runFlywheel(desiredVelocity); } } - public double getVelocity(double elementHeight) { - return m_shooter.convertToRPM(m_shooter.calculateVelocity(ShooterConfig.AmpHeight, desiredAngle).magnitude()); - } - public boolean isFinished() { return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()) - && ((m_type == FieldElement.AMP || m_type == FieldElement.TRAP) - && m_shooter.getShieldStatus()); // check if shield is extended + && m_shooter.isAtFlywheelSetpoint(desiredVelocity); } } diff --git a/src/main/java/frc/robot/commands/shooter/Shoot.java b/src/main/java/frc/robot/commands/shooter/Shoot.java index 4da90f4f..777410fa 100644 --- a/src/main/java/frc/robot/commands/shooter/Shoot.java +++ b/src/main/java/frc/robot/commands/shooter/Shoot.java @@ -3,27 +3,27 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConfig; -import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.intake.Intake; public class Shoot extends Command { - private final Shooter m_shooter; + private final Intake m_intake; private double timer; - public Shoot(Shooter shooter) { - m_shooter = shooter; + public Shoot(Intake intake) { + m_intake = intake; - addRequirements(m_shooter); + addRequirements(m_intake); } @Override public void initialize() { timer = Timer.getFPGATimestamp(); - m_shooter.startFeedNote(); + m_intake.startFeedNote(true); } @Override public void end(boolean interrupted) { - m_shooter.stopFeedNote(); + m_intake.stopFeedNote(); } @Override diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 70ce9044..06ca2db1 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -63,17 +63,18 @@ public static final class ShooterConfig { // Flywheel default speed public static final double kFlywheelDefaultRPM = 0; // Shield Extended position - public static final double kShieldExtendedRotations = 124.140855612; - // Shield Retracted position - public static final double kShieldRetractedRotations = 0; + public static final double kShieldDefaultSpeed = 0.3; + // Timeout time (in seconds) public static final double kRunIntakeTimeoutTime = 0; - public static final double kShieldExtendedPosition = 10; // TODO get correct value + + public static final double kShieldExtendedPosition = 85; + public static final double kShieldRetractedPosition = 15; // Speaker height public static final double SpeakerHeight = 1.9812; public static final double AmpHeight = .46; public static final double ShooterHeight = 0.28575; - public static final double TrapHeight = -1; + public static final double TrapHeight = 1; public static final double SpeakerBillLength = 0.6604; @@ -85,11 +86,16 @@ public static final class ShooterConfig { public static final long kReleaseTime = 500; public static final Measure> kFlywheelError = Units.RPM.of(1); - public static final Measure kAngleError = Units.Radians.of(0.5*Math.PI/180); - public static final Measure kSpeakerAngle = Units.Radians.of(75*Math.PI/180); - public static final Measure kAmpAngle = Units.Radians.of(109*Math.PI/180); - public static final Measure kTrapAngle = Units.Radians.of(105*Math.PI/180); - public static final Measure kAdjustAmountDegrees = Units.Radians.of(0.5*Math.PI/180); + public static final Measure kAngleError = Units.Radians.of(0.5 * Math.PI / 180); + public static final Measure kSpeakerAngle = Units.Radians.of(75 * Math.PI / 180); + public static final Measure kAmpAngle = Units.Radians.of(109 * Math.PI / 180); + public static final Measure kTrapAngle = Units.Radians.of(105 * Math.PI / 180); + public static final Measure kAdjustAmountDegrees = Units.Radians.of(0.5 * Math.PI / 180); + + // rpm + public static final double kDefaultTrapVelocity = 1000; + public static final double kDefaultAmpVelocity = 600; + public static final double kDefaultSpeakerVelocity = 2000; } public static class DriveConfig { diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index b9255bf8..69552ba2 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -46,13 +46,12 @@ public static final class NeoMotorConstants { } public final class ShooterConstants { - public static final int kRollerMotorLeftId = -1; - public static final int kRollerMotorRightId = -1; + public static final int kRollerMotorId = 15; public static final int kAngleMotorLeaderId = -1; public static final int kAngleMotorFollowerId = -1; - public static final int kTopFlywheelMotorId = -1; - public static final int kBottomFlywheelMotorId = -1; - public static final int kShieldMotorId = -1; + public static final int kTopFlywheelMotorId = 16; + public static final int kBottomFlywheelMotorId = 17; + public static final int kShieldMotorId = 18; public static final double FlywheelDiameter = 0.0762; public static final double ShooterLength = 0.4064; public static final double Gravity = 9.81; @@ -70,17 +69,17 @@ public static final class DriveConstants { public static final double kDriveDeadband = 0.06; - public static final int kFrontLeftDrivingCanId = 6; - public static final int kFrontLeftTurningCanId = 5; + public static final int kFrontLeftDrivingCanId = 2; + public static final int kFrontLeftTurningCanId = 1; - public static final int kFrontRightDrivingCanId = 8; - public static final int kFrontRightTurningCanId = 7; + public static final int kFrontRightDrivingCanId = 6; + public static final int kFrontRightTurningCanId = 5; public static final int kRearLeftDrivingCanId = 4; public static final int kRearLeftTurningCanId = 3; - public static final int kRearRightDrivingCanId = 2; - public static final int kRearRightTurningCanId = 1; + public static final int kRearRightDrivingCanId = 8; + public static final int kRearRightTurningCanId = 7; public static final int kGyroId = 15; @@ -164,4 +163,11 @@ public static final class SwerveModuleConstants { public static final int kTurningMotorCurrentLimit = 20; // amps } } + + public static final class IntakeConstants { + public static final int kLineBreakSensor = 0; + + // Roller motor ID + public static final int kMotorID = 10; + } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 4bb924bf..9b81c3c1 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,18 +1,60 @@ package frc.robot.subsystems.intake; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.RobotConfig; +import frc.robot.constants.RobotConstants.IntakeConstants; +import frc.robot.constants.RobotConstants.ShooterConstants; public class Intake extends SubsystemBase { + private CANSparkMax m_shooterRollerMotor; + private final CANSparkMax m_intakeRollerMotor; // Intake roller motor + private final DigitalInput m_linebreak; + /** Creates a new ExampleSubsystem. */ - public Intake() {} + public Intake() { + // Roller + m_shooterRollerMotor = + new CANSparkMax(ShooterConstants.kRollerMotorId, MotorType.kBrushless); + m_intakeRollerMotor = new CANSparkMax(IntakeConstants.kMotorID, MotorType.kBrushless); + // TODO maybe use to terminate intake command + m_linebreak = new DigitalInput(IntakeConstants.kLineBreakSensor); + } @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putBoolean("Intake/linebreak sensor", m_linebreak.get()); } - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation + // runs the rollers + public void startFeedNote(boolean reverse) { + if (reverse) { + m_shooterRollerMotor.set(-RobotConfig.ShooterConfig.kRollerDefaultSpeed); + } else { + m_shooterRollerMotor.set(RobotConfig.ShooterConfig.kRollerDefaultSpeed); + } + } + + // stops the rollers + public void stopFeedNote() { + m_shooterRollerMotor.stopMotor(); + } + + /** + * Runs the intake at given speed + * + * @param motorOutput Motor speed from -1.0 to 1.0 as a percentage + */ + public void run(double motorOutput) { + m_intakeRollerMotor.set(motorOutput); + } + + /** Stops the roller motor */ + public void stop() { + m_intakeRollerMotor.stopMotor(); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index a29e1f7e..d54b63da 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -7,7 +7,6 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; import edu.wpi.first.units.*; -import edu.wpi.first.units.Measure; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -21,8 +20,6 @@ */ public class Shooter extends SubsystemBase { /** 1. create motor and pid controller objects */ - private CANSparkMax m_rollerMotor; - private CANSparkMax m_angleMotorLeader; private CANSparkMax m_angleMotorFollower; private SparkPIDController m_anglePIDController; @@ -38,16 +35,13 @@ public class Shooter extends SubsystemBase { private CANSparkMax m_shieldController; private RelativeEncoder m_shieldEncoder; - private MutableMeasure> m_shooterSpeed; + private MutableMeasure> m_shooterSpeed; + private MutableMeasure> m_targetVelocity; private MutableMeasure m_shooterAngle; private MutableMeasure m_targetAngle; private MutableMeasure m_shieldPosition; - private MutableMeasure> m_targetVelocity; public Shooter() { - // Roller - m_rollerMotor = new CANSparkMax(ShooterConstants.kRollerMotorLeftId, MotorType.kBrushless); - // Flywheel m_topFlywheelMotor = new CANSparkMax(ShooterConstants.kTopFlywheelMotorId, MotorType.kBrushed); m_topFlywheelEncoder = m_topFlywheelMotor.getEncoder(); @@ -100,6 +94,12 @@ public Shooter() { SmartDashboard.putNumber("Amp Angle", ShooterConfig.kAmpAngle.magnitude()); SmartDashboard.putNumber("Trap Angle", ShooterConfig.kTrapAngle.magnitude()); + m_targetAngle = MutableMeasure.zero(Units.Degrees); + m_shooterAngle = MutableMeasure.mutable(getCurrentAngle()); + m_targetVelocity = MutableMeasure.zero(Units.MetersPerSecond); + m_shooterSpeed = MutableMeasure.zero(Units.MetersPerSecond); + m_shieldPosition = MutableMeasure.zero(Units.Rotations); + if (DriverStation.isTest()) { putAngleOnSmartDashboard(); } @@ -198,18 +198,10 @@ public double rotationsToDegrees(double rotations) { return angle; } - // runs the rollers - public void startFeedNote() { - m_rollerMotor.set(RobotConfig.ShooterConfig.kRollerDefaultSpeed); - } - - public void setShieldPosition(double position) { - m_shieldController.getEncoder().setPosition(position); - } - // stops the rollers - public void stopFeedNote() { - m_rollerMotor.stopMotor(); + public void setShield(boolean forward) { + double multiplier = forward ? 1 : -1; + m_shieldController.set(ShooterConfig.kShieldDefaultSpeed * multiplier); } // runs the flywheel at a speed in rotations per minute @@ -229,8 +221,8 @@ public void zeroEncoders() { m_shieldEncoder.setPosition(0); } - public Measure> getCurrentRPM() { - return m_shooterSpeed.mut_replace(m_topFlywheelEncoder.getVelocity(), Units.RPM); + public Measure> getCurrentRPM() { + return m_shooterSpeed.mut_replace(m_topFlywheelEncoder.getVelocity(), Units.MetersPerSecond); } public Measure getCurrentAngle() { @@ -243,24 +235,23 @@ public Measure calculateAngle(double targetX, double targetY) { public Measure> calculateVelocity(double targetY, Measure targetAngle) { return m_targetVelocity.mut_replace( - Math.sqrt(2 * ShooterConstants.Gravity * targetY) - / (Math.sin(targetAngle.in(Units.Degrees))), + Math.abs( + Math.sqrt(2 * ShooterConstants.Gravity * targetY) + / (Math.sin(targetAngle.magnitude()))), Units.MetersPerSecond); } public double convertToRPM(double velocity) { - // 0.0762 meters is diameter of flywheel double circumference = ShooterConstants.FlywheelDiameter * Math.PI; - double rpm = velocity / circumference; + double rpm = velocity / circumference * 60; return rpm; } - // returns true if extended - public boolean getShieldStatus() { - if (Math.abs(m_shieldEncoder.getPosition()) < ShooterConfig.kShieldExtendedPosition) { - return false; + public boolean getShieldStatus(boolean extend) { + if (extend) { + return Math.abs(m_shieldEncoder.getPosition()) > ShooterConfig.kShieldExtendedPosition; } else { - return true; + return Math.abs(m_shieldEncoder.getPosition()) < ShooterConfig.kShieldRetractedPosition; } } From 071357bb816625ba6d4bae6d847f2d36f3323294 Mon Sep 17 00:00:00 2001 From: iris Date: Fri, 1 Mar 2024 07:56:03 -0800 Subject: [PATCH 10/51] fixed ids --- .../java/frc/robot/constants/RobotConstants.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 40d038d6..776c15df 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -51,12 +51,12 @@ public static final class NeoMotorConstants { } public final class ShooterConstants { - public static final int kRollerMotorLeftId = 1; - public static final int kTopFlywheelMotorId = 2; - public static final int kBottomFlywheelMotorId = 3; - public static final int kShieldMotorId = 4; - public static final int kAngleMotorLeaderId = -1; - public static final int kAngleMotorFollowerId = -1; + public static final int kRollerMotorLeftId = 15; + public static final int kTopFlywheelMotorId = 16; + public static final int kBottomFlywheelMotorId = 17; + public static final int kShieldMotorId = 18; + public static final int kAngleMotorLeaderId = 13; + public static final int kAngleMotorFollowerId = 14; public static final double FlywheelDiameter = 0.0762; public static final double ShooterLength = 0.4064; public static final double Gravity = 9.81; From 30fe31d5a54e83000c4fdae2bb104325394ff665 Mon Sep 17 00:00:00 2001 From: iris Date: Sat, 2 Mar 2024 12:53:00 -0800 Subject: [PATCH 11/51] added absolute encoder + constant types --- .../java/frc/robot/constants/RobotConfig.java | 20 +++++++++---------- .../frc/robot/subsystems/shooter/Shooter.java | 5 ++++- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 6f343468..cd824c1f 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -28,13 +28,13 @@ public enum FieldElement { public static final class ShooterConfig { // Angle controller PID coefficients - public static final double kAngleControlP = 0; + public static final double kAngleControlP = 0.1; public static final double kAngleControlI = 0; public static final double kAngleControlD = 0; - public static final double kAngleControlFF = 0; - public static final double kAngleControlIZone = 0; - public static final double kAngleControlMinOutput = 0; - public static final double kAngleControlMaxOutput = 0; + public static final double kAngleControlFF = 0.001; + public static final double kAngleControlIZone = 0.0001; + public static final double kAngleControlMinOutput = -1; + public static final double kAngleControlMaxOutput = 1; // top Flywheel controller PID coefficients public static final double kTopFlywheelP = 0.2; @@ -90,11 +90,11 @@ public static final class ShooterConfig { public static final long kShieldTime = 2; // seconds public static final double kShieldDefaultSpeed = 0.5; public static final Measure> kFlywheelError = Units.RPM.of(1); - public static final Measure kAngleError = Units.Radians.of(0.5 * Math.PI / 180); - public static final Measure kSpeakerAngle = Units.Radians.of(75 * Math.PI / 180); - public static final Measure kAmpAngle = Units.Radians.of(109 * Math.PI / 180); - public static final Measure kTrapAngle = Units.Radians.of(105 * Math.PI / 180); - public static final Measure kAdjustAmountDegrees = Units.Radians.of(0.5 * Math.PI / 180); + public static final Measure kAngleError = Units.Rotations.of(0.5 / 360); + public static final Measure kSpeakerAngle = Units.Rotations.of(75 / 360); + public static final Measure kAmpAngle = Units.Rotations.of(109 / 360); + public static final Measure kTrapAngle = Units.Rotations.of(105 / 360); + public static final Measure kAdjustAmountDegrees = Units.Rotations.of(0.5 / 360); public static final double kDefaultAmpVelocity = 1500; // rpm public static final double kDefaultTrapVelocity = 2000; // rpm diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index e4ce14a0..d99b26c1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.shooter; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; @@ -22,6 +23,7 @@ public class Shooter extends SubsystemBase { private CANSparkMax m_angleMotorLeader; private CANSparkMax m_angleMotorFollower; private SparkPIDController m_anglePIDController; + private AbsoluteEncoder m_angleEncoder; private CANSparkMax m_topFlywheelMotor; @@ -86,6 +88,7 @@ public Shooter() { new CANSparkMax(ShooterConstants.kAngleMotorFollowerId, MotorType.kBrushless); // sets follower motor to run inversely to the leader m_angleMotorFollower.follow(m_angleMotorLeader, true); + m_angleEncoder = m_angleMotorLeader.getAbsoluteEncoder(); m_anglePIDController = m_angleMotorLeader.getPIDController(); m_anglePIDController.setP(RobotConfig.ShooterConfig.kAngleControlP); @@ -169,7 +172,7 @@ public void setAngle(Measure targetAngle) { } public Measure getCurrentAngle() { - return m_shooterAngle.mut_replace(m_angleMotorLeader.getEncoder().getPosition(), Units.Revolutions); + return m_shooterAngle.mut_replace(m_angleEncoder.getPosition(), Units.Revolutions); } public void stopAngleMotor() { From 7414d62332e34e6742de6331a277a4d0a0e8b2f8 Mon Sep 17 00:00:00 2001 From: iris Date: Sat, 2 Mar 2024 14:07:39 -0800 Subject: [PATCH 12/51] shooter commands --- .../robot/commands/shooter/ManualAdjust.java | 43 +++++++++++++++++++ .../robot/commands/shooter/StowShooter.java | 33 ++++++++++++++ .../frc/robot/subsystems/shooter/Shooter.java | 1 + 3 files changed, 77 insertions(+) create mode 100644 src/main/java/frc/robot/commands/shooter/ManualAdjust.java create mode 100644 src/main/java/frc/robot/commands/shooter/StowShooter.java diff --git a/src/main/java/frc/robot/commands/shooter/ManualAdjust.java b/src/main/java/frc/robot/commands/shooter/ManualAdjust.java new file mode 100644 index 00000000..3b5699aa --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/ManualAdjust.java @@ -0,0 +1,43 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConfig.*; +import frc.robot.constants.RobotConfig.AdjustType; +import frc.robot.subsystems.shooter.Shooter; + +public class ManualAdjust extends Command { + private final Shooter m_shooter; + private final AdjustType m_type; + private Measure desiredAngle; + + public ManualAdjust(Shooter shooter, AdjustType type) { + m_shooter = shooter; + m_type = type; + addRequirements(m_shooter); + } + + @Override + public void initialize() { + switch (m_type) { + case up: + desiredAngle = m_shooter.getCurrentAngle().plus(ShooterConfig.kAdjustAmountDegrees); + m_shooter.setAngle(desiredAngle); + break; + case down: + desiredAngle = m_shooter.getCurrentAngle().minus(ShooterConfig.kAdjustAmountDegrees); + m_shooter.setAngle(desiredAngle); + break; + default: + desiredAngle = m_shooter.getCurrentAngle(); + m_shooter.setAngle(desiredAngle); + break; + } + } + + @Override + public boolean isFinished() { + return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/shooter/StowShooter.java b/src/main/java/frc/robot/commands/shooter/StowShooter.java new file mode 100644 index 00000000..b8e54932 --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/StowShooter.java @@ -0,0 +1,33 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.units.Units; +// done +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConfig.ShooterConfig; +import frc.robot.subsystems.shooter.Shooter; + +public class StowShooter extends Command { + private final Shooter m_shooter; + + public StowShooter(Shooter shooter) { + m_shooter = shooter; + + addRequirements(m_shooter); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + m_shooter.setAngle(Units.Degrees.of(ShooterConfig.kShooterStowAngle)); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return m_shooter.isAtAngleSetpoint(ShooterConfig.kShooterStowAngle); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d99b26c1..c1a371ba 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -89,6 +89,7 @@ public Shooter() { // sets follower motor to run inversely to the leader m_angleMotorFollower.follow(m_angleMotorLeader, true); m_angleEncoder = m_angleMotorLeader.getAbsoluteEncoder(); + m_angleEncoder.setZeroOffset(0); m_anglePIDController = m_angleMotorLeader.getPIDController(); m_anglePIDController.setP(RobotConfig.ShooterConfig.kAngleControlP); From c8e36bbacee2fadaf925b22029ca1e541143540e Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Sat, 2 Mar 2024 15:34:26 -0800 Subject: [PATCH 13/51] pivot commands --- .../deploy/pathplanner/paths/leave 1.path | 8 ++-- .../robot/commands/shooter/ManualAdjust.java | 43 +++++++++++++++++++ .../robot/commands/shooter/StowShooter.java | 33 ++++++++++++++ 3 files changed, 80 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/commands/shooter/ManualAdjust.java create mode 100644 src/main/java/frc/robot/commands/shooter/StowShooter.java diff --git a/src/main/deploy/pathplanner/paths/leave 1.path b/src/main/deploy/pathplanner/paths/leave 1.path index 2d636bf7..5dbe6be7 100644 --- a/src/main/deploy/pathplanner/paths/leave 1.path +++ b/src/main/deploy/pathplanner/paths/leave 1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.449326898653798, - "y": 7.0 + "x": 3.541181596252082, + "y": 7.124496534707355 }, "prevControl": { - "x": 5.499618999237998, - "y": 7.0 + "x": 1.5914736968362821, + "y": 7.124496534707355 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/commands/shooter/ManualAdjust.java b/src/main/java/frc/robot/commands/shooter/ManualAdjust.java new file mode 100644 index 00000000..4c780213 --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/ManualAdjust.java @@ -0,0 +1,43 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConfig.*; +import frc.robot.constants.RobotConfig.AdjustType; +import frc.robot.subsystems.shooter.Shooter; + +public class ManualAdjust extends Command { + private final Shooter m_shooter; + private final AdjustType m_type; + private Measure desiredAngle; + + public ManualAdjust(Shooter shooter, AdjustType type) { + m_shooter = shooter; + m_type = type; + addRequirements(m_shooter); + } + + @Override + public void initialize() { + switch (m_type) { + case up: + desiredAngle = m_shooter.getCurrentAngle().plus(ShooterConfig.kAdjustAmountDegrees); + m_shooter.setAngle(desiredAngle); + break; + case down: + desiredAngle = m_shooter.getCurrentAngle().minus(ShooterConfig.kAdjustAmountDegrees); + m_shooter.setAngle(desiredAngle); + break; + default: + desiredAngle = m_shooter.getCurrentAngle(); + m_shooter.setAngle(desiredAngle); + break; + } + } + + @Override + public boolean isFinished() { + return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()); + } +} diff --git a/src/main/java/frc/robot/commands/shooter/StowShooter.java b/src/main/java/frc/robot/commands/shooter/StowShooter.java new file mode 100644 index 00000000..eba8064b --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/StowShooter.java @@ -0,0 +1,33 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.units.Units; +// done +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConfig.ShooterConfig; +import frc.robot.subsystems.shooter.Shooter; + +public class StowShooter extends Command { + private final Shooter m_shooter; + + public StowShooter(Shooter shooter) { + m_shooter = shooter; + + addRequirements(m_shooter); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + m_shooter.setAngle(Units.Degrees.of(ShooterConfig.kShooterStowAngle)); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return m_shooter.isAtAngleSetpoint(ShooterConfig.kShooterStowAngle); + } +} From b148e52d3bdbe5d24ad3569f4d855f3c25394258 Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Sat, 2 Mar 2024 17:10:27 -0800 Subject: [PATCH 14/51] added variable feedforward to pivot position calcualtions --- .../java/frc/robot/commands/shooter/Aim.java | 12 +++++++++- .../robot/commands/shooter/ManualAdjust.java | 13 +++++++++-- .../robot/commands/shooter/StowShooter.java | 6 +++-- .../java/frc/robot/constants/RobotConfig.java | 4 +++- .../frc/robot/subsystems/shooter/Shooter.java | 22 ++++++++++++++++--- 5 files changed, 48 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/commands/shooter/Aim.java b/src/main/java/frc/robot/commands/shooter/Aim.java index 46652a03..0f45691d 100644 --- a/src/main/java/frc/robot/commands/shooter/Aim.java +++ b/src/main/java/frc/robot/commands/shooter/Aim.java @@ -5,6 +5,7 @@ import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; import edu.wpi.first.units.Velocity; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConfig.FieldElement; import frc.robot.constants.RobotConfig.ShooterConfig; @@ -17,11 +18,13 @@ public class Aim extends Command { private final FieldElement m_type; private Measure desiredAngle; private double desiredVelocity; + private Timer timer; public Aim(Shooter shooter, FieldElement type) { m_shooter = shooter; m_vision = new Vision(); m_type = type; + timer = new Timer(); addRequirements(m_shooter); } @@ -35,6 +38,7 @@ public Aim(Shooter shooter, Vision eyes) { @Override public void initialize() { + timer.start(); if (m_type == null) { if (m_vision.getHasTarget()) { double desiredAngle = @@ -72,8 +76,14 @@ public void initialize() { } } + @Override + public void execute() { + m_shooter.setFF(Math.cos(m_shooter.getCurrentAngle().in(Units.Radians))*ShooterConfig.kAngleControlFF); + } + public boolean isFinished() { - return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()) && m_shooter.isAtFlywheelSetpoint(desiredVelocity); + return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()) && m_shooter.isAtFlywheelSetpoint(desiredVelocity) || + timer.get() > ShooterConfig.kAimTimeout; } public double getVelocity(double elementHeight) { diff --git a/src/main/java/frc/robot/commands/shooter/ManualAdjust.java b/src/main/java/frc/robot/commands/shooter/ManualAdjust.java index 4c780213..64cd69ae 100644 --- a/src/main/java/frc/robot/commands/shooter/ManualAdjust.java +++ b/src/main/java/frc/robot/commands/shooter/ManualAdjust.java @@ -2,24 +2,28 @@ import edu.wpi.first.units.Angle; import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConfig.*; -import frc.robot.constants.RobotConfig.AdjustType; import frc.robot.subsystems.shooter.Shooter; public class ManualAdjust extends Command { private final Shooter m_shooter; private final AdjustType m_type; private Measure desiredAngle; + private Timer timer; public ManualAdjust(Shooter shooter, AdjustType type) { m_shooter = shooter; m_type = type; + timer = new Timer(); addRequirements(m_shooter); } @Override public void initialize() { + timer.start(); switch (m_type) { case up: desiredAngle = m_shooter.getCurrentAngle().plus(ShooterConfig.kAdjustAmountDegrees); @@ -36,8 +40,13 @@ public void initialize() { } } + @Override + public void execute() { + m_shooter.setFF(Math.cos(m_shooter.getCurrentAngle().in(Units.Radians))*ShooterConfig.kAngleControlFF); + } + @Override public boolean isFinished() { - return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()); + return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()) || timer.get() > ShooterConfig.kAimTimeout; } } diff --git a/src/main/java/frc/robot/commands/shooter/StowShooter.java b/src/main/java/frc/robot/commands/shooter/StowShooter.java index eba8064b..ea7a35f8 100644 --- a/src/main/java/frc/robot/commands/shooter/StowShooter.java +++ b/src/main/java/frc/robot/commands/shooter/StowShooter.java @@ -16,11 +16,13 @@ public StowShooter(Shooter shooter) { } @Override - public void initialize() {} + public void initialize() { + m_shooter.setAngle(Units.Degrees.of(ShooterConfig.kShooterStowAngle)); + } @Override public void execute() { - m_shooter.setAngle(Units.Degrees.of(ShooterConfig.kShooterStowAngle)); + m_shooter.setFF(Math.cos(m_shooter.getCurrentAngle().in(Units.Radians))*ShooterConfig.kAngleControlFF); } @Override diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index cd824c1f..ccdfc5bd 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -88,6 +88,8 @@ public static final class ShooterConfig { public static final long kReleaseTime = 5000; public static final long kShieldTime = 2; // seconds + public static final double kAimTimeout = 2; + public static final double kShieldDefaultSpeed = 0.5; public static final Measure> kFlywheelError = Units.RPM.of(1); public static final Measure kAngleError = Units.Rotations.of(0.5 / 360); @@ -173,4 +175,4 @@ public static final class Bindings { public static final int kReverseIntakeButtonID = 8; } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d99b26c1..825cc8fc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -3,6 +3,7 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.SparkPIDController.ArbFFUnits; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; @@ -39,6 +40,7 @@ public class Shooter extends SubsystemBase { private MutableMeasure m_shieldPosition; private MutableMeasure> m_targetVelocity; private MutableMeasure m_shooterAngle; + private MutableMeasure m_targetAngle; public Shooter() { @@ -89,6 +91,8 @@ public Shooter() { // sets follower motor to run inversely to the leader m_angleMotorFollower.follow(m_angleMotorLeader, true); m_angleEncoder = m_angleMotorLeader.getAbsoluteEncoder(); + SmartDashboard.putNumber("absolute encoder pos", m_angleEncoder.getPosition()); + m_angleEncoder.setZeroOffset(0); //TODO figure out what angle is zero m_anglePIDController = m_angleMotorLeader.getPIDController(); m_anglePIDController.setP(RobotConfig.ShooterConfig.kAngleControlP); @@ -101,8 +105,9 @@ public Shooter() { RobotConfig.ShooterConfig.kAngleControlMaxOutput); m_anglePIDController.setIZone(ShooterConfig.kAngleControlIZone); - m_shooterAngle = MutableMeasure.zero(Units.Degrees); + m_shooterAngle = MutableMeasure.mutable(getCurrentAngle()); + m_targetAngle = MutableMeasure.zero(Units.Rotations); m_targetVelocity = MutableMeasure.zero(Units.MetersPerSecond); m_shooterSpeed = MutableMeasure.zero(Units.RPM); m_shieldPosition = MutableMeasure.zero(Units.Rotations); @@ -166,11 +171,22 @@ public void periodic() { } } - // sets the target angle the shooter should be at + // sets the target angle the shooter should be at, called only once public void setAngle(Measure targetAngle) { + m_targetAngle.mut_replace(targetAngle); m_anglePIDController.setReference(targetAngle.in(Units.Rotations), CANSparkBase.ControlType.kPosition); } + //called periodically + public void setFF(double ff) { + m_anglePIDController.setReference( + m_targetAngle.in(Units.Rotations), + CANSparkBase.ControlType.kPosition, + 0, + ff, + ArbFFUnits.kPercentOut); + } + public Measure getCurrentAngle() { return m_shooterAngle.mut_replace(m_angleEncoder.getPosition(), Units.Revolutions); } @@ -258,4 +274,4 @@ public boolean isAtFlywheelSetpoint(double setpoint) { return Math.abs(m_topFlywheelEncoder.getPosition() - setpoint) < ShooterConfig.kFlywheelError.magnitude(); } -} \ No newline at end of file +} From 2f8e07cfa3d7a99213568f11b8d7e591d4fe954e Mon Sep 17 00:00:00 2001 From: iris Date: Sat, 2 Mar 2024 17:54:50 -0800 Subject: [PATCH 15/51] integraiton --- .../robot/commands/shooter/ManualAdjust.java | 26 ++----------------- .../robot/commands/shooter/StowShooter.java | 12 --------- 2 files changed, 2 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/commands/shooter/ManualAdjust.java b/src/main/java/frc/robot/commands/shooter/ManualAdjust.java index bd262072..3d729969 100644 --- a/src/main/java/frc/robot/commands/shooter/ManualAdjust.java +++ b/src/main/java/frc/robot/commands/shooter/ManualAdjust.java @@ -1,44 +1,29 @@ package frc.robot.commands.shooter; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.Angle; import edu.wpi.first.units.Measure; -<<<<<<< HEAD -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.RobotConfig.*; -import frc.robot.constants.RobotConfig.AdjustType; -======= -import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConfig.*; ->>>>>>> b148e52d3bdbe5d24ad3569f4d855f3c25394258 import frc.robot.subsystems.shooter.Shooter; public class ManualAdjust extends Command { private final Shooter m_shooter; private final AdjustType m_type; private Measure desiredAngle; -<<<<<<< HEAD -======= private Timer timer; ->>>>>>> b148e52d3bdbe5d24ad3569f4d855f3c25394258 public ManualAdjust(Shooter shooter, AdjustType type) { m_shooter = shooter; m_type = type; -<<<<<<< HEAD -======= timer = new Timer(); ->>>>>>> b148e52d3bdbe5d24ad3569f4d855f3c25394258 addRequirements(m_shooter); } @Override public void initialize() { -<<<<<<< HEAD -======= timer.start(); ->>>>>>> b148e52d3bdbe5d24ad3569f4d855f3c25394258 switch (m_type) { case up: desiredAngle = m_shooter.getCurrentAngle().plus(ShooterConfig.kAdjustAmountDegrees); @@ -56,14 +41,8 @@ public void initialize() { } @Override -<<<<<<< HEAD - public boolean isFinished() { - return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()); - } -} -======= public void execute() { - m_shooter.setFF(Math.cos(m_shooter.getCurrentAngle().in(Units.Radians))*ShooterConfig.kAngleControlFF); + m_shooter.setFF(Math.cos(Units.rotationsToRadians(m_shooter.getCurrentAngle().magnitude()))*ShooterConfig.kAngleControlFF); } @Override @@ -71,4 +50,3 @@ public boolean isFinished() { return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()) || timer.get() > ShooterConfig.kAimTimeout; } } ->>>>>>> b148e52d3bdbe5d24ad3569f4d855f3c25394258 diff --git a/src/main/java/frc/robot/commands/shooter/StowShooter.java b/src/main/java/frc/robot/commands/shooter/StowShooter.java index c7f28500..ea7a35f8 100644 --- a/src/main/java/frc/robot/commands/shooter/StowShooter.java +++ b/src/main/java/frc/robot/commands/shooter/StowShooter.java @@ -16,13 +16,6 @@ public StowShooter(Shooter shooter) { } @Override -<<<<<<< HEAD - public void initialize() {} - - @Override - public void execute() { - m_shooter.setAngle(Units.Degrees.of(ShooterConfig.kShooterStowAngle)); -======= public void initialize() { m_shooter.setAngle(Units.Degrees.of(ShooterConfig.kShooterStowAngle)); } @@ -30,7 +23,6 @@ public void initialize() { @Override public void execute() { m_shooter.setFF(Math.cos(m_shooter.getCurrentAngle().in(Units.Radians))*ShooterConfig.kAngleControlFF); ->>>>>>> b148e52d3bdbe5d24ad3569f4d855f3c25394258 } @Override @@ -40,8 +32,4 @@ public void end(boolean interrupted) {} public boolean isFinished() { return m_shooter.isAtAngleSetpoint(ShooterConfig.kShooterStowAngle); } -<<<<<<< HEAD -} -======= } ->>>>>>> b148e52d3bdbe5d24ad3569f4d855f3c25394258 From 324d72fd185179fb2333b9cc63c253944d0116da Mon Sep 17 00:00:00 2001 From: Iris Date: Sun, 3 Mar 2024 08:34:46 -0800 Subject: [PATCH 16/51] shooter test code --- src/main/java/frc/robot/RobotContainer.java | 2 ++ src/main/java/frc/robot/subsystems/shooter/Shooter.java | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fa311ebb..a791d9ba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -67,6 +67,8 @@ public RobotContainer() { } private void configureBindings() { + new Trigger(() -> m_operatorController.getRawButton(11)) + .whileTrue(new RunCommand(() -> m_shooter.setBasic(), m_shooter)); // angle on 8-directional button m_autoAim = new POVButton(m_operatorController, 0); m_trapAim = new POVButton(m_operatorController, 90); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 825cc8fc..5d0b1f1c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -112,6 +112,8 @@ public Shooter() { m_shooterSpeed = MutableMeasure.zero(Units.RPM); m_shieldPosition = MutableMeasure.zero(Units.Rotations); + SmartDashboard.putNumber("angle pos", 0.1); + if (DriverStation.isTest()) { putAngleOnSmartDashboard(); } @@ -187,6 +189,10 @@ public void setFF(double ff) { ArbFFUnits.kPercentOut); } + public void setBasic() { + m_angleMotorLeader.set(SmartDashboard.getNumber("angle pos", 0.1)); + } + public Measure getCurrentAngle() { return m_shooterAngle.mut_replace(m_angleEncoder.getPosition(), Units.Revolutions); } From 04a010851e1bf6eeab4198a0a36264a4e7b7c2cf Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Sun, 3 Mar 2024 14:10:43 -0800 Subject: [PATCH 17/51] set break mode --- src/main/java/frc/robot/constants/RobotConstants.java | 8 ++++---- src/main/java/frc/robot/subsystems/climber/Climber.java | 5 +++++ 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 3c1a0d42..95a9aea9 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -11,13 +11,13 @@ public static final class ClimberConstants { public static final double kSetPointTolerance = 0.01; - public static final double kClimberP = 0; + public static final double kClimberP = 0.1; public static final double kClimberI = 0; public static final double kClimberD = 0; public static final double kClimberMotorRadius = 0.003175; - public static final double kClimberIZone = 0; + public static final double kClimberIZone = 0.001; public static final double kClimberFeedForward = 0; - public static final double kClimberMaxOutput = 0; - public static final double kClimberMinOutput = 0; + public static final double kClimberMaxOutput = 1; + public static final double kClimberMinOutput = -1; } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 1e60afa0..ebd932b4 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -4,6 +4,8 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.IdleMode; + import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.RobotConstants.ClimberConstants; @@ -20,6 +22,9 @@ public Climber() { followerController = new CANSparkMax(ClimberConstants.CLIMBER_CONTROLLER_ID2, MotorType.kBrushless); + leaderController.setIdleMode(IdleMode.kBrake); + followerController.setIdleMode(IdleMode.kBrake); + followerController.follow(leaderController); m_pidController = leaderController.getPIDController(); From 862f7f261af6411374326ad937482c11144fcbb6 Mon Sep 17 00:00:00 2001 From: iris Date: Sat, 9 Mar 2024 14:01:16 -0800 Subject: [PATCH 18/51] pivot works and tuned --- src/main/java/frc/robot/RobotContainer.java | 10 +++++-- .../frc/robot/commands/shooter/AngleTest.java | 30 +++++++++++++++++++ .../java/frc/robot/constants/RobotConfig.java | 4 +-- .../frc/robot/subsystems/shooter/Shooter.java | 12 ++++---- 4 files changed, 45 insertions(+), 11 deletions(-) create mode 100644 src/main/java/frc/robot/commands/shooter/AngleTest.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fa311ebb..66f30b42 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,6 +16,7 @@ import frc.robot.commands.intake.RunIntake; import frc.robot.commands.shooter.ActuateShield; import frc.robot.commands.shooter.Aim; +import frc.robot.commands.shooter.AngleTest; import frc.robot.commands.shooter.Shoot; import frc.robot.constants.RobotConfig; import frc.robot.constants.RobotConfig.FieldElement; @@ -62,11 +63,17 @@ public RobotContainer() { autoChooser = AutoBuilder.buildAutoChooser(); configureBindings(); + + autoChooser.setDefaultOption("Leave Top", AutoBuilder.buildAuto("LeaveFromTop")); + SmartDashboard.putData("Auto Chooser", autoChooser); + /*m_shooter.setDefaultCommand( new RunCommand(() -> m_shooter.runFlywheel(ShooterConfig.kDefaultFlywheelRPM), m_shooter));*/ } private void configureBindings() { + new Trigger(() -> m_operatorController.getRawButton(5)) + .whileTrue(new AngleTest(m_shooter)); // angle on 8-directional button m_autoAim = new POVButton(m_operatorController, 0); m_trapAim = new POVButton(m_operatorController, 90); @@ -111,9 +118,6 @@ private void configureBindings() { // extend shield new Trigger(() -> m_operatorController.getRawButton(Bindings.kRetractShield)) .onTrue(new ActuateShield(m_shooter, true)); - - autoChooser.setDefaultOption("Leave Top", AutoBuilder.buildAuto("LeaveFromTop")); - SmartDashboard.putData("Auto Chooser", autoChooser); } private void updateInput() { diff --git a/src/main/java/frc/robot/commands/shooter/AngleTest.java b/src/main/java/frc/robot/commands/shooter/AngleTest.java new file mode 100644 index 00000000..39d74a7f --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/AngleTest.java @@ -0,0 +1,30 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConfig.ShooterConfig; +import frc.robot.subsystems.shooter.Shooter; + +public class AngleTest extends Command { + private Shooter m_shooter; + + public AngleTest(Shooter shooter) { + m_shooter = shooter; + + addRequirements(m_shooter); + } + + @Override + public void initialize() { + m_shooter.setAngle(MutableMeasure.ofBaseUnits(160, Units.Rotations)); + } + + @Override + public void execute() { + double ff = Math.cos(m_shooter.getCurrentAngle().in(Units.Radians))*ShooterConfig.kAngleControlFF; + System.out.println("ff at: " + ff); + m_shooter.setFF(ff); + } + +} diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index ccdfc5bd..8ca8760d 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -28,10 +28,10 @@ public enum FieldElement { public static final class ShooterConfig { // Angle controller PID coefficients - public static final double kAngleControlP = 0.1; + public static final double kAngleControlP = 0.2; public static final double kAngleControlI = 0; public static final double kAngleControlD = 0; - public static final double kAngleControlFF = 0.001; + public static final double kAngleControlFF = 0.2; public static final double kAngleControlIZone = 0.0001; public static final double kAngleControlMinOutput = -1; public static final double kAngleControlMaxOutput = 1; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 825cc8fc..4767d134 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -90,23 +90,21 @@ public Shooter() { new CANSparkMax(ShooterConstants.kAngleMotorFollowerId, MotorType.kBrushless); // sets follower motor to run inversely to the leader m_angleMotorFollower.follow(m_angleMotorLeader, true); - m_angleEncoder = m_angleMotorLeader.getAbsoluteEncoder(); - SmartDashboard.putNumber("absolute encoder pos", m_angleEncoder.getPosition()); - m_angleEncoder.setZeroOffset(0); //TODO figure out what angle is zero + m_angleEncoder = m_angleMotorFollower.getAbsoluteEncoder(); + m_angleEncoder.setZeroOffset(28.6/360*160); m_anglePIDController = m_angleMotorLeader.getPIDController(); m_anglePIDController.setP(RobotConfig.ShooterConfig.kAngleControlP); m_anglePIDController.setI(RobotConfig.ShooterConfig.kAngleControlI); m_anglePIDController.setD(RobotConfig.ShooterConfig.kAngleControlD); - m_anglePIDController.setFF(RobotConfig.ShooterConfig.kAngleControlD); + m_anglePIDController.setFF(RobotConfig.ShooterConfig.kAngleControlFF); m_anglePIDController.setIZone(RobotConfig.ShooterConfig.kAngleControlIZone); m_anglePIDController.setOutputRange( RobotConfig.ShooterConfig.kAngleControlMinOutput, RobotConfig.ShooterConfig.kAngleControlMaxOutput); - m_anglePIDController.setIZone(ShooterConfig.kAngleControlIZone); - m_shooterAngle = MutableMeasure.mutable(getCurrentAngle()); + m_shooterAngle = MutableMeasure.zero(Units.Revolutions); m_targetAngle = MutableMeasure.zero(Units.Rotations); m_targetVelocity = MutableMeasure.zero(Units.MetersPerSecond); m_shooterSpeed = MutableMeasure.zero(Units.RPM); @@ -169,6 +167,8 @@ public void periodic() { if (m_topFlywheelEncoder.getVelocity() != flywheelRPM) { flywheelRPM = m_topFlywheelEncoder.getVelocity(); } + + SmartDashboard.putNumber("angle error", m_targetAngle.magnitude()-m_angleEncoder.getPosition()); } // sets the target angle the shooter should be at, called only once From 21ea345daafe9e45a0cdbfc50436bab913f6a12a Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Mon, 11 Mar 2024 19:55:39 -0700 Subject: [PATCH 19/51] adjusted shoot angle constants --- src/main/java/frc/robot/constants/RobotConfig.java | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 8ca8760d..d8b879b3 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -91,12 +91,13 @@ public static final class ShooterConfig { public static final double kAimTimeout = 2; public static final double kShieldDefaultSpeed = 0.5; + public static final double kEncoderRotsToPivotRot = 160; public static final Measure> kFlywheelError = Units.RPM.of(1); - public static final Measure kAngleError = Units.Rotations.of(0.5 / 360); - public static final Measure kSpeakerAngle = Units.Rotations.of(75 / 360); - public static final Measure kAmpAngle = Units.Rotations.of(109 / 360); - public static final Measure kTrapAngle = Units.Rotations.of(105 / 360); - public static final Measure kAdjustAmountDegrees = Units.Rotations.of(0.5 / 360); + public static final Measure kAngleError = Units.Rotations.of(0.5 / 360 * kEncoderRotsToPivotRot); + public static final Measure kSpeakerAngle = Units.Rotations.of(75 / 360 * kEncoderRotsToPivotRot); + public static final Measure kAmpAngle = Units.Rotations.of(109 / 360 * kEncoderRotsToPivotRot); + public static final Measure kTrapAngle = Units.Rotations.of(105 / 360 * kEncoderRotsToPivotRot); + public static final Measure kAdjustAmountDegrees = Units.Rotations.of(0.5 / 360 * kEncoderRotsToPivotRot); public static final double kDefaultAmpVelocity = 1500; // rpm public static final double kDefaultTrapVelocity = 2000; // rpm From 85c57fbff4b307c6b1ea8c12a045be0eb8a92995 Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Mon, 11 Mar 2024 20:08:10 -0700 Subject: [PATCH 20/51] climber testing ready --- .../frc/robot/commands/ClimberCommand.java | 27 +++++++------------ .../java/frc/robot/constants/RobotConfig.java | 2 +- .../frc/robot/constants/RobotConstants.java | 4 +-- .../frc/robot/subsystems/climber/Climber.java | 13 ++++++--- 4 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java index fcbee844..fa63032f 100644 --- a/src/main/java/frc/robot/commands/ClimberCommand.java +++ b/src/main/java/frc/robot/commands/ClimberCommand.java @@ -2,41 +2,34 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConfig.ClimberConfig; -import frc.robot.constants.RobotConstants; import frc.robot.subsystems.climber.Climber; +import frc.robot.constants.RobotConstants; public class ClimberCommand extends Command { - private final Climber m_subsystem; + private final Climber m_climber; + private final double m_setpoint; - /** - * Creates a new ClimberCommand. - * - * @param subsystem The subsystem used by this command. - */ - public ClimberCommand(Climber subsystem) { - m_subsystem = subsystem; + public ClimberCommand(Climber climber, double setpoint) { + m_climber = climber; + m_setpoint = setpoint; - addRequirements(subsystem); + addRequirements(climber); } - // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_subsystem.setSetpoint(ClimberConfig.setpoint); + m_climber.setSetpoint(m_setpoint); } - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_subsystem.setMotorSpeed(0); + m_climber.setMotorSpeed(ClimberConfig.kStallInput); } - // Returns true when the command should end. - // If absolute value of error is less than or equal to tolerance, returns true @Override public boolean isFinished() { double error = - Climber.metersToRotations(ClimberConfig.setpoint) - m_subsystem.getEncoderPosition(); + Climber.metersToRotations(m_setpoint) - m_climber.getEncoderPosition(); return Math.abs(error) <= RobotConstants.ClimberConstants.kSetPointTolerance; } } diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 955f0990..b6ef09a1 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -6,6 +6,6 @@ */ public class RobotConfig { public static final class ClimberConfig { - public static double setpoint = 0; + public static final double kStallInput = 0.02; } } diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 95a9aea9..ad6fd51a 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -6,8 +6,8 @@ */ public final class RobotConstants { public static final class ClimberConstants { - public static final int CLIMBER_CONTROLLER_ID1 = -1; - public static final int CLIMBER_CONTROLLER_ID2 = -1; + public static final int kClimberLeaderID = -1; + public static final int kClimberFollowerID = -1; public static final double kSetPointTolerance = 0.01; diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index ebd932b4..7746715a 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -6,6 +6,7 @@ import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkBase.IdleMode; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.RobotConstants.ClimberConstants; @@ -18,9 +19,9 @@ public class Climber extends SubsystemBase { public Climber() { leaderController = - new CANSparkMax(ClimberConstants.CLIMBER_CONTROLLER_ID1, MotorType.kBrushless); + new CANSparkMax(ClimberConstants.kClimberLeaderID, MotorType.kBrushless); followerController = - new CANSparkMax(ClimberConstants.CLIMBER_CONTROLLER_ID2, MotorType.kBrushless); + new CANSparkMax(ClimberConstants.kClimberFollowerID, MotorType.kBrushless); leaderController.setIdleMode(IdleMode.kBrake); followerController.setIdleMode(IdleMode.kBrake); @@ -29,6 +30,7 @@ public Climber() { m_pidController = leaderController.getPIDController(); m_encoder = leaderController.getEncoder(); + m_encoder.setPosition(0); // set PID coefficients m_pidController.setP(ClimberConstants.kClimberP); @@ -38,10 +40,14 @@ public Climber() { m_pidController.setFF(ClimberConstants.kClimberFeedForward); m_pidController.setOutputRange( ClimberConstants.kClimberMinOutput, ClimberConstants.kClimberMaxOutput); + + SmartDashboard.putNumber("climber encoder rots", m_encoder.getPosition()); } @Override - public void periodic() {} + public void periodic() { + SmartDashboard.putNumber("climber encoder rots", m_encoder.getPosition()); + } public double getEncoderPosition() { return m_encoder.getPosition(); @@ -55,6 +61,7 @@ public static double rotationsToMeters(double rotations) { return 2 * Math.PI * ClimberConstants.kClimberMotorRadius * rotations; } + //TODO determine climber rot conversion factor empirically public static double metersToRotations(double meters) { return meters / (2 * Math.PI * ClimberConstants.kClimberMotorRadius); } From 270245806d18dbd1633b2398799a11d656d49351 Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Tue, 12 Mar 2024 10:24:23 -0700 Subject: [PATCH 21/51] buddy climb --- src/main/java/frc/robot/RobotContainer.java | 5 ++ .../java/frc/robot/commands/BuddyClimb.java | 49 +++++++++++++ .../{ClimberCommand.java => Climb.java} | 14 ++-- .../java/frc/robot/constants/RobotConfig.java | 7 ++ .../frc/robot/constants/RobotConstants.java | 17 ++++- .../frc/robot/subsystems/climber/Climber.java | 73 ++++++++++++------- 6 files changed, 127 insertions(+), 38 deletions(-) create mode 100644 src/main/java/frc/robot/commands/BuddyClimb.java rename src/main/java/frc/robot/commands/{ClimberCommand.java => Climb.java} (55%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d09..d2fedab3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,11 +4,16 @@ package frc.robot; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.constants.RobotConstants.OIConstants; public class RobotContainer { + private Joystick m_operatorController; + public RobotContainer() { + m_operatorController = new Joystick(OIConstants.kOperatorJoystickPort); configureBindings(); } diff --git a/src/main/java/frc/robot/commands/BuddyClimb.java b/src/main/java/frc/robot/commands/BuddyClimb.java new file mode 100644 index 00000000..505d4030 --- /dev/null +++ b/src/main/java/frc/robot/commands/BuddyClimb.java @@ -0,0 +1,49 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConfig.ClimberConfig; +import frc.robot.subsystems.climber.Climber; + +public class BuddyClimb extends Command { + private final Climber m_climber; + private double m_setpoint; + private boolean m_leftClimb; + + public BuddyClimb(Climber climber, double setpoint, boolean leftClimb) { + m_climber = climber; + m_setpoint = setpoint; + m_leftClimb = leftClimb; + + addRequirements(climber); + } + + @Override + public void execute() { + double leftSetpoint, rightSetpoint = 0; + if (m_leftClimb) { + leftSetpoint = m_setpoint + ClimberConfig.buddyClimbExtensionDiff.magnitude(); + rightSetpoint = m_setpoint; + } else { + leftSetpoint = m_setpoint; + rightSetpoint = m_setpoint + ClimberConfig.buddyClimbExtensionDiff.magnitude(); + } + m_climber.setSetpoint(m_climber.getLeaderPidController(), leftSetpoint); + m_climber.setSetpoint(m_climber.getFollowerPidController(), rightSetpoint); + } + + @Override + public void end(boolean interrupted) { + m_climber.resetFollower(); + m_climber.setMotorSpeed(ClimberConfig.kStallInput); + } + + @Override + public boolean isFinished() { + double leaderError = + Climber.metersToRotations(m_setpoint) - m_climber.getLeaderEncoderPosition(); + double followerError = + Climber.metersToRotations(m_setpoint) - m_climber.getFollowerEncoderPosition(); + double accError = leaderError + followerError; + return Math.abs(accError) <= ClimberConfig.kSetPointTolerance; + } +} diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/Climb.java similarity index 55% rename from src/main/java/frc/robot/commands/ClimberCommand.java rename to src/main/java/frc/robot/commands/Climb.java index fa63032f..7424360b 100644 --- a/src/main/java/frc/robot/commands/ClimberCommand.java +++ b/src/main/java/frc/robot/commands/Climb.java @@ -3,13 +3,12 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConfig.ClimberConfig; import frc.robot.subsystems.climber.Climber; -import frc.robot.constants.RobotConstants; -public class ClimberCommand extends Command { +public class Climb extends Command { private final Climber m_climber; - private final double m_setpoint; + private double m_setpoint; - public ClimberCommand(Climber climber, double setpoint) { + public Climb(Climber climber, double setpoint) { m_climber = climber; m_setpoint = setpoint; @@ -18,7 +17,7 @@ public ClimberCommand(Climber climber, double setpoint) { @Override public void execute() { - m_climber.setSetpoint(m_setpoint); + m_climber.setSetpoint(m_climber.getLeaderPidController(), m_setpoint); } @Override @@ -28,8 +27,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - double error = - Climber.metersToRotations(m_setpoint) - m_climber.getEncoderPosition(); - return Math.abs(error) <= RobotConstants.ClimberConstants.kSetPointTolerance; + double error = Climber.metersToRotations(m_setpoint) - m_climber.getLeaderEncoderPosition(); + return Math.abs(error) <= ClimberConfig.kSetPointTolerance; } } diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index b6ef09a1..36b9dff7 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -1,11 +1,18 @@ package frc.robot.constants; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Units; + /** * Software config settings (e.g. max speed, PID values). For hardware constants @see * RobotConstants" */ public class RobotConfig { public static final class ClimberConfig { + public static final double kSetPointTolerance = 0.1; public static final double kStallInput = 0.02; + public static final Measure buddyClimbExtensionDiff = + Units.Meters.of(Units.Inches.of(5).in(Units.Meters)); } } diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index ad6fd51a..49f60f63 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -6,10 +6,8 @@ */ public final class RobotConstants { public static final class ClimberConstants { - public static final int kClimberLeaderID = -1; - public static final int kClimberFollowerID = -1; - - public static final double kSetPointTolerance = 0.01; + public static final int kClimberLeaderID = 10; + public static final int kClimberFollowerID = 11; public static final double kClimberP = 0.1; public static final double kClimberI = 0; @@ -20,4 +18,15 @@ public static final class ClimberConstants { public static final double kClimberMaxOutput = 1; public static final double kClimberMinOutput = -1; } + + public static final class OIConstants { + public static final int kDriverControllerPort = 0; + public static final int kOperatorJoystickPort = 1; + + public static final double kDriveDeadband = 0.06; + public static final double kMagnitudeDeadband = 0.06; + public static final double kDirectionSlewRate = 10; // radians per second + public static final double kMagnitudeSlewRate = 90; // percent per second (1 = 100%) + public static final double kRotationalSlewRate = 90; // percent per second (1 = 100%) + } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 7746715a..c7a16ec0 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,59 +1,68 @@ package frc.robot.subsystems.climber; +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkBase.IdleMode; - import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.RobotConstants.ClimberConstants; +import java.util.ArrayList; +import java.util.List; public class Climber extends SubsystemBase { private CANSparkMax leaderController; private CANSparkMax followerController; - private SparkPIDController m_pidController; - private RelativeEncoder m_encoder; + private SparkPIDController m_leaderPidController; + private SparkPIDController m_followerPidController; + private List m_pidControllers; public Climber() { - leaderController = - new CANSparkMax(ClimberConstants.kClimberLeaderID, MotorType.kBrushless); - followerController = - new CANSparkMax(ClimberConstants.kClimberFollowerID, MotorType.kBrushless); + leaderController = new CANSparkMax(ClimberConstants.kClimberLeaderID, MotorType.kBrushless); + followerController = new CANSparkMax(ClimberConstants.kClimberFollowerID, MotorType.kBrushless); leaderController.setIdleMode(IdleMode.kBrake); followerController.setIdleMode(IdleMode.kBrake); followerController.follow(leaderController); - m_pidController = leaderController.getPIDController(); - m_encoder = leaderController.getEncoder(); - m_encoder.setPosition(0); + m_leaderPidController = leaderController.getPIDController(); + m_followerPidController = followerController.getPIDController(); + m_pidControllers = new ArrayList<>(); + m_pidControllers.add(m_leaderPidController); + m_pidControllers.add(m_followerPidController); + leaderController.getEncoder().setPosition(0); // set PID coefficients - m_pidController.setP(ClimberConstants.kClimberP); - m_pidController.setI(ClimberConstants.kClimberI); - m_pidController.setD(ClimberConstants.kClimberP); - m_pidController.setIZone(ClimberConstants.kClimberIZone); - m_pidController.setFF(ClimberConstants.kClimberFeedForward); - m_pidController.setOutputRange( - ClimberConstants.kClimberMinOutput, ClimberConstants.kClimberMaxOutput); - - SmartDashboard.putNumber("climber encoder rots", m_encoder.getPosition()); + m_pidControllers.forEach( + (m_pidController) -> { + m_pidController.setP(ClimberConstants.kClimberP); + m_pidController.setI(ClimberConstants.kClimberI); + m_pidController.setD(ClimberConstants.kClimberP); + m_pidController.setIZone(ClimberConstants.kClimberIZone); + m_pidController.setFF(ClimberConstants.kClimberFeedForward); + m_pidController.setOutputRange( + ClimberConstants.kClimberMinOutput, ClimberConstants.kClimberMaxOutput); + }); + + SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); } @Override public void periodic() { - SmartDashboard.putNumber("climber encoder rots", m_encoder.getPosition()); + SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); } - public double getEncoderPosition() { - return m_encoder.getPosition(); + public double getLeaderEncoderPosition() { + return leaderController.getEncoder().getPosition(); } - public void setSetpoint(double setpoint) { + public double getFollowerEncoderPosition() { + return followerController.getEncoder().getPosition(); + } + + public void setSetpoint(SparkPIDController m_pidController, double setpoint) { m_pidController.setReference(metersToRotations(setpoint), CANSparkMax.ControlType.kPosition); } @@ -61,7 +70,7 @@ public static double rotationsToMeters(double rotations) { return 2 * Math.PI * ClimberConstants.kClimberMotorRadius * rotations; } - //TODO determine climber rot conversion factor empirically + // TODO determine climber rot conversion factor empirically public static double metersToRotations(double meters) { return meters / (2 * Math.PI * ClimberConstants.kClimberMotorRadius); } @@ -69,4 +78,16 @@ public static double metersToRotations(double meters) { public void setMotorSpeed(double speed) { leaderController.set(speed); } + + public void resetFollower() { + followerController.follow((leaderController)); + } + + public SparkPIDController getLeaderPidController() { + return m_leaderPidController; + } + + public SparkPIDController getFollowerPidController() { + return m_followerPidController; + } } From 05eafdeede3e555c6789a9f56b45f51428fd01d1 Mon Sep 17 00:00:00 2001 From: iris Date: Tue, 12 Mar 2024 18:30:44 -0700 Subject: [PATCH 22/51] removed pid --- src/main/java/frc/robot/RobotContainer.java | 11 +++- .../java/frc/robot/commands/BuddyClimb.java | 49 ---------------- src/main/java/frc/robot/commands/Climb.java | 11 ++-- .../java/frc/robot/constants/RobotConfig.java | 4 +- .../frc/robot/constants/RobotConstants.java | 4 +- .../frc/robot/subsystems/climber/Climber.java | 57 ++----------------- 6 files changed, 23 insertions(+), 113 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/BuddyClimb.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d2fedab3..a9130b5f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,17 +7,26 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.Climb; import frc.robot.constants.RobotConstants.OIConstants; +import frc.robot.subsystems.climber.Climber; public class RobotContainer { private Joystick m_operatorController; + private Climber m_climber; public RobotContainer() { m_operatorController = new Joystick(OIConstants.kOperatorJoystickPort); + m_climber = new Climber(); + configureBindings(); } - private void configureBindings() {} + private void configureBindings() { + new Trigger(() -> m_operatorController.getRawButton(1)) + .whileTrue(new Climb(m_climber)); + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/commands/BuddyClimb.java b/src/main/java/frc/robot/commands/BuddyClimb.java deleted file mode 100644 index 505d4030..00000000 --- a/src/main/java/frc/robot/commands/BuddyClimb.java +++ /dev/null @@ -1,49 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.RobotConfig.ClimberConfig; -import frc.robot.subsystems.climber.Climber; - -public class BuddyClimb extends Command { - private final Climber m_climber; - private double m_setpoint; - private boolean m_leftClimb; - - public BuddyClimb(Climber climber, double setpoint, boolean leftClimb) { - m_climber = climber; - m_setpoint = setpoint; - m_leftClimb = leftClimb; - - addRequirements(climber); - } - - @Override - public void execute() { - double leftSetpoint, rightSetpoint = 0; - if (m_leftClimb) { - leftSetpoint = m_setpoint + ClimberConfig.buddyClimbExtensionDiff.magnitude(); - rightSetpoint = m_setpoint; - } else { - leftSetpoint = m_setpoint; - rightSetpoint = m_setpoint + ClimberConfig.buddyClimbExtensionDiff.magnitude(); - } - m_climber.setSetpoint(m_climber.getLeaderPidController(), leftSetpoint); - m_climber.setSetpoint(m_climber.getFollowerPidController(), rightSetpoint); - } - - @Override - public void end(boolean interrupted) { - m_climber.resetFollower(); - m_climber.setMotorSpeed(ClimberConfig.kStallInput); - } - - @Override - public boolean isFinished() { - double leaderError = - Climber.metersToRotations(m_setpoint) - m_climber.getLeaderEncoderPosition(); - double followerError = - Climber.metersToRotations(m_setpoint) - m_climber.getFollowerEncoderPosition(); - double accError = leaderError + followerError; - return Math.abs(accError) <= ClimberConfig.kSetPointTolerance; - } -} diff --git a/src/main/java/frc/robot/commands/Climb.java b/src/main/java/frc/robot/commands/Climb.java index 7424360b..0eddc8ab 100644 --- a/src/main/java/frc/robot/commands/Climb.java +++ b/src/main/java/frc/robot/commands/Climb.java @@ -6,18 +6,16 @@ public class Climb extends Command { private final Climber m_climber; - private double m_setpoint; - public Climb(Climber climber, double setpoint) { + public Climb(Climber climber) { m_climber = climber; - m_setpoint = setpoint; addRequirements(climber); } @Override - public void execute() { - m_climber.setSetpoint(m_climber.getLeaderPidController(), m_setpoint); + public void initialize() { + m_climber.setMotorSpeed(ClimberConfig.kDefaultSpeed); } @Override @@ -27,7 +25,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - double error = Climber.metersToRotations(m_setpoint) - m_climber.getLeaderEncoderPosition(); - return Math.abs(error) <= ClimberConfig.kSetPointTolerance; + return m_climber.getLeaderEncoderPosition() > ClimberConfig.kUpperRotSoftStop - ClimberConfig.kStopMargin; } } diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 36b9dff7..55dcb442 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -10,8 +10,10 @@ */ public class RobotConfig { public static final class ClimberConfig { - public static final double kSetPointTolerance = 0.1; + public static final double kDefaultSpeed = 0.4; public static final double kStallInput = 0.02; + public static final double kUpperRotSoftStop = 200; + public static final double kStopMargin = 10; public static final Measure buddyClimbExtensionDiff = Units.Meters.of(Units.Inches.of(5).in(Units.Meters)); } diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 49f60f63..34f87220 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -6,8 +6,8 @@ */ public final class RobotConstants { public static final class ClimberConstants { - public static final int kClimberLeaderID = 10; - public static final int kClimberFollowerID = 11; + public static final int kClimberLeaderID = 11; + public static final int kClimberFollowerID = 12; public static final double kClimberP = 0.1; public static final double kClimberI = 0; diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index c7a16ec0..366b3df1 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -3,20 +3,15 @@ import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkPIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.RobotConfig.ClimberConfig; import frc.robot.constants.RobotConstants.ClimberConstants; -import java.util.ArrayList; -import java.util.List; public class Climber extends SubsystemBase { private CANSparkMax leaderController; private CANSparkMax followerController; - private SparkPIDController m_leaderPidController; - private SparkPIDController m_followerPidController; - private List m_pidControllers; public Climber() { leaderController = new CANSparkMax(ClimberConstants.kClimberLeaderID, MotorType.kBrushless); @@ -26,68 +21,24 @@ public Climber() { followerController.setIdleMode(IdleMode.kBrake); followerController.follow(leaderController); - - m_leaderPidController = leaderController.getPIDController(); - m_followerPidController = followerController.getPIDController(); - m_pidControllers = new ArrayList<>(); - m_pidControllers.add(m_leaderPidController); - m_pidControllers.add(m_followerPidController); leaderController.getEncoder().setPosition(0); - // set PID coefficients - m_pidControllers.forEach( - (m_pidController) -> { - m_pidController.setP(ClimberConstants.kClimberP); - m_pidController.setI(ClimberConstants.kClimberI); - m_pidController.setD(ClimberConstants.kClimberP); - m_pidController.setIZone(ClimberConstants.kClimberIZone); - m_pidController.setFF(ClimberConstants.kClimberFeedForward); - m_pidController.setOutputRange( - ClimberConstants.kClimberMinOutput, ClimberConstants.kClimberMaxOutput); - }); - SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); } @Override public void periodic() { SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); + if (leaderController.getEncoder().getPosition() < 0 || leaderController.getEncoder().getPosition() > ClimberConfig.kUpperRotSoftStop) { + leaderController.set(0); + } } public double getLeaderEncoderPosition() { return leaderController.getEncoder().getPosition(); } - public double getFollowerEncoderPosition() { - return followerController.getEncoder().getPosition(); - } - - public void setSetpoint(SparkPIDController m_pidController, double setpoint) { - m_pidController.setReference(metersToRotations(setpoint), CANSparkMax.ControlType.kPosition); - } - - public static double rotationsToMeters(double rotations) { - return 2 * Math.PI * ClimberConstants.kClimberMotorRadius * rotations; - } - - // TODO determine climber rot conversion factor empirically - public static double metersToRotations(double meters) { - return meters / (2 * Math.PI * ClimberConstants.kClimberMotorRadius); - } - public void setMotorSpeed(double speed) { leaderController.set(speed); } - - public void resetFollower() { - followerController.follow((leaderController)); - } - - public SparkPIDController getLeaderPidController() { - return m_leaderPidController; - } - - public SparkPIDController getFollowerPidController() { - return m_followerPidController; - } } From c95e93136bc7e5c5f0af653ba6b4c756ab980768 Mon Sep 17 00:00:00 2001 From: Iris Date: Tue, 12 Mar 2024 20:36:27 -0700 Subject: [PATCH 23/51] practice field testing changes 3/12 --- src/main/java/frc/robot/RobotContainer.java | 5 ++++- src/main/java/frc/robot/commands/Climb.java | 8 ++++++-- src/main/java/frc/robot/constants/RobotConfig.java | 2 +- src/main/java/frc/robot/subsystems/climber/Climber.java | 5 +++++ 4 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a9130b5f..81f62132 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,7 +25,10 @@ public RobotContainer() { private void configureBindings() { new Trigger(() -> m_operatorController.getRawButton(1)) - .whileTrue(new Climb(m_climber)); + .whileTrue(new Climb(m_climber, false)); + + new Trigger(() -> m_operatorController.getRawButton(2)) + .whileTrue(new Climb(m_climber, true)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Climb.java b/src/main/java/frc/robot/commands/Climb.java index 0eddc8ab..d99bf400 100644 --- a/src/main/java/frc/robot/commands/Climb.java +++ b/src/main/java/frc/robot/commands/Climb.java @@ -6,18 +6,22 @@ public class Climb extends Command { private final Climber m_climber; + private boolean m_reverse; - public Climb(Climber climber) { + public Climb(Climber climber, boolean reverse) { m_climber = climber; + m_reverse = reverse; addRequirements(climber); } @Override public void initialize() { - m_climber.setMotorSpeed(ClimberConfig.kDefaultSpeed); + int multiplier = m_reverse ? -1 : 1; + m_climber.setMotorSpeed(ClimberConfig.kDefaultSpeed*multiplier); } + @Override public void end(boolean interrupted) { m_climber.setMotorSpeed(ClimberConfig.kStallInput); diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 55dcb442..f539e39a 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -12,7 +12,7 @@ public class RobotConfig { public static final class ClimberConfig { public static final double kDefaultSpeed = 0.4; public static final double kStallInput = 0.02; - public static final double kUpperRotSoftStop = 200; + public static final double kUpperRotSoftStop = 5000; public static final double kStopMargin = 10; public static final Measure buddyClimbExtensionDiff = Units.Meters.of(Units.Inches.of(5).in(Units.Meters)); diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 366b3df1..0ed85359 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -21,11 +21,16 @@ public Climber() { followerController.setIdleMode(IdleMode.kBrake); followerController.follow(leaderController); + followerController.setInverted(true); leaderController.getEncoder().setPosition(0); + followerController.getEncoder().setPosition(0); SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); } + + + @Override public void periodic() { SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); From a103a4bb6b9b2025aaf18307c482fe61ee5e0753 Mon Sep 17 00:00:00 2001 From: iris Date: Tue, 12 Mar 2024 20:50:21 -0700 Subject: [PATCH 24/51] shooter practice field working 3/12 --- src/main/java/frc/robot/RobotContainer.java | 17 +++++++++++++++-- .../java/frc/robot/commands/shooter/Aim.java | 18 +++++++++--------- .../frc/robot/commands/shooter/AngleTest.java | 16 +++++++++++++--- .../java/frc/robot/constants/RobotConfig.java | 4 ++-- .../frc/robot/subsystems/shooter/Shooter.java | 5 +++++ 5 files changed, 44 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 66f30b42..a19bfd03 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import frc.robot.commands.shooter.Aim; import frc.robot.commands.shooter.AngleTest; import frc.robot.commands.shooter.Shoot; +import frc.robot.commands.shooter.StowShooter; import frc.robot.constants.RobotConfig; import frc.robot.constants.RobotConfig.FieldElement; import frc.robot.constants.RobotConstants.Bindings; @@ -72,8 +73,6 @@ public RobotContainer() { } private void configureBindings() { - new Trigger(() -> m_operatorController.getRawButton(5)) - .whileTrue(new AngleTest(m_shooter)); // angle on 8-directional button m_autoAim = new POVButton(m_operatorController, 0); m_trapAim = new POVButton(m_operatorController, 90); @@ -118,6 +117,20 @@ private void configureBindings() { // extend shield new Trigger(() -> m_operatorController.getRawButton(Bindings.kRetractShield)) .onTrue(new ActuateShield(m_shooter, true)); + + new Trigger(() -> m_operatorController.getRawButton(6)) + .whileTrue(new AngleTest(m_shooter, 1)); + + new Trigger(() -> m_operatorController.getRawButton(13)) + .whileTrue(new StowShooter(m_shooter)); + + //speaker + new Trigger(() -> m_operatorController.getRawButton(12)) + .whileTrue(new AngleTest(m_shooter, 0.3)); + + //amp + new Trigger(() -> m_operatorController.getRawButton(11)) + .whileTrue(new AngleTest(m_shooter, 0.6)); } private void updateInput() { diff --git a/src/main/java/frc/robot/commands/shooter/Aim.java b/src/main/java/frc/robot/commands/shooter/Aim.java index 0f45691d..a6991633 100644 --- a/src/main/java/frc/robot/commands/shooter/Aim.java +++ b/src/main/java/frc/robot/commands/shooter/Aim.java @@ -16,15 +16,14 @@ public class Aim extends Command { private final Shooter m_shooter; private final Vision m_vision; private final FieldElement m_type; - private Measure desiredAngle; private double desiredVelocity; - private Timer timer; + private double initTime; + private Measure desiredAngle; public Aim(Shooter shooter, FieldElement type) { m_shooter = shooter; m_vision = new Vision(); m_type = type; - timer = new Timer(); addRequirements(m_shooter); } @@ -38,7 +37,7 @@ public Aim(Shooter shooter, Vision eyes) { @Override public void initialize() { - timer.start(); + initTime = Timer.getFPGATimestamp(); if (m_type == null) { if (m_vision.getHasTarget()) { double desiredAngle = @@ -70,20 +69,21 @@ public void initialize() { desiredVelocity = 0; break; } - - m_shooter.setAngle(desiredAngle); m_shooter.runFlywheel(desiredVelocity); } } @Override public void execute() { - m_shooter.setFF(Math.cos(m_shooter.getCurrentAngle().in(Units.Radians))*ShooterConfig.kAngleControlFF); + System.out.println("shoot command"); + double ff = Math.cos(m_shooter.getCurrentAngle().in(Units.Radians))*ShooterConfig.kAngleControlFF; + m_shooter.setFF(ff); } + public boolean isFinished() { - return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()) && m_shooter.isAtFlywheelSetpoint(desiredVelocity) || - timer.get() > ShooterConfig.kAimTimeout; + return m_shooter.isAtFlywheelSetpoint(desiredVelocity) || + Math.abs(Timer.getFPGATimestamp() - initTime) > ShooterConfig.kAimTimeout; } public double getVelocity(double elementHeight) { diff --git a/src/main/java/frc/robot/commands/shooter/AngleTest.java b/src/main/java/frc/robot/commands/shooter/AngleTest.java index 39d74a7f..ad92c662 100644 --- a/src/main/java/frc/robot/commands/shooter/AngleTest.java +++ b/src/main/java/frc/robot/commands/shooter/AngleTest.java @@ -2,28 +2,38 @@ import edu.wpi.first.units.MutableMeasure; import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConfig.ShooterConfig; import frc.robot.subsystems.shooter.Shooter; public class AngleTest extends Command { private Shooter m_shooter; + private double m_multiplier; + private double startTime; - public AngleTest(Shooter shooter) { + public AngleTest(Shooter shooter, double multiplier) { m_shooter = shooter; + m_multiplier = multiplier; addRequirements(m_shooter); } @Override public void initialize() { - m_shooter.setAngle(MutableMeasure.ofBaseUnits(160, Units.Rotations)); + startTime = Timer.getFPGATimestamp(); + m_shooter.setAngle(MutableMeasure.ofBaseUnits(160*m_multiplier, Units.Rotations)); + } + + @Override + public boolean isFinished() { + return (Timer.getFPGATimestamp() - startTime) > 1; } @Override public void execute() { + System.out.println("angle movement cmd"); double ff = Math.cos(m_shooter.getCurrentAngle().in(Units.Radians))*ShooterConfig.kAngleControlFF; - System.out.println("ff at: " + ff); m_shooter.setFF(ff); } diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index d8b879b3..36a17ba8 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -101,7 +101,7 @@ public static final class ShooterConfig { public static final double kDefaultAmpVelocity = 1500; // rpm public static final double kDefaultTrapVelocity = 2000; // rpm - public static final double kDefaultSpeakerVelocity = 2500; // rpm + public static final double kDefaultSpeakerVelocity = 4000; // rpm } public static class DriveConfig { @@ -147,7 +147,7 @@ public static class TurnConfig { new ReplanningConfig()); // 4.45 m/s max speed - public static final double kMaxSpeedBase = 4.8; + public static final double kMaxSpeedBase = 6; public static final double kMaxSpeedScaleFactor = 0.9; public static final double kMaxSpeedMetersPerSecond = kMaxSpeedBase * kMaxSpeedScaleFactor; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 4767d134..b0c3b970 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -113,6 +113,9 @@ public Shooter() { if (DriverStation.isTest()) { putAngleOnSmartDashboard(); } + + SmartDashboard.putNumber("amp multiplier", 4/9); + SmartDashboard.putNumber("speaker multiplier", 2/9); } public void putAngleOnSmartDashboard() { @@ -140,6 +143,8 @@ public void putAngleOnSmartDashboard() { @Override public void periodic() { + SmartDashboard.putNumber("amp multiplier", 4/9); + SmartDashboard.putNumber("speaker multiplier", 2/9); SmartDashboard.putNumber("shield rots", m_shieldController.getEncoder().getPosition()); double pval = SmartDashboard.getNumber("flywheel p", 0.1); From 6a6f19fa26b71c1d0237a953be40513542f3c31d Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Tue, 12 Mar 2024 23:03:51 -0700 Subject: [PATCH 25/51] removed magic numbers --- src/main/java/frc/robot/RobotContainer.java | 36 +++++++---------- .../frc/robot/commands/shooter/AngleTest.java | 40 ------------------- .../robot/commands/shooter/ManualAdjust.java | 7 +++- .../frc/robot/commands/shooter/PivotMove.java | 40 +++++++++++++++++++ .../shooter/{Aim.java => SpinFlywheels.java} | 16 ++++---- .../robot/commands/shooter/StowShooter.java | 3 +- .../java/frc/robot/constants/RobotConfig.java | 15 ++++--- .../frc/robot/constants/RobotConstants.java | 8 ++-- .../frc/robot/subsystems/intake/Intake.java | 3 +- .../frc/robot/subsystems/shooter/Shooter.java | 37 ++++++++--------- src/main/java/frc/utils/Vector.java | 2 +- 11 files changed, 105 insertions(+), 102 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/shooter/AngleTest.java create mode 100644 src/main/java/frc/robot/commands/shooter/PivotMove.java rename src/main/java/frc/robot/commands/shooter/{Aim.java => SpinFlywheels.java} (85%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a19bfd03..1d01c5dc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,9 +15,9 @@ import frc.robot.commands.BasicDriveCommand; import frc.robot.commands.intake.RunIntake; import frc.robot.commands.shooter.ActuateShield; -import frc.robot.commands.shooter.Aim; -import frc.robot.commands.shooter.AngleTest; +import frc.robot.commands.shooter.PivotMove; import frc.robot.commands.shooter.Shoot; +import frc.robot.commands.shooter.SpinFlywheels; import frc.robot.commands.shooter.StowShooter; import frc.robot.constants.RobotConfig; import frc.robot.constants.RobotConfig.FieldElement; @@ -64,7 +64,6 @@ public RobotContainer() { autoChooser = AutoBuilder.buildAutoChooser(); configureBindings(); - autoChooser.setDefaultOption("Leave Top", AutoBuilder.buildAuto("LeaveFromTop")); SmartDashboard.putData("Auto Chooser", autoChooser); @@ -103,12 +102,12 @@ private void configureBindings() { .whileTrue(new Shoot(m_indexer, false)); new Trigger(() -> m_operatorController.getRawButton(Bindings.kShootReverse)) .whileTrue(new Shoot(m_indexer, true)); - new Trigger(() -> m_operatorController.getRawButton(Bindings.kAimAmp)) - .whileTrue(new Aim(m_shooter, FieldElement.AMP)); - new Trigger(() -> m_operatorController.getRawButton(Bindings.kAimSpeaker)) - .whileTrue(new Aim(m_shooter, FieldElement.SPEAKER)); + new Trigger(() -> m_operatorController.getRawButton(Bindings.kFlywheelAmp)) + .whileTrue(new SpinFlywheels(m_shooter, FieldElement.AMP)); + new Trigger(() -> m_operatorController.getRawButton(Bindings.kFlywheelSpeaker)) + .whileTrue(new SpinFlywheels(m_shooter, FieldElement.SPEAKER)); - m_trapAim.whileTrue(new Aim(m_shooter, FieldElement.TRAP)); + m_trapAim.whileTrue(new SpinFlywheels(m_shooter, FieldElement.TRAP)); // triggers for extending and retracting shield manually // don't extend shield @@ -118,19 +117,14 @@ private void configureBindings() { new Trigger(() -> m_operatorController.getRawButton(Bindings.kRetractShield)) .onTrue(new ActuateShield(m_shooter, true)); - new Trigger(() -> m_operatorController.getRawButton(6)) - .whileTrue(new AngleTest(m_shooter, 1)); - - new Trigger(() -> m_operatorController.getRawButton(13)) + new Trigger(() -> m_operatorController.getRawButton(Bindings.kStowShooter)) .whileTrue(new StowShooter(m_shooter)); - - //speaker - new Trigger(() -> m_operatorController.getRawButton(12)) - .whileTrue(new AngleTest(m_shooter, 0.3)); - - //amp - new Trigger(() -> m_operatorController.getRawButton(11)) - .whileTrue(new AngleTest(m_shooter, 0.6)); + + new Trigger(() -> m_operatorController.getRawButton(Bindings.kAimSpeaker)) + .whileTrue(new PivotMove(m_shooter, 0.3)); + + new Trigger(() -> m_operatorController.getRawButton(Bindings.kAimAmp)) + .whileTrue(new PivotMove(m_shooter, 0.55)); } private void updateInput() { @@ -185,4 +179,4 @@ public boolean triggerPressed() { public Command getAutonomousCommand() { return autoChooser.getSelected(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/shooter/AngleTest.java b/src/main/java/frc/robot/commands/shooter/AngleTest.java deleted file mode 100644 index ad92c662..00000000 --- a/src/main/java/frc/robot/commands/shooter/AngleTest.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.robot.commands.shooter; - -import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.Units; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.RobotConfig.ShooterConfig; -import frc.robot.subsystems.shooter.Shooter; - -public class AngleTest extends Command { - private Shooter m_shooter; - private double m_multiplier; - private double startTime; - - public AngleTest(Shooter shooter, double multiplier) { - m_shooter = shooter; - m_multiplier = multiplier; - - addRequirements(m_shooter); - } - - @Override - public void initialize() { - startTime = Timer.getFPGATimestamp(); - m_shooter.setAngle(MutableMeasure.ofBaseUnits(160*m_multiplier, Units.Rotations)); - } - - @Override - public boolean isFinished() { - return (Timer.getFPGATimestamp() - startTime) > 1; - } - - @Override - public void execute() { - System.out.println("angle movement cmd"); - double ff = Math.cos(m_shooter.getCurrentAngle().in(Units.Radians))*ShooterConfig.kAngleControlFF; - m_shooter.setFF(ff); - } - -} diff --git a/src/main/java/frc/robot/commands/shooter/ManualAdjust.java b/src/main/java/frc/robot/commands/shooter/ManualAdjust.java index 3d729969..2dba9bc8 100644 --- a/src/main/java/frc/robot/commands/shooter/ManualAdjust.java +++ b/src/main/java/frc/robot/commands/shooter/ManualAdjust.java @@ -42,11 +42,14 @@ public void initialize() { @Override public void execute() { - m_shooter.setFF(Math.cos(Units.rotationsToRadians(m_shooter.getCurrentAngle().magnitude()))*ShooterConfig.kAngleControlFF); + m_shooter.setFF( + Math.cos(Units.rotationsToRadians(m_shooter.getCurrentAngle().magnitude())) + * ShooterConfig.kAngleControlFF); } @Override public boolean isFinished() { - return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()) || timer.get() > ShooterConfig.kAimTimeout; + return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()) + || timer.get() > ShooterConfig.kAimTimeout; } } diff --git a/src/main/java/frc/robot/commands/shooter/PivotMove.java b/src/main/java/frc/robot/commands/shooter/PivotMove.java new file mode 100644 index 00000000..7fecf3af --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/PivotMove.java @@ -0,0 +1,40 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConfig.ShooterConfig; +import frc.robot.subsystems.shooter.Shooter; + +public class PivotMove extends Command { + private Shooter m_shooter; + private double m_multiplier; + private double startTime; + + public PivotMove(Shooter shooter, double multiplier) { + m_shooter = shooter; + m_multiplier = multiplier; + + addRequirements(m_shooter); + } + + @Override + public void initialize() { + startTime = Timer.getFPGATimestamp(); + m_shooter.setAngle(MutableMeasure.ofBaseUnits(160 * m_multiplier, Units.Rotations)); + } + + @Override + public boolean isFinished() { + return (Timer.getFPGATimestamp() - startTime) > 1; + } + + @Override + public void execute() { + System.out.println("angle movement cmd"); + double ff = + Math.cos(m_shooter.getCurrentAngle().in(Units.Radians)) * ShooterConfig.kAngleControlFF; + m_shooter.setFF(ff); + } +} diff --git a/src/main/java/frc/robot/commands/shooter/Aim.java b/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java similarity index 85% rename from src/main/java/frc/robot/commands/shooter/Aim.java rename to src/main/java/frc/robot/commands/shooter/SpinFlywheels.java index a6991633..9b534567 100644 --- a/src/main/java/frc/robot/commands/shooter/Aim.java +++ b/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java @@ -12,7 +12,7 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.vision.Vision; -public class Aim extends Command { +public class SpinFlywheels extends Command { private final Shooter m_shooter; private final Vision m_vision; private final FieldElement m_type; @@ -20,7 +20,7 @@ public class Aim extends Command { private double initTime; private Measure desiredAngle; - public Aim(Shooter shooter, FieldElement type) { + public SpinFlywheels(Shooter shooter, FieldElement type) { m_shooter = shooter; m_vision = new Vision(); m_type = type; @@ -28,7 +28,7 @@ public Aim(Shooter shooter, FieldElement type) { addRequirements(m_shooter); } - public Aim(Shooter shooter, Vision eyes) { + public SpinFlywheels(Shooter shooter, Vision eyes) { m_shooter = shooter; m_vision = eyes; m_type = null; @@ -75,15 +75,14 @@ public void initialize() { @Override public void execute() { - System.out.println("shoot command"); - double ff = Math.cos(m_shooter.getCurrentAngle().in(Units.Radians))*ShooterConfig.kAngleControlFF; + double ff = + Math.cos(m_shooter.getCurrentAngle().in(Units.Radians)) * ShooterConfig.kAngleControlFF; m_shooter.setFF(ff); } - public boolean isFinished() { - return m_shooter.isAtFlywheelSetpoint(desiredVelocity) || - Math.abs(Timer.getFPGATimestamp() - initTime) > ShooterConfig.kAimTimeout; + return m_shooter.isAtFlywheelSetpoint(desiredVelocity) + || Math.abs(Timer.getFPGATimestamp() - initTime) > ShooterConfig.kAimTimeout; } public double getVelocity(double elementHeight) { @@ -95,5 +94,4 @@ public double getVelocity(double elementHeight) { public void end(boolean interrupted) { m_shooter.stopFlywheel(); } - } diff --git a/src/main/java/frc/robot/commands/shooter/StowShooter.java b/src/main/java/frc/robot/commands/shooter/StowShooter.java index ea7a35f8..0a0bfb1e 100644 --- a/src/main/java/frc/robot/commands/shooter/StowShooter.java +++ b/src/main/java/frc/robot/commands/shooter/StowShooter.java @@ -22,7 +22,8 @@ public void initialize() { @Override public void execute() { - m_shooter.setFF(Math.cos(m_shooter.getCurrentAngle().in(Units.Radians))*ShooterConfig.kAngleControlFF); + m_shooter.setFF( + Math.cos(m_shooter.getCurrentAngle().in(Units.Radians)) * ShooterConfig.kAngleControlFF); } @Override diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 36a17ba8..af728f36 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -93,11 +93,16 @@ public static final class ShooterConfig { public static final double kShieldDefaultSpeed = 0.5; public static final double kEncoderRotsToPivotRot = 160; public static final Measure> kFlywheelError = Units.RPM.of(1); - public static final Measure kAngleError = Units.Rotations.of(0.5 / 360 * kEncoderRotsToPivotRot); - public static final Measure kSpeakerAngle = Units.Rotations.of(75 / 360 * kEncoderRotsToPivotRot); - public static final Measure kAmpAngle = Units.Rotations.of(109 / 360 * kEncoderRotsToPivotRot); - public static final Measure kTrapAngle = Units.Rotations.of(105 / 360 * kEncoderRotsToPivotRot); - public static final Measure kAdjustAmountDegrees = Units.Rotations.of(0.5 / 360 * kEncoderRotsToPivotRot); + public static final Measure kAngleError = + Units.Rotations.of(0.5 / 360 * kEncoderRotsToPivotRot); + public static final Measure kSpeakerAngle = + Units.Rotations.of(75 / 360 * kEncoderRotsToPivotRot); + public static final Measure kAmpAngle = + Units.Rotations.of(109 / 360 * kEncoderRotsToPivotRot); + public static final Measure kTrapAngle = + Units.Rotations.of(105 / 360 * kEncoderRotsToPivotRot); + public static final Measure kAdjustAmountDegrees = + Units.Rotations.of(0.5 / 360 * kEncoderRotsToPivotRot); public static final double kDefaultAmpVelocity = 1500; // rpm public static final double kDefaultTrapVelocity = 2000; // rpm diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 4898e796..a57bf0be 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -19,17 +19,19 @@ public final class RobotConstants { public final class Bindings { - public static final int kAimAmp = 4; - public static final int kAimSpeaker = 3; + public static final int kFlywheelAmp = 4; + public static final int kFlywheelSpeaker = 3; public static final int kShoot = 1; public static final int kShootReverse = 7; public static final int kAimTrap = 2; - public static final int kStowShooter = 14; public static final int kToggleFlywheel = 5; public static final int kRetractShield = 10; public static final int kExtendShield = 9; public static final int kManualAdjustDown = 18; public static final int kManualAdjustUp = 19; + public static final int kStowShooter = 13; + public static final int kAimAmp = 11; + public static final int kAimSpeaker = 12; } public static final class VisionConstants { diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index af0c6309..ec32536b 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -42,5 +42,4 @@ public void stop() { public void stopFeedNote() { m_intakeRollerMotor.stopMotor(); } - -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b0c3b970..618990d0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -3,10 +3,10 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.SparkPIDController.ArbFFUnits; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.ArbFFUnits; import edu.wpi.first.units.*; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -22,6 +22,7 @@ public class Shooter extends SubsystemBase { /** 1. create motor and pid controller objects */ private CANSparkMax m_angleMotorLeader; + private CANSparkMax m_angleMotorFollower; private SparkPIDController m_anglePIDController; private AbsoluteEncoder m_angleEncoder; @@ -82,16 +83,15 @@ public Shooter() { m_targetVelocity = MutableMeasure.zero(Units.MetersPerSecond); - - // Angle + // Angle m_angleMotorLeader = - new CANSparkMax(ShooterConstants.kAngleMotorLeaderId, MotorType.kBrushless); + new CANSparkMax(ShooterConstants.kAngleMotorLeaderId, MotorType.kBrushless); m_angleMotorFollower = new CANSparkMax(ShooterConstants.kAngleMotorFollowerId, MotorType.kBrushless); // sets follower motor to run inversely to the leader m_angleMotorFollower.follow(m_angleMotorLeader, true); m_angleEncoder = m_angleMotorFollower.getAbsoluteEncoder(); - m_angleEncoder.setZeroOffset(28.6/360*160); + m_angleEncoder.setZeroOffset(28.6 / 360 * 160); m_anglePIDController = m_angleMotorLeader.getPIDController(); m_anglePIDController.setP(RobotConfig.ShooterConfig.kAngleControlP); @@ -103,7 +103,6 @@ public Shooter() { RobotConfig.ShooterConfig.kAngleControlMinOutput, RobotConfig.ShooterConfig.kAngleControlMaxOutput); - m_shooterAngle = MutableMeasure.zero(Units.Revolutions); m_targetAngle = MutableMeasure.zero(Units.Rotations); m_targetVelocity = MutableMeasure.zero(Units.MetersPerSecond); @@ -114,8 +113,8 @@ public Shooter() { putAngleOnSmartDashboard(); } - SmartDashboard.putNumber("amp multiplier", 4/9); - SmartDashboard.putNumber("speaker multiplier", 2/9); + SmartDashboard.putNumber("amp multiplier", 4 / 9); + SmartDashboard.putNumber("speaker multiplier", 2 / 9); } public void putAngleOnSmartDashboard() { @@ -143,8 +142,8 @@ public void putAngleOnSmartDashboard() { @Override public void periodic() { - SmartDashboard.putNumber("amp multiplier", 4/9); - SmartDashboard.putNumber("speaker multiplier", 2/9); + SmartDashboard.putNumber("amp multiplier", 4 / 9); + SmartDashboard.putNumber("speaker multiplier", 2 / 9); SmartDashboard.putNumber("shield rots", m_shieldController.getEncoder().getPosition()); double pval = SmartDashboard.getNumber("flywheel p", 0.1); @@ -173,23 +172,25 @@ public void periodic() { flywheelRPM = m_topFlywheelEncoder.getVelocity(); } - SmartDashboard.putNumber("angle error", m_targetAngle.magnitude()-m_angleEncoder.getPosition()); + SmartDashboard.putNumber( + "angle error", m_targetAngle.magnitude() - m_angleEncoder.getPosition()); } // sets the target angle the shooter should be at, called only once public void setAngle(Measure targetAngle) { m_targetAngle.mut_replace(targetAngle); - m_anglePIDController.setReference(targetAngle.in(Units.Rotations), CANSparkBase.ControlType.kPosition); + m_anglePIDController.setReference( + targetAngle.in(Units.Rotations), CANSparkBase.ControlType.kPosition); } - //called periodically + // called periodically public void setFF(double ff) { m_anglePIDController.setReference( - m_targetAngle.in(Units.Rotations), - CANSparkBase.ControlType.kPosition, - 0, - ff, - ArbFFUnits.kPercentOut); + m_targetAngle.in(Units.Rotations), + CANSparkBase.ControlType.kPosition, + 0, + ff, + ArbFFUnits.kPercentOut); } public Measure getCurrentAngle() { diff --git a/src/main/java/frc/utils/Vector.java b/src/main/java/frc/utils/Vector.java index c1ed0d7a..96da268f 100644 --- a/src/main/java/frc/utils/Vector.java +++ b/src/main/java/frc/utils/Vector.java @@ -559,4 +559,4 @@ public boolean isWithinBounds(Vector boundsMin, Vector boundsMax) { return minX <= x() && x() <= maxX && minY <= y() && y() <= maxY; } -} \ No newline at end of file +} From 7178976980ad9f2b8c3c34ee7eace50f20c15fe6 Mon Sep 17 00:00:00 2001 From: Iris Date: Fri, 15 Mar 2024 08:05:01 -0700 Subject: [PATCH 26/51] pid for flywheels and removed shoot timeout --- src/main/java/frc/robot/commands/shooter/SpinFlywheels.java | 6 +----- src/main/java/frc/robot/constants/RobotConfig.java | 6 +++--- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 4 +--- 3 files changed, 5 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java b/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java index 9b534567..34733d27 100644 --- a/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java +++ b/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java @@ -5,7 +5,6 @@ import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; import edu.wpi.first.units.Velocity; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConfig.FieldElement; import frc.robot.constants.RobotConfig.ShooterConfig; @@ -17,7 +16,6 @@ public class SpinFlywheels extends Command { private final Vision m_vision; private final FieldElement m_type; private double desiredVelocity; - private double initTime; private Measure desiredAngle; public SpinFlywheels(Shooter shooter, FieldElement type) { @@ -37,7 +35,6 @@ public SpinFlywheels(Shooter shooter, Vision eyes) { @Override public void initialize() { - initTime = Timer.getFPGATimestamp(); if (m_type == null) { if (m_vision.getHasTarget()) { double desiredAngle = @@ -81,8 +78,7 @@ public void execute() { } public boolean isFinished() { - return m_shooter.isAtFlywheelSetpoint(desiredVelocity) - || Math.abs(Timer.getFPGATimestamp() - initTime) > ShooterConfig.kAimTimeout; + return m_shooter.isAtFlywheelSetpoint(desiredVelocity); } public double getVelocity(double elementHeight) { diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index af728f36..2c6d894b 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -39,7 +39,7 @@ public static final class ShooterConfig { // top Flywheel controller PID coefficients public static final double kTopFlywheelP = 0.2; public static final double kTopFlywheelI = 0; - public static final double kTopFlywheelD = 0.001; + public static final double kTopFlywheelD = 0.005; public static final double kTopFlywheelFF = 0; public static final double kTopFlywheelIZone = 0.0001; public static final double kTopFlywheelMinOutput = -1; @@ -88,7 +88,7 @@ public static final class ShooterConfig { public static final long kReleaseTime = 5000; public static final long kShieldTime = 2; // seconds - public static final double kAimTimeout = 2; + public static final double kAimTimeout = 20; public static final double kShieldDefaultSpeed = 0.5; public static final double kEncoderRotsToPivotRot = 160; @@ -106,7 +106,7 @@ public static final class ShooterConfig { public static final double kDefaultAmpVelocity = 1500; // rpm public static final double kDefaultTrapVelocity = 2000; // rpm - public static final double kDefaultSpeakerVelocity = 4000; // rpm + public static final double kDefaultSpeakerVelocity = 4500; // rpm } public static class DriveConfig { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index f9248502..a6fe1a6f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -144,8 +144,6 @@ public void putAngleOnSmartDashboard() { @Override public void periodic() { - SmartDashboard.putNumber("amp multiplier", 4 / 9); - SmartDashboard.putNumber("speaker multiplier", 2 / 9); SmartDashboard.putNumber("shield rots", m_shieldController.getEncoder().getPosition()); double pval = SmartDashboard.getNumber("flywheel p", 0.1); @@ -168,7 +166,7 @@ public void periodic() { "Shooter/bottom flywheel output", m_bottomFlywheelMotor.getAppliedOutput()); double flywheelRPM = SmartDashboard.getNumber("Shooter/Flywheel RPM", m_topFlywheelEncoder.getVelocity()); - SmartDashboard.putNumber("Shooter/Flywheel RPM", flywheelRPM); + SmartDashboard.putNumber("Shooter/Flywheel RPM", m_topFlywheelEncoder.getVelocity()); if (m_topFlywheelEncoder.getVelocity() != flywheelRPM) { flywheelRPM = m_topFlywheelEncoder.getVelocity(); From 2d952ba393ce834f4977176c15f2307e5a90cb19 Mon Sep 17 00:00:00 2001 From: TurtleMeds Date: Fri, 15 Mar 2024 21:21:18 -0700 Subject: [PATCH 27/51] used constant in followerController.setInverted() --- src/main/java/frc/robot/constants/RobotConfig.java | 1 + src/main/java/frc/robot/subsystems/climber/Climber.java | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index f539e39a..44f36fa6 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -14,6 +14,7 @@ public static final class ClimberConfig { public static final double kStallInput = 0.02; public static final double kUpperRotSoftStop = 5000; public static final double kStopMargin = 10; + public static final boolean kInverted = true; public static final Measure buddyClimbExtensionDiff = Units.Meters.of(Units.Inches.of(5).in(Units.Meters)); } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 0ed85359..e1ab0a59 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -21,7 +21,7 @@ public Climber() { followerController.setIdleMode(IdleMode.kBrake); followerController.follow(leaderController); - followerController.setInverted(true); + followerController.setInverted(ClimberConfig.kInverted); leaderController.getEncoder().setPosition(0); followerController.getEncoder().setPosition(0); From 6b1465ea26ab9b15782a8186f335772259de6c58 Mon Sep 17 00:00:00 2001 From: TurtleMeds Date: Fri, 15 Mar 2024 21:52:25 -0700 Subject: [PATCH 28/51] applied spotless --- src/main/java/frc/robot/RobotContainer.java | 6 ++---- src/main/java/frc/robot/commands/Climb.java | 6 +++--- src/main/java/frc/robot/subsystems/climber/Climber.java | 6 ++---- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 81f62132..59b9f997 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,11 +24,9 @@ public RobotContainer() { } private void configureBindings() { - new Trigger(() -> m_operatorController.getRawButton(1)) - .whileTrue(new Climb(m_climber, false)); + new Trigger(() -> m_operatorController.getRawButton(1)).whileTrue(new Climb(m_climber, false)); - new Trigger(() -> m_operatorController.getRawButton(2)) - .whileTrue(new Climb(m_climber, true)); + new Trigger(() -> m_operatorController.getRawButton(2)).whileTrue(new Climb(m_climber, true)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Climb.java b/src/main/java/frc/robot/commands/Climb.java index d99bf400..cda3346a 100644 --- a/src/main/java/frc/robot/commands/Climb.java +++ b/src/main/java/frc/robot/commands/Climb.java @@ -18,10 +18,9 @@ public Climb(Climber climber, boolean reverse) { @Override public void initialize() { int multiplier = m_reverse ? -1 : 1; - m_climber.setMotorSpeed(ClimberConfig.kDefaultSpeed*multiplier); + m_climber.setMotorSpeed(ClimberConfig.kDefaultSpeed * multiplier); } - @Override public void end(boolean interrupted) { m_climber.setMotorSpeed(ClimberConfig.kStallInput); @@ -29,6 +28,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return m_climber.getLeaderEncoderPosition() > ClimberConfig.kUpperRotSoftStop - ClimberConfig.kStopMargin; + return m_climber.getLeaderEncoderPosition() + > ClimberConfig.kUpperRotSoftStop - ClimberConfig.kStopMargin; } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index e1ab0a59..b5f5c57b 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -28,13 +28,11 @@ public Climber() { SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); } - - - @Override public void periodic() { SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); - if (leaderController.getEncoder().getPosition() < 0 || leaderController.getEncoder().getPosition() > ClimberConfig.kUpperRotSoftStop) { + if (leaderController.getEncoder().getPosition() < 0 + || leaderController.getEncoder().getPosition() > ClimberConfig.kUpperRotSoftStop) { leaderController.set(0); } } From e40eeea36188a0fe515a34035f249958e9041698 Mon Sep 17 00:00:00 2001 From: iris Date: Sat, 16 Mar 2024 13:57:23 -0700 Subject: [PATCH 29/51] idividual climbing --- src/main/java/frc/robot/RobotContainer.java | 17 ++++++-- .../frc/robot/commands/IndividualClimb.java | 37 +++++++++++++++++ .../frc/robot/subsystems/climber/Climber.java | 40 +++++++++++++++---- 3 files changed, 84 insertions(+), 10 deletions(-) create mode 100644 src/main/java/frc/robot/commands/IndividualClimb.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 59b9f997..4f5bdffb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,8 +7,10 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.Climb; +import frc.robot.commands.IndividualClimb; import frc.robot.constants.RobotConstants.OIConstants; import frc.robot.subsystems.climber.Climber; @@ -24,9 +26,18 @@ public RobotContainer() { } private void configureBindings() { - new Trigger(() -> m_operatorController.getRawButton(1)).whileTrue(new Climb(m_climber, false)); - - new Trigger(() -> m_operatorController.getRawButton(2)).whileTrue(new Climb(m_climber, true)); + //new Trigger(() -> m_operatorController.getRawButton(15)).whileTrue(new Climb(m_climber, false)); + + //new Trigger(() -> m_operatorController.getRawButton(16)).whileTrue(new Climb(m_climber, true)); + + new Trigger(() -> m_operatorController.getRawButton(11)) + .whileTrue(new IndividualClimb(m_climber, true, true)); + new Trigger(() -> m_operatorController.getRawButton(12)) + .whileTrue(new IndividualClimb(m_climber, true, false)); + new Trigger(() -> m_operatorController.getRawButton(13)) + .whileTrue(new IndividualClimb(m_climber, false, true)); + new Trigger(() -> m_operatorController.getRawButton(14)) + .whileTrue(new IndividualClimb(m_climber, false, false)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/IndividualClimb.java b/src/main/java/frc/robot/commands/IndividualClimb.java new file mode 100644 index 00000000..dc626706 --- /dev/null +++ b/src/main/java/frc/robot/commands/IndividualClimb.java @@ -0,0 +1,37 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.climber.Climber; + +public class IndividualClimb extends Command { + private Climber m_climber; + private boolean m_isLeader; + private boolean m_reverse; + + public IndividualClimb(Climber climber, boolean isLeader, boolean reverse) { + m_climber = climber; + m_isLeader = isLeader; + m_reverse = reverse; + + addRequirements(m_climber); + } + + @Override + public void initialize() { + if (m_isLeader) { + m_climber.setLeader(m_reverse); + } else { + m_climber.setFollower(m_reverse); + } + } + + @Override + public void end(boolean interrupted) { + if (m_isLeader) { + m_climber.stopLeader(); + } else { + m_climber.stopFollower(); + } + } + +} diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index b5f5c57b..b84f9dd8 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -3,9 +3,9 @@ import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.RobotConfig.ClimberConfig; import frc.robot.constants.RobotConstants.ClimberConstants; public class Climber extends SubsystemBase { @@ -20,8 +20,8 @@ public Climber() { leaderController.setIdleMode(IdleMode.kBrake); followerController.setIdleMode(IdleMode.kBrake); - followerController.follow(leaderController); - followerController.setInverted(ClimberConfig.kInverted); + //followerController.follow(leaderController); + //followerController.setInverted(ClimberConfig.kInverted); leaderController.getEncoder().setPosition(0); followerController.getEncoder().setPosition(0); @@ -30,11 +30,11 @@ public Climber() { @Override public void periodic() { - SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); - if (leaderController.getEncoder().getPosition() < 0 - || leaderController.getEncoder().getPosition() > ClimberConfig.kUpperRotSoftStop) { + /*SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); + if (leaderController.getEncoder().getPosition() < 0|| + leaderController.getEncoder().getPosition() > ClimberConfig.kUpperRotSoftStop) { leaderController.set(0); - } + }*/ } public double getLeaderEncoderPosition() { @@ -44,4 +44,30 @@ public double getLeaderEncoderPosition() { public void setMotorSpeed(double speed) { leaderController.set(speed); } + + public void setLeader(boolean reverse) { + int multiplier = reverse ? -1 : 1; + leaderController.set(0.3*multiplier); + } + + public void stopLeader() { + leaderController.set(0); + } + + public void stopFollower() { + followerController.set(0); + } + + public void setFollower(boolean reverse) { + int multiplier = reverse ? -1 : 1; + followerController.set(-0.3*multiplier); + } + + public CANSparkMax getLeader() { + return leaderController; + } + + public CANSparkMax getFollower() { + return followerController; + } } From af3a1c59f4359c97ad18882b0ca36cf851aa7bc7 Mon Sep 17 00:00:00 2001 From: iris Date: Sat, 16 Mar 2024 15:49:37 -0700 Subject: [PATCH 30/51] with soft stop --- src/main/java/frc/robot/RobotContainer.java | 54 +- .../frc/robot/commands/BasicDriveCommand.java | 50 ++ .../robot/commands/{ => climber}/Climb.java | 8 +- .../{ => climber}/IndividualClimb.java | 2 +- .../java/frc/robot/constants/RobotConfig.java | 69 +++ .../frc/robot/constants/RobotConstants.java | 110 ++++ .../frc/robot/subsystems/climber/Climber.java | 25 +- .../robot/subsystems/drive/Drivetrain.java | 427 ++++++++++++- .../subsystems/drive/MAXSwerveModule.java | 172 ++++++ src/main/java/frc/utils/SwerveUtils.java | 117 ++++ src/main/java/frc/utils/Vector.java | 562 ++++++++++++++++++ 11 files changed, 1572 insertions(+), 24 deletions(-) create mode 100644 src/main/java/frc/robot/commands/BasicDriveCommand.java rename src/main/java/frc/robot/commands/{ => climber}/Climb.java (77%) rename src/main/java/frc/robot/commands/{ => climber}/IndividualClimb.java (95%) create mode 100644 src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java create mode 100644 src/main/java/frc/utils/SwerveUtils.java create mode 100644 src/main/java/frc/utils/Vector.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4f5bdffb..26b008b2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,23 +4,53 @@ package frc.robot; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.commands.Climb; -import frc.robot.commands.IndividualClimb; +import frc.robot.commands.climber.Climb; +import frc.robot.commands.climber.IndividualClimb; import frc.robot.constants.RobotConstants.OIConstants; import frc.robot.subsystems.climber.Climber; +import frc.robot.subsystems.drive.Drivetrain; +import frc.utils.Vector; public class RobotContainer { private Joystick m_operatorController; + private XboxController m_driverController; + private Climber m_climber; + private Drivetrain m_robotDrive; + private Vector leftInputVec; + private Vector rightInputVec; public RobotContainer() { + m_driverController = new XboxController(OIConstants.kDriverControllerPort); m_operatorController = new Joystick(OIConstants.kOperatorJoystickPort); m_climber = new Climber(); + m_robotDrive = new Drivetrain(); + + leftInputVec = new Vector(); + rightInputVec = new Vector(); + + m_robotDrive.setDefaultCommand( + // The left stick controls translation of the robot. + // Turning is controlled by the X axis of the right stick. + new RunCommand( + () -> { + // update the values of leftInputVec and rightInputVec to the values of the controller + // I'm avoiding re-instantiting Vectors to save memory + updateInput(); + m_robotDrive.drive( + leftInputVec, + rightInputVec, + m_driverController.getRightBumper(), + m_driverController.getAButton()); + }, + m_robotDrive)); configureBindings(); } @@ -30,14 +60,34 @@ private void configureBindings() { //new Trigger(() -> m_operatorController.getRawButton(16)).whileTrue(new Climb(m_climber, true)); + //down new Trigger(() -> m_operatorController.getRawButton(11)) .whileTrue(new IndividualClimb(m_climber, true, true)); new Trigger(() -> m_operatorController.getRawButton(12)) .whileTrue(new IndividualClimb(m_climber, true, false)); + //down new Trigger(() -> m_operatorController.getRawButton(13)) .whileTrue(new IndividualClimb(m_climber, false, true)); new Trigger(() -> m_operatorController.getRawButton(14)) .whileTrue(new IndividualClimb(m_climber, false, false)); + + //up + new Trigger(() -> m_operatorController.getRawButton(15)) + .whileTrue(new Climb(m_climber, true)); + //down + new Trigger(() -> m_operatorController.getRawButton(16)) + .whileTrue(new Climb(m_climber, false)); + } + + private void updateInput() { + leftInputVec.setX( + MathUtil.applyDeadband(-m_driverController.getLeftY(), OIConstants.kDriveDeadband)); + leftInputVec.setY( + MathUtil.applyDeadband(-m_driverController.getLeftX(), OIConstants.kDriveDeadband)); + rightInputVec.setX( + MathUtil.applyDeadband(-m_driverController.getRightX(), OIConstants.kDriveDeadband)); + rightInputVec.setY( + MathUtil.applyDeadband(-m_driverController.getRightY(), OIConstants.kDriveDeadband)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/BasicDriveCommand.java b/src/main/java/frc/robot/commands/BasicDriveCommand.java new file mode 100644 index 00000000..60cf4501 --- /dev/null +++ b/src/main/java/frc/robot/commands/BasicDriveCommand.java @@ -0,0 +1,50 @@ +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConstants.OIConstants; +import frc.robot.subsystems.drive.Drivetrain; +import frc.utils.Vector; + +public class BasicDriveCommand extends Command { + private Drivetrain m_drive; + private XboxController m_controller; + private double m_multiplier; + + public BasicDriveCommand(Drivetrain drive, XboxController controller) { + this.m_drive = drive; + this.m_controller = controller; + + this.m_multiplier = 1; + + addRequirements(this.m_drive); + } + + @Override + public void execute() { + if (this.m_controller.getLeftTriggerAxis() != 0) { + this.m_multiplier = 2; + SmartDashboard.putBoolean("slow mode", false); + } else if (this.m_controller.getRightTriggerAxis() != 0) { + this.m_multiplier = 0.5; + SmartDashboard.putBoolean("slow mode", true); + } + + Vector lStickPos = + new Vector( + MathUtil.applyDeadband(-m_controller.getLeftY(), OIConstants.kDriveDeadband), + MathUtil.applyDeadband(-m_controller.getLeftX(), OIConstants.kDriveDeadband)); + Vector rStickPos = + new Vector( + MathUtil.applyDeadband(-m_controller.getRightX(), OIConstants.kDriveDeadband), + MathUtil.applyDeadband(-m_controller.getRightY(), OIConstants.kDriveDeadband)); + + m_drive.drive( + lStickPos.mult(m_multiplier), + rStickPos, + m_controller.getRightBumper(), + m_controller.getAButton()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Climb.java b/src/main/java/frc/robot/commands/climber/Climb.java similarity index 77% rename from src/main/java/frc/robot/commands/Climb.java rename to src/main/java/frc/robot/commands/climber/Climb.java index cda3346a..81b1d4a7 100644 --- a/src/main/java/frc/robot/commands/Climb.java +++ b/src/main/java/frc/robot/commands/climber/Climb.java @@ -1,4 +1,4 @@ -package frc.robot.commands; +package frc.robot.commands.climber; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConfig.ClimberConfig; @@ -17,13 +17,13 @@ public Climb(Climber climber, boolean reverse) { @Override public void initialize() { - int multiplier = m_reverse ? -1 : 1; - m_climber.setMotorSpeed(ClimberConfig.kDefaultSpeed * multiplier); + m_climber.setBoth(m_reverse); } @Override public void end(boolean interrupted) { - m_climber.setMotorSpeed(ClimberConfig.kStallInput); + m_climber.stopFollower(); + m_climber.stopLeader(); } @Override diff --git a/src/main/java/frc/robot/commands/IndividualClimb.java b/src/main/java/frc/robot/commands/climber/IndividualClimb.java similarity index 95% rename from src/main/java/frc/robot/commands/IndividualClimb.java rename to src/main/java/frc/robot/commands/climber/IndividualClimb.java index dc626706..e0b79082 100644 --- a/src/main/java/frc/robot/commands/IndividualClimb.java +++ b/src/main/java/frc/robot/commands/climber/IndividualClimb.java @@ -1,4 +1,4 @@ -package frc.robot.commands; +package frc.robot.commands.climber; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.climber.Climber; diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 44f36fa6..9940cef8 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -1,8 +1,14 @@ package frc.robot.constants; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; + import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; +import frc.robot.constants.RobotConstants.DriveConstants; +import frc.robot.constants.RobotConstants.SwerveModuleConstants; /** * Software config settings (e.g. max speed, PID values). For hardware constants @see @@ -18,4 +24,67 @@ public static final class ClimberConfig { public static final Measure buddyClimbExtensionDiff = Units.Meters.of(Units.Inches.of(5).in(Units.Meters)); } + + public static class DriveConfig { + public static class TranslateConfig { + public static final String kPKey = "Vision Translate P"; + public static final String kIKey = "Vision Translate I"; + public static final String kDKey = "Vision Translate D"; + public static final double kP = 0.0; + public static final double kI = 0.0; + public static final double kD = 0.0; + public static final double kTolerance = 1.0; + public static final double minIntegral = 0; + public static final double maxIntegral = 2; + } + + public static class TurnConfig { + public static final String kPKey = "Vision Turn P"; + public static final String kIKey = "Vision Turn I"; + public static final String kDKey = "Vision Turn D"; + public static final double kP = 0.0; + public static final double kI = 0.0; + public static final double kD = 0.0; + public static final double kTolerance = 1.0; + public static final double minIntegral = 0; + public static final double maxIntegral = 8; + } + + public static final String kSlewRateTranslationMagOutput = "translation magnitude output"; + public static final String kSlewRateTranslationDirRadOutput = "translation dir rad"; + + public static final HolonomicPathFollowerConfig kPathFollowerConfig = + new HolonomicPathFollowerConfig( + new PIDConstants( + SwerveModuleConstants.kDrivingP, + SwerveModuleConstants.kDrivingI, + SwerveModuleConstants.kDrivingD), + new PIDConstants( + SwerveModuleConstants.kTurningP, + SwerveModuleConstants.kTurningI, + SwerveModuleConstants.kTurningD), + SwerveModuleConstants.kMaxModuleSpeed, + DriveConstants.kWheelBaseRadius.in(Units.Meters), + new ReplanningConfig()); + + // 4.45 m/s max speed + public static final double kMaxSpeedBase = 4.8; + public static final double kMaxSpeedScaleFactor = 0.9; + public static final double kMaxSpeedMetersPerSecond = kMaxSpeedBase * kMaxSpeedScaleFactor; + + public static final double kMaxAngularSpeedBase = Math.PI; + public static final double kMaxAngularSpeedScaleFactor = 0.7; + public static final double kMaxAngularSpeed = + kMaxAngularSpeedBase * kMaxAngularSpeedScaleFactor; // radians per second + + public static final double kFrontLeftChassisAngularOffset = 0.0; + public static final double kFrontRightChassisAngularOffset = 0.0; + public static final double kBackLeftChassisAngularOffset = 0.0; + public static final double kBackRightChassisAngularOffset = 0.0; + // scaling factor for the alternative turning mode + public static final int altTurnSmoothing = 20; + public static final double HIGH_DIRECTION_SLEW_RATE = 500; + public static final double MIN_ANGLE_SLEW_RATE = 0.45 * Math.PI; + public static final double MAX_ANGLE_SLEW_RATE = 0.85 * Math.PI; + } } diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 34f87220..2335bbc3 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -1,5 +1,14 @@ package frc.robot.constants; +import com.revrobotics.CANSparkBase.IdleMode; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.Velocity; + /** * Software/hardware constants (e.g. CAN IDs, gear ratios, field measurements, etc.). For software * configs @see RobotConfig @@ -19,6 +28,107 @@ public static final class ClimberConstants { public static final double kClimberMinOutput = -1; } + public static final class NeoMotorConstants { + public static final double kFreeSpeedRpm = 5676; + } + + public static final class DriveConstants { + public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2; + public static final double kFrontRightChassisAngularOffset = 0; + public static final double kBackLeftChassisAngularOffset = Math.PI; + public static final double kBackRightChassisAngularOffset = Math.PI / 2; + + public static final double kDriveDeadband = 0.06; + + public static final int kFrontLeftDrivingCanId = 2; + public static final int kFrontLeftTurningCanId = 1; + + public static final int kFrontRightDrivingCanId = 6; + public static final int kFrontRightTurningCanId = 5; + + public static final int kRearLeftDrivingCanId = 4; + public static final int kRearLeftTurningCanId = 3; + + public static final int kRearRightDrivingCanId = 8; + public static final int kRearRightTurningCanId = 7; + + public static final int kGyroId = 9; + + // Chassis configuration + public static final Measure kTrackWidth = Units.Inches.of(22.5); + + // Distance between centers of right and left wheels on robot + public static final Measure kWheelBase = Units.Inches.of(22.5); + + public static final Measure kWheelBaseRadius = Units.Meters.of(0.404); + + // Distance between front and back wheels on robot + public static final SwerveDriveKinematics kDriveKinematics = + new SwerveDriveKinematics( + new Translation2d(kWheelBase.in(Units.Meters) / 2, kTrackWidth.in(Units.Meters) / 2), + new Translation2d(kWheelBase.in(Units.Meters) / 2, -kTrackWidth.in(Units.Meters) / 2), + new Translation2d(-kWheelBase.in(Units.Meters) / 2, kTrackWidth.in(Units.Meters) / 2), + new Translation2d(-kWheelBase.in(Units.Meters) / 2, -kTrackWidth.in(Units.Meters) / 2)); + } + + + public static final class SwerveModuleConstants { + // The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T. + // This changes the drive speed of the module (a pinion gear with more teeth will result in a + // robot that drives faster). + public static final int kDrivingMotorPinionTeeth = 14; + + public static final double kMaxModuleSpeed = 1; + + // Invert the turning encoder, since the output shaft rotates in the opposite direction of + // the steering motor in the MAXSwerve Module. + public static final boolean kTurningEncoderInverted = true; + + // Calculations required for driving motor conversion factors and feed forward + public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60; + public static final double kWheelDiameterMeters = 0.0762; + public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; + // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the + // bevel pinion + public static final double kDrivingMotorReduction = + (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); + public static final double kDriveWheelFreeSpeedRps = + (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) / kDrivingMotorReduction; + + public static final double kDrivingEncoderPositionFactor = + (kWheelDiameterMeters * Math.PI) / kDrivingMotorReduction; // meters + public static final double kDrivingEncoderVelocityFactor = + ((kWheelDiameterMeters * Math.PI) / kDrivingMotorReduction) / 60.0; // meters per second + + public static final double kTurningEncoderPositionFactor = (2 * Math.PI); // radians + public static final double kTurningEncoderVelocityFactor = + (2 * Math.PI) / 60.0; // radians per second + + public static final double kTurningEncoderPositionPIDMinInput = 0; // radians + public static final double kTurningEncoderPositionPIDMaxInput = + kTurningEncoderPositionFactor; // radians + + public static final double kDrivingP = 0.04; + public static final double kDrivingI = 0; + public static final double kDrivingD = 0; + public static final double kDrivingFF = 1 / kDriveWheelFreeSpeedRps; + public static final double kDrivingMinOutput = -1; + public static final double kDrivingMaxOutput = 1; + + public static final double kTurningP = 1; + public static final double kTurningI = 0; + public static final double kTurningD = 0; + public static final double kTurningFF = 0; + public static final double kTurningMinOutput = -1; + public static final double kTurningMaxOutput = 1; + + public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake; + public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake; + + public static final int kDrivingMotorCurrentLimit = 50; // amps + public static final int kTurningMotorCurrentLimit = 20; // amps + } + public static final class OIConstants { public static final int kDriverControllerPort = 0; public static final int kOperatorJoystickPort = 1; diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index b84f9dd8..c80be672 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -6,12 +6,14 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.RobotConfig.ClimberConfig; import frc.robot.constants.RobotConstants.ClimberConstants; public class Climber extends SubsystemBase { private CANSparkMax leaderController; private CANSparkMax followerController; + private int multiplier; public Climber() { leaderController = new CANSparkMax(ClimberConstants.kClimberLeaderID, MotorType.kBrushless); @@ -20,8 +22,8 @@ public Climber() { leaderController.setIdleMode(IdleMode.kBrake); followerController.setIdleMode(IdleMode.kBrake); - //followerController.follow(leaderController); - //followerController.setInverted(ClimberConfig.kInverted); + multiplier = 1; + leaderController.getEncoder().setPosition(0); followerController.getEncoder().setPosition(0); @@ -30,24 +32,27 @@ public Climber() { @Override public void periodic() { - /*SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); + SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); if (leaderController.getEncoder().getPosition() < 0|| leaderController.getEncoder().getPosition() > ClimberConfig.kUpperRotSoftStop) { leaderController.set(0); - }*/ + followerController.set(0); + } } public double getLeaderEncoderPosition() { return leaderController.getEncoder().getPosition(); } - public void setMotorSpeed(double speed) { - leaderController.set(speed); + public void setBoth(boolean reverse) { + multiplier = reverse ? -1 : 1; + leaderController.set(-0.5 * multiplier); + followerController.set(0.5 * multiplier); } public void setLeader(boolean reverse) { - int multiplier = reverse ? -1 : 1; - leaderController.set(0.3*multiplier); + multiplier = reverse ? -1 : 1; + leaderController.set(0.7*multiplier); } public void stopLeader() { @@ -59,8 +64,8 @@ public void stopFollower() { } public void setFollower(boolean reverse) { - int multiplier = reverse ? -1 : 1; - followerController.set(-0.3*multiplier); + multiplier = reverse ? -1 : 1; + followerController.set(-0.7*multiplier); } public CANSparkMax getLeader() { diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index 82fb8d44..188b71fc 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -1,18 +1,431 @@ package frc.robot.subsystems.drive; +import com.ctre.phoenix6.hardware.Pigeon2; +import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.*; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.RobotConfig; +import frc.robot.constants.RobotConfig.DriveConfig; +import frc.robot.constants.RobotConstants.DriveConstants; +import frc.robot.constants.RobotConstants.OIConstants; +import frc.utils.SwerveUtils; +import frc.utils.Vector; +/** an object representing the Drivetrain of a swerve drive frc robot */ public class Drivetrain extends SubsystemBase { - /** Creates a new ExampleSubsystem. */ - public Drivetrain() {} + // Create MAXSwerveModules + private final MAXSwerveModule m_frontLeft; + private final MAXSwerveModule m_frontRight; + private final MAXSwerveModule m_rearLeft; + private final MAXSwerveModule m_rearRight; + private Pigeon2 m_gyro; + + private final PowerDistribution m_powerDistribution; + + private double m_prevAngleRadians; + private double m_rightAngGoalRadians; + private double m_turnDirRadians; + + // Slew rate filter variables for controlling lateral acceleration + private double m_currentRotationRadians; + private double m_currentTranslationDirRadians; + private double m_currentTranslationMag; + + private double m_headingOffsetRadians; + + private SlewRateLimiter m_magLimiter; + private SlewRateLimiter m_rotLimiter; + private Vector spdCommanded; + + private Timer m_timer; + private double m_prevSlewRateTime; + + private MutableMeasure m_heading; + + // Odometry class for tracking robot pose + SwerveDriveOdometry m_odometry; + private Pose2d m_pose; + private ChassisSpeeds m_relativeSpeeds; + + private SwerveModulePosition[] m_swerveModulePositions; + + /** constructs a new Drivetrain object */ + public Drivetrain() { + m_frontLeft = + new MAXSwerveModule( + DriveConstants.kFrontLeftDrivingCanId, + DriveConstants.kFrontLeftTurningCanId, + DriveConstants.kFrontLeftChassisAngularOffset); + + m_frontRight = + new MAXSwerveModule( + DriveConstants.kFrontRightDrivingCanId, + DriveConstants.kFrontRightTurningCanId, + DriveConstants.kFrontRightChassisAngularOffset); + + m_rearLeft = + new MAXSwerveModule( + DriveConstants.kRearLeftDrivingCanId, + DriveConstants.kRearLeftTurningCanId, + DriveConstants.kBackLeftChassisAngularOffset); + + m_rearRight = + new MAXSwerveModule( + DriveConstants.kRearRightDrivingCanId, + DriveConstants.kRearRightTurningCanId, + DriveConstants.kBackRightChassisAngularOffset); + + // TODO: initialize this to where we place the robot on the field, will get from auto chosen + // from Smart Dashboard + m_pose = new Pose2d(); + + m_swerveModulePositions = + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition() + }; + + m_gyro = new Pigeon2(DriveConstants.kGyroId); + m_gyro.reset(); + + m_heading = MutableMeasure.ofBaseUnits(m_gyro.getAngle(), Units.Degrees); + + m_timer = new Timer(); + + m_powerDistribution = new PowerDistribution(); + + m_magLimiter = new SlewRateLimiter(OIConstants.kMagnitudeSlewRate); + m_rotLimiter = new SlewRateLimiter(OIConstants.kRotationalSlewRate); + spdCommanded = new Vector(); + + m_timer.start(); + m_prevSlewRateTime = m_timer.get(); + + m_odometry = + new SwerveDriveOdometry( + DriveConstants.kDriveKinematics, + Rotation2d.fromRadians(-getGyroAngle().in(Units.Radians)), + m_swerveModulePositions, + m_pose); + + configureAutoBuilder(); + + m_powerDistribution.clearStickyFaults(); + SmartDashboard.putNumber("driveVelocity", 0); + } + + /** configures the pathplanner AutoBuilder */ + private void configureAutoBuilder() { + AutoBuilder.configureHolonomic( + this::getPose, + this::resetOdometry, + this::getSpeeds, + this::driveChassisSpeeds, + RobotConfig.DriveConfig.kPathFollowerConfig, + this::allianceCheck, + this); + } + + /** + * returns the current speed of the drivetrain + * + * @return the current speed of the drivetrain + */ + public ChassisSpeeds getSpeeds() { + return m_relativeSpeeds; + } + + /** stops the drivetrain's movement */ + public void stop() { + move(Vector.Origin, 0); + } + + /** runs the periodic functionality of the drivetrain */ @Override public void periodic() { - // This method will be called once per scheduler run + m_odometry.update(m_gyro.getRotation2d(), m_swerveModulePositions); + double ang = getGyroAngle().in(Units.Radians); + SmartDashboard.putNumber("delta heading", ang - m_prevAngleRadians); + + m_prevAngleRadians = ang; + m_relativeSpeeds = getRobotRelativeSpeeds(); + m_pose = m_odometry.getPoseMeters(); + + SmartDashboard.putNumber("heading", ang - m_headingOffsetRadians); + + SmartDashboard.putNumber("right stick angle", m_rightAngGoalRadians); + SmartDashboard.putNumber("turn direction", m_turnDirRadians); } - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation + /** + * Resets the pose estimator to the specified pose. + * + * @param pose The pose to which to set the estimator. + */ + public void resetOdometry(Pose2d pose) { + m_odometry.resetPosition( + Rotation2d.fromRadians(-getGyroAngle().in(Units.Radians)), + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition(), + }, + pose); + } + + /** + * drives the drivatrain using the given inputs the magnitude of the joystick components shouldn't + * be > 1 (x^2 + y^2 <= 1) + * + * @param xSpeed the x-pos of the left joystick (-1, 1) + * @param ySpeed the y-pos of the left joystick (-1, 1) + * @param xRot the x-pos of the right joystick (-1, 1) + * @param yRot the x-pos of the right joystick (-1, 1) + * @param altDrive whether or not to use the alternative turning mode + * @param centerGyro whether or not to reset the gyro position to the current rotation + */ + public void drive(Vector spdVec, Vector rotVec, boolean altDrive, boolean centerGyro) { + if (centerGyro) zeroHeading(); + if (altDrive) { + altDrive(spdVec, rotVec); + } else { + mainDrive(spdVec, rotVec.x()); + } + } + + /** + * moves the divetrain based on the given ChassisSpeeds + * + * @param spds the target speeds of the drivetrain chassis + */ + public void driveChassisSpeeds(ChassisSpeeds spds) { + Vector spd = new Vector(spds.vxMetersPerSecond, spds.vyMetersPerSecond); + spdCommanded = spd; + double angVel = spds.omegaRadiansPerSecond; + move(spd, angVel); + } + + /** + * moves the drivetrain using the main turning mode + * + * @param xSpeed the proportion of the robot's max velocity to move in the x direction + * @param ySpeed the proportion of the robot's max velocity to move in the y direction + * @param xRot the speed to rotate with (-1, 1) + */ + public void mainDrive(Vector spdVec, double xRot) { + double rot = xRot * DriveConfig.kMaxAngularSpeed; + move(spdVec, rot); + } + + /** + * gets the value of the robot's gyro as a Measure + * + * @see Measure + * @return the angle of the robot gyro + */ + public Measure getGyroAngle() { + return m_heading.mut_replace(m_gyro.getAngle(), Units.Degrees); + } + + /** + * moves the drivetrain using the alternative turning mode + * + * @param xSpeed the proportion of the robot's max velocity to move in the x direction + * @param ySpeed the proportion of the robot's max velocity to move in the y direction + * @param xRot the x component of the direction vector to point towards + * @param yRot the y component of the direction vector to point towards + */ + public void altDrive(Vector spdVec, Vector rotVec) { + double rot = 0; + m_rightAngGoalRadians = rotVec.angle(); + if (rotVec.squaredMag() > 0) { + double stickAng = m_rightAngGoalRadians; + // gets the difference in angle, then uses mod to make sure its from -PI rad to PI rad + rot = altTurnSmooth(stickAng); + } + m_turnDirRadians = rot; + move(spdVec, rot); + } + + /** + * returns the current speed of the robot from it's reference frame + * + * @return the current speed of the robot from it's reference frame + */ + public ChassisSpeeds getRobotRelativeSpeeds() { + return DriveConstants.kDriveKinematics.toChassisSpeeds( + new SwerveModuleState[] { + m_frontLeft.getState(), + m_frontRight.getState(), + m_rearLeft.getState(), + m_rearRight.getState() + }); + } + + /** + * applies smoothing to the turning input of altDrive + * + * @param stickAng the given angle of the driver turning stick + * @return the commanded rotation based on the rotation input + */ + private double altTurnSmooth(double stickAng) { + return Math.tanh( + ((getGyroAngle().in(Units.Radians) + stickAng + Math.PI) % (2 * Math.PI) - Math.PI) + / DriveConfig.altTurnSmoothing) + * DriveConfig.kMaxAngularSpeed; + } + + /** + * returns the current position of the robot on the field + * + * @return the current position of the robot on the field + */ + private Pose2d getPose() { + Pose2d pose = m_odometry.getPoseMeters(); + return pose; + } + + /** + * moves the drivetrain using the given values + * + * @param xSpeed the proportion of the robot's max velocity to move in the x direction + * @param ySpeed the proportion of the robot's max velocity to move in the y direction + * @param rot the angular velocity to rotate the drivetrain in radians/s + */ + public void move(Vector spdVec, double rot) { + move(spdVec, rot, true); + } + + /** + * moves the drivetrain using the given values + * + * @param xSpeed the proportion of the robot's max velocity to move in the x direction + * @param ySpeed the proportion of the robot's max velocity to move in the y direction + * @param rot the angular velocity to rotate the drivetrain in radians/s + * @param rateLimit whether or not to use slew rate limiting + */ + private void move(Vector spdVec, double rot, boolean rateLimit) { + m_currentRotationRadians = rot; + + spdCommanded.setX(spdVec.x()); + spdCommanded.setY(spdVec.y()); + + if (rateLimit) { + limitDirectionSlewRate(spdCommanded); + m_currentRotationRadians = m_rotLimiter.calculate(rot); + SmartDashboard.putNumber(DriveConfig.kSlewRateTranslationMagOutput, spdCommanded.mag()); + SmartDashboard.putNumber(DriveConfig.kSlewRateTranslationDirRadOutput, spdCommanded.angle()); + } + + // Adjust input based on max speed + spdCommanded.mult(DriveConfig.kMaxSpeedMetersPerSecond); + + double rotDelivered = m_currentRotationRadians * DriveConfig.kMaxAngularSpeed; + + var swerveModuleStates = + DriveConstants.kDriveKinematics.toSwerveModuleStates( + ChassisSpeeds.fromFieldRelativeSpeeds( + spdCommanded.x(), + spdCommanded.y(), + rotDelivered, + Rotation2d.fromDegrees(-m_gyro.getAngle()))); + SwerveDriveKinematics.desaturateWheelSpeeds( + swerveModuleStates, DriveConfig.kMaxSpeedMetersPerSecond); + m_frontLeft.setDesiredState(swerveModuleStates[0]); + m_frontRight.setDesiredState(swerveModuleStates[1]); + m_rearLeft.setDesiredState(swerveModuleStates[2]); + m_rearRight.setDesiredState(swerveModuleStates[3]); + } + + /** + * applies slewrate limiting to the given control vector + * + * @param spdVec the vector which represents the commanded speed of the drivetrain + * @return the slew rate limited Vector for controlling the drivetrain + */ + private void limitDirectionSlewRate(Vector spdVec) { + // Convert XY to polar for rate limiting + double inputTranslationDir = spdVec.angle(); + double inputTranslationMag = spdVec.mag(); + + // Calculate the direction slew rate based on an estimate of the lateral acceleration + double directionSlewRate; + // if very close to zero but not exactly zero, there is no in division by zero due to floating + // point precision errors + if (m_currentTranslationMag != 0) { + // set lower rate of change/slew rate for higher translation speeds + directionSlewRate = Math.abs(OIConstants.kDirectionSlewRate / m_currentTranslationMag); + } else { + directionSlewRate = DriveConfig.HIGH_DIRECTION_SLEW_RATE; + } + + double currentTime = m_timer.get(); + double elapsedTime = currentTime - m_prevSlewRateTime; + + double angleDif = + SwerveUtils.AngleDifference(inputTranslationDir, m_currentTranslationDirRadians); + + if (angleDif < DriveConfig.MIN_ANGLE_SLEW_RATE) { + m_currentTranslationDirRadians = + SwerveUtils.StepTowardsCircular( + m_currentTranslationDirRadians, inputTranslationDir, directionSlewRate * elapsedTime); + m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); + SmartDashboard.putNumber("translation magnitude output", inputTranslationMag); + } else if (angleDif > DriveConfig.MAX_ANGLE_SLEW_RATE) { + if (m_currentTranslationMag > 1e-4) { + m_currentTranslationMag = m_magLimiter.calculate(0.0); + } else { + m_currentTranslationDirRadians = + SwerveUtils.WrapAngle(m_currentTranslationDirRadians + Math.PI); + m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); + } + } else { + m_currentTranslationDirRadians = + SwerveUtils.StepTowardsCircular( + m_currentTranslationDirRadians, inputTranslationDir, directionSlewRate * elapsedTime); + + m_currentTranslationMag = m_magLimiter.calculate(0.0); + + m_prevSlewRateTime = currentTime; + } + + spdVec.setX(m_currentTranslationMag); + spdVec.setY(0); + spdVec.rot(m_currentTranslationDirRadians); + } + + /** + * checks whether pathplanner paths should be flipped based on the current alliance + * + * @return whether pathplanner paths should be flipped + */ + private boolean allianceCheck() { + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + } + + /** Zeroes the heading of the robot. */ + public void zeroHeading() { + m_headingOffsetRadians = getGyroAngle().in(Units.Radians); + m_gyro.reset(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java new file mode 100644 index 00000000..a8ac2e94 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java @@ -0,0 +1,172 @@ +// 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 frc.robot.subsystems.drive; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.SparkPIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import frc.robot.constants.RobotConstants.SwerveModuleConstants; + +public class MAXSwerveModule { + private final CANSparkMax m_drivingSparkMax; + private final CANSparkMax m_turningSparkMax; + + private final RelativeEncoder m_drivingEncoder; + private final AbsoluteEncoder m_turningEncoder; + + private final SparkPIDController m_drivingPIDController; + private final SparkPIDController m_turningPIDController; + + private double m_chassisAngularOffset = 0; + private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d()); + + /** + * Constructs a MAXSwerveModule and configures the driving and turning motor, encoder, and PID + * controller. This configuration is specific to the REV MAXSwerve Module built with NEOs, SPARKS + * MAX, and a Through Bore Encoder. + */ + public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) { + this.m_drivingSparkMax = new CANSparkMax(drivingCANId, MotorType.kBrushless); + this.m_turningSparkMax = new CANSparkMax(turningCANId, MotorType.kBrushless); + + // Factory reset, so we get the SPARKS MAX to a known state before configuring + // them. This is useful in case a SPARK MAX is swapped out. + this.m_drivingSparkMax.restoreFactoryDefaults(); + this.m_turningSparkMax.restoreFactoryDefaults(); + + // Setup encoders and PID controllers for the driving and turning SPARKS MAX. + this.m_drivingEncoder = this.m_drivingSparkMax.getEncoder(); + this.m_turningEncoder = this.m_turningSparkMax.getAbsoluteEncoder(Type.kDutyCycle); + this.m_drivingPIDController = this.m_drivingSparkMax.getPIDController(); + this.m_turningPIDController = this.m_turningSparkMax.getPIDController(); + this.m_drivingPIDController.setFeedbackDevice(this.m_drivingEncoder); + this.m_turningPIDController.setFeedbackDevice(this.m_turningEncoder); + + // Apply position and velocity conversion factors for the driving encoder. The + // native units for position and velocity are rotations and RPM, respectively, + // but we want meters and meters per second to use with WPILib's swerve APIs. + this.m_drivingEncoder.setPositionConversionFactor( + SwerveModuleConstants.kDrivingEncoderPositionFactor); + this.m_drivingEncoder.setVelocityConversionFactor( + SwerveModuleConstants.kDrivingEncoderVelocityFactor); + + // Apply position and velocity conversion factors for the turning encoder. We + // want these in radians and radians per second to use with WPILib's swerve + // APIs. + this.m_turningEncoder.setPositionConversionFactor( + SwerveModuleConstants.kTurningEncoderPositionFactor); + this.m_turningEncoder.setVelocityConversionFactor( + SwerveModuleConstants.kTurningEncoderVelocityFactor); + + // Invert the turning encoder, since the output shaft rotates in the opposite direction of + // the steering motor in the MAXSwerve Module. + this.m_turningEncoder.setInverted(SwerveModuleConstants.kTurningEncoderInverted); + + // Enable PID wrap around for the turning motor. This will allow the PID + // controller to go through 0 to get to the setpoint i.e. going from 350 degrees + // to 10 degrees will go through 0 rather than the other direction which is a + // longer route. + this.m_turningPIDController.setPositionPIDWrappingEnabled(true); + this.m_turningPIDController.setPositionPIDWrappingMinInput( + SwerveModuleConstants.kTurningEncoderPositionPIDMinInput); + this.m_turningPIDController.setPositionPIDWrappingMaxInput( + SwerveModuleConstants.kTurningEncoderPositionPIDMaxInput); + + // Set the PID gains for the driving motor. Note these are example gains, and you + // may need to tune them for your own robot! + this.m_drivingPIDController.setP(SwerveModuleConstants.kDrivingP); + this.m_drivingPIDController.setI(SwerveModuleConstants.kDrivingI); + this.m_drivingPIDController.setD(SwerveModuleConstants.kDrivingD); + this.m_drivingPIDController.setFF(SwerveModuleConstants.kDrivingFF); + this.m_drivingPIDController.setOutputRange( + SwerveModuleConstants.kDrivingMinOutput, SwerveModuleConstants.kDrivingMaxOutput); + + // Set the PID gains for the turning motor. Note these are example gains, and you + // may need to tune them for your own robot! + this.m_turningPIDController.setP(SwerveModuleConstants.kTurningP); + this.m_turningPIDController.setI(SwerveModuleConstants.kTurningI); + this.m_turningPIDController.setD(SwerveModuleConstants.kTurningD); + this.m_turningPIDController.setFF(SwerveModuleConstants.kTurningFF); + this.m_turningPIDController.setOutputRange( + SwerveModuleConstants.kTurningMinOutput, SwerveModuleConstants.kTurningMaxOutput); + + this.m_drivingSparkMax.setIdleMode(SwerveModuleConstants.kDrivingMotorIdleMode); + this.m_turningSparkMax.setIdleMode(SwerveModuleConstants.kTurningMotorIdleMode); + this.m_drivingSparkMax.setSmartCurrentLimit(SwerveModuleConstants.kDrivingMotorCurrentLimit); + this.m_turningSparkMax.setSmartCurrentLimit(SwerveModuleConstants.kTurningMotorCurrentLimit); + + // Save the SPARK MAX configurations. If a SPARK MAX browns out during + // operation, it will maintain the above configurations. + this.m_drivingSparkMax.burnFlash(); + this.m_turningSparkMax.burnFlash(); + + this.m_chassisAngularOffset = chassisAngularOffset; + this.m_desiredState.angle = new Rotation2d(this.m_turningEncoder.getPosition()); + this.m_drivingEncoder.setPosition(0); + } + + /** + * Returns the current state of the module. + * + * @return The current state of the module. + */ + public SwerveModuleState getState() { + // Apply chassis angular offset to the encoder position to get the position + // relative to the chassis. + return new SwerveModuleState( + this.m_drivingEncoder.getVelocity(), + new Rotation2d(this.m_turningEncoder.getPosition() - this.m_chassisAngularOffset)); + } + + /** + * Returns the current position of the module. + * + * @return The current position of the module. + */ + public SwerveModulePosition getPosition() { + // Apply chassis angular offset to the encoder position to get the position + // relative to the chassis. + return new SwerveModulePosition( + this.m_drivingEncoder.getPosition(), + new Rotation2d(this.m_turningEncoder.getPosition() - this.m_chassisAngularOffset)); + } + + /** + * Sets the desired state for the module. + * + * @param desiredState Desired state with speed and angle. + */ + public void setDesiredState(SwerveModuleState desiredState) { + // Apply chassis angular offset to the desired state. + SwerveModuleState correctedDesiredState = new SwerveModuleState(); + correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond; + correctedDesiredState.angle = + desiredState.angle.plus(Rotation2d.fromRadians(this.m_chassisAngularOffset)); + + // Optimize the reference state to avoid spinning further than 90 degrees. + SwerveModuleState optimizedDesiredState = + SwerveModuleState.optimize( + correctedDesiredState, new Rotation2d(this.m_turningEncoder.getPosition())); + + // Command driving and turning SPARKS MAX towards their respective setpoints. + this.m_drivingPIDController.setReference( + optimizedDesiredState.speedMetersPerSecond, CANSparkMax.ControlType.kVelocity); + this.m_turningPIDController.setReference( + optimizedDesiredState.angle.getRadians(), CANSparkMax.ControlType.kPosition); + + this.m_desiredState = desiredState; + } + + /** Zeroes all the SwerveModule encoders. */ + public void resetEncoders() { + this.m_drivingEncoder.setPosition(0); + } +} \ No newline at end of file diff --git a/src/main/java/frc/utils/SwerveUtils.java b/src/main/java/frc/utils/SwerveUtils.java new file mode 100644 index 00000000..7443aad2 --- /dev/null +++ b/src/main/java/frc/utils/SwerveUtils.java @@ -0,0 +1,117 @@ +package frc.utils; + +public class SwerveUtils { + + /** + * Steps a value towards a target with a specified step size. + * + * @param _current The current or starting value. Can be positive or negative. + * @param _target The target value the algorithm will step towards. Can be positive or negative. + * @param _stepsize The maximum step size that can be taken. + * @return The new value for {@code _current} after performing the specified step towards the + * specified target. + */ + public static double StepTowards(double _current, double _target, double _stepsize) { + if (Math.abs(_current - _target) <= _stepsize) { + return _target; + } else if (_target < _current) { + return _current - _stepsize; + } else { + return _current + _stepsize; + } + } + + /** + * compares to doubles with the given tolerance and returns if they are approximately equal + * + * @param a the first double to compare + * @param b the second double to compare + * @param tol the maximum difference between doubles to still be considered equal + * @return whether the doubles are approximately equal + */ + public static final boolean approxEqual(double a, double b, double tol) { + return Math.abs(a - b) <= tol; + } + + /** + * compares two doubles and returns whether they are equal within a small tolerance + * + * @param a the first double to compare + * @param b the second double to compare + * @return whether the doubles are approximately equal + */ + public static final boolean approxEqual(double a, double b) { + return approxEqual(a, b, 1e-6); + } + + /** + * Steps a value (angle) towards a target (angle) taking the shortest path with a specified step + * size. + * + * @param _current The current or starting angle (in radians). Can lie outside the 0 to 2*PI + * range. + * @param _target The target angle (in radians) the algorithm will step towards. Can lie outside + * the 0 to 2*PI range. + * @param _stepsize The maximum step size that can be taken (in radians). + * @return The new angle (in radians) for {@code _current} after performing the specified step + * towards the specified target. This value will always lie in the range 0 to 2*PI + * (exclusive). + */ + public static double StepTowardsCircular(double _current, double _target, double _stepsize) { + _current = WrapAngle(_current); + _target = WrapAngle(_target); + + double stepDirection = Math.signum(_target - _current); + double difference = Math.abs(_current - _target); + + if (difference <= _stepsize) { + return _target; + } else if (difference > Math.PI) { // does the system need to wrap over eventually? + // handle the special case where you can reach the target in one step while also wrapping + if (_current + 2 * Math.PI - _target < _stepsize + || _target + 2 * Math.PI - _current < _stepsize) { + return _target; + } else { + return WrapAngle( + _current - stepDirection * _stepsize); // this will handle wrapping gracefully + } + } else { + return _current + stepDirection * _stepsize; + } + } + + /** + * Finds the (unsigned) minimum difference between two angles including calculating across 0. + * + * @param _angleA An angle (in radians). + * @param _angleB An angle (in radians). + * @return The (unsigned) minimum difference between the two angles (in radians). + */ + public static double AngleDifference(double _angleA, double _angleB) { + double difference = Math.abs(_angleA - _angleB); + return difference > Math.PI ? (2 * Math.PI) - difference : difference; + } + + /** + * Wraps an angle until it lies within the range from 0 to 2*PI (exclusive). + * + * @param _angle The angle (in radians) to wrap. Can be positive or negative and can lie multiple + * wraps outside the output range. + * @return An angle (in radians) from 0 and 2*PI (exclusive). + */ + public static double WrapAngle(double _angle) { + double twoPi = 2 * Math.PI; + + if (_angle + == twoPi) { // Handle this case separately to avoid floating point errors with the floor + // after the division in the case below + return 0.0; + } else if (_angle > twoPi) { + return _angle - twoPi * Math.floor(_angle / twoPi); + } else if (_angle < 0.0) { + return _angle + twoPi * (Math.floor((-_angle) / twoPi) + 1); + } else { + return _angle; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/utils/Vector.java b/src/main/java/frc/utils/Vector.java new file mode 100644 index 00000000..c1ed0d7a --- /dev/null +++ b/src/main/java/frc/utils/Vector.java @@ -0,0 +1,562 @@ +package frc.utils; + +// a class representing either a vector or a Point in space in 2 or more dimensions with double +// precision +// with operations for manipulating them + +public class Vector { + public static final Vector Origin = new Vector(0, 0); + + // the array of values for the location of the Point, + // from lowest dimension to highest, + // ie. x-value is vals[0], y-value is vals[1], etc. + protected double[] m_vals; + + /** constructs a new 2d Vector at (0,0) */ + public Vector() { + this(0, 0); + } + + /** + * constructs a new 2d Vector at (x, y) + * + * @param x the x value for the Vector + * @param y the y value for the Vector + */ + public Vector(double x, double y) { + m_vals = new double[2]; + m_vals[0] = x; + m_vals[1] = y; + } + + /** + * constructs a new 3d Vector at (x, y, z) + * + * @param x the x value for the Vector + * @param y the y value for the Vector + * @param z the z value for the Vector + */ + public Vector(double x, double y, double z) { + m_vals = new double[3]; + m_vals[0] = x; + m_vals[1] = y; + m_vals[2] = z; + } + + /** + * constructs a new n-dimensional Vector at (vals[0], vals[1], ..., vals[n]) where n is the final + * element of vals + * + * @param vals the dimensions to use for the new Vector + */ + public Vector(double[] vals) { + if (vals.length < 2) { + throw new IllegalArgumentException("dimension counts less than 2 not supported"); + } + this.m_vals = vals; + } + + /** + * constructs a new n-dimensional Vector where all dimensions start as 0 + * + * @param dimensions the number of dimensions to construct the Vector with + */ + public Vector(int dimensions) { + if (dimensions < 2) { + throw new IllegalArgumentException("dimension counts less than 2 not supported"); + } + m_vals = new double[dimensions]; + } + + /** + * returns a Vector which is a copy of this one + * + * @return a copy of this Vector + */ + public Vector copy() { + Vector newP = new Vector(m_vals.length); + for (int i = 0; i < m_vals.length; i++) { + newP.set(i, m_vals[i]); + } + return newP; + } + + /** + * returns the number of dimensions of this Vector + * + * @return the number of dimensions of this Vector + */ + public int dims() { + return m_vals.length; + } + + @Override + /** returns a String representation of this Vector in the format (x, y, ..., n) */ + public String toString() { + String s = "(" + m_vals[0]; + for (int i = 1; i < m_vals.length; i++) { + s += "," + m_vals[i]; + } + return s + ")"; + } + + /** + * returns a new Vector using the String representation of a Vector as returned by + * Vector.toString() + * + * @param data the String representation of a vector + * @return the new Vector + */ + public static Vector fromString(String data) { + data = data.substring(1, data.length() - 1); + String[] valStrings = data.split(","); + double[] vals = new double[valStrings.length]; + for (int i = 0; i < valStrings.length; i++) vals[i] = Double.parseDouble(valStrings[i]); + return new Vector(vals); + } + + /** + * returns the magnitude of this Vector + * + * @return the magnitude of this Vector + */ + public double mag() { + double n = 0; + for (double d : m_vals) { + n += d * d; + } + return Math.sqrt(n); + } + + /** + * returns the magnitude of this Vector squared more quickly than mag() + * + * @return the magnitude of this Vector squared + */ + public double squaredMag() { + double n = 0; + for (double d : m_vals) { + n += d * d; + } + return n; + } + + /** + * returns the X component of this Vector + * + * @return the X component of this Vector + */ + public double x() { + return m_vals[0]; + } + + /** + * sets the X component of this Vector to the given value + * + * @param n the new value for the X component of this Vector + */ + public void setX(double n) { + m_vals[0] = n; + } + + /** + * returns the Y component of this Vector + * + * @return the Y component of this Vector + */ + public double y() { + return m_vals[1]; + } + + /** + * sets the Y component of this Vector to the given value + * + * @param n the new value for the Y component of this Vector + */ + public void setY(double n) { + m_vals[1] = n; + } + + /** + * returns the Z component of this Vector + * + * @return the Z component of this Vector + */ + public double z() { + if (m_vals.length < 2) + throw new IllegalStateException("z-value requires a point with at least 3 dimensions"); + return m_vals[2]; + } + + /** + * sets the Z component of this Vector to the given value + * + * @param n the new value for the Z component of this Vector + */ + public void setZ(double n) { + if (m_vals.length < 2) + throw new IllegalStateException("z-value requires a point with at least 3 dimensions"); + m_vals[2] = n; + } + + /** + * returns the value of dimension dim of this Vector where X is dimension 0, Y is dimension 1, and + * so on + * + * @param dim the dimension to be returned + * @return the value of dimension dim + */ + public double get(int dim) { + if (m_vals.length <= dim) { + return 0; + } + return m_vals[dim]; + } + + /** + * sets the value of dimension dim of this Vector to n + * + * @param dim the dimension to be set + * @param n the value to set dim to + */ + public void set(int dim, double n) { + m_vals[dim] = n; + } + + /** + * returns the distance between this Vector and Vector p + * + * @param p the Vector to get the distance to + * @return the distance between this Vector and Vector p + */ + public double dist(Vector p) { + if (p.m_vals.length != this.m_vals.length) { + throw new IllegalArgumentException( + "points to compare must have the same number of dimensions"); + } + return p.copy().sub(this).mag(); + } + + /** + * sets the magnitude of this Vector to 1 while maintaining the relative proportions of each + * dimension + * + * @return this Vector + */ + public Vector normalize() { + div(mag()); + return this; + } + + /** + * divides all the dimensions of this Vector by the given value + * + * @param d the number to divide this Vector by + * @return this Vector + */ + public Vector div(double d) { + for (int i = 0; i < m_vals.length; i++) { + m_vals[i] /= d; + } + return this; + } + + /** + * divides all the dimensions of this Vector by the given double + * + * @param d the number to multiply by + * @return this Vector + */ + public Vector mult(double d) { + for (int i = 0; i < m_vals.length; i++) { + m_vals[i] *= d; + } + return this; + } + + /** + * divides all the dimensions of this Vector by the given long value + * + * @param d the number to multiply by + * @return this Vector + */ + public Vector mult(long l) { + for (int i = 0; i < m_vals.length; i++) { + m_vals[i] *= l; + } + return this; + } + + /** + * adds all the shared dimensions of another vaector to this one + * + * @param p the Vector to be added to this one + * @return this Vector + */ + public Vector add(Vector p) { + if (p.m_vals.length > this.m_vals.length) { + throw new IllegalArgumentException( + "points to add must have the same number of dimensions or less"); + } + for (int i = 0; i < Math.min(m_vals.length, p.m_vals.length); i++) { + this.m_vals[i] += p.m_vals[i]; + } + return this; + } + + /** + * adds the given values to the x and y dimensions of this Vector + * + * @param x the value to add to the x dimension of this Vector + * @param y the value to add to the y dimension of this Vector + * @return this Vector + */ + public Vector add(double x, double y) { + m_vals[0] += x; + m_vals[1] += y; + return this; + } + + /** + * subtracts the shared dimensions of another Vector from this Vector + * + * @param p the Vector to subtract from this one + * @return this Vector + */ + public Vector sub(Vector p) { + for (int i = 0; i < Math.min(m_vals.length, p.m_vals.length); i++) { + this.m_vals[i] -= p.m_vals[i]; + } + return this; + } + + /** + * subtracts the given x and y values from the x and y dimensions of this Vector + * + * @param x the value to subtract from the x dimension of this Vector + * @param y the value to subtract from the y value of this Vector + * @return this Vector + */ + public Vector sub(double x, double y) { + m_vals[0] -= x; + m_vals[1] -= y; + return this; + } + + /** + * returns the dot product between this Vector and another if one vector is normalized, this can + * be thought of as getting the distance along that vector as an axis if both vectors are + * normalized this can be used to get the cosine of the angle between the two vectors + * + * @param p the Vector to get the dot product from + * @return this Vector + */ + public double dot(Vector p) { + if (p.m_vals.length != this.m_vals.length) { + throw new IllegalArgumentException( + "dot product requires the same number of dimensions between points"); + } + double sum = 0; + for (int i = 0; i < m_vals.length; i++) { + sum += this.m_vals[i] * p.m_vals[i]; + } + return sum; + } + + /** + * returns the dot product between this Vector and another using only the X and Y dimensions if + * one vector is normalized, this can be thought of as getting the distance along that vector as + * an axis if both vectors are normalized this can be used to get the cosine of the angle between + * the two vectors + * + * @param p the Vector to get the dot product from + * @return this Vector + */ + public double dot2d(Vector p) { + double sum = 0; + for (int i = 0; i < 2; i++) { + sum += this.m_vals[i] * p.m_vals[i]; + } + return sum; + } + + /** + * sets all of the dimensions of this Vector to their absolute value + * + * @return this Vector + */ + public Vector abs() { + for (int i = 0; i < m_vals.length; i++) { + m_vals[i] = Math.abs(m_vals[i]); + } + return this; + } + + /** + * applies a 2d linear transformation to this Vector, where the transformed location of (1, 0) is + * iHatLoc, and the transformed location of (0, 1) is jHatLoc, and is also equivalent to + * multiplying the matrix with iHatLoc and jHatLoc as it's columns by this Vector + * + * @param iHatLoc the transformed location of the x axis basis vector + * @param jHatLoc the transformed location of the y axis basis vector + * @return this Vector + */ + public Vector matrixTransform(Vector iHatLoc, Vector jHatLoc) { + double newX = y() * jHatLoc.x() + x() * iHatLoc.x(); + double newY = y() * jHatLoc.y() + x() * iHatLoc.y(); + setX(newX); + setY(newY); + return this; + } + + /** + * returns the Vector 90 degrees counter-clockwise from this one + * + * @return the Vector 90 degrees counter-clockwise from this one + */ + public Vector getPerpendicular() { + return new Vector(-y(), x()); + } + + /** + * rotates this Vector the given number of radians around the origin + * + * @param theta the number of radians to rotate this Vector around the origin + * @return this Vector + */ + public Vector rot(double theta) { + double prevX = x(); + double prevY = y(); + setX(Math.cos(theta) * prevX - Math.sin(theta) * prevY); + setY(Math.cos(theta) * prevY + Math.sin(theta) * prevX); + return this; + } + + /** + * restricts this Point to a maximum length, setting it to that length it it it longer, then + * returns itself + * + * @param maxLength the maximum length to be clamped to + * @return this Vector + */ + public Vector clampLength(double maxLength) { + return clampLength(0, maxLength); + } + + /** + * restricts this Point to a maximum length, setting it to that length it it it longer, then + * returns itself + * + * @param maxLength the maximum length to be clamped to + * @return this Vector + */ + public Vector clampLength(double minLength, double maxLength) { + if (minLength > maxLength) throw new IllegalArgumentException(); + if (this.squaredMag() < minLength * minLength) { + normalize(); + mult(minLength); + return this; + } + if (this.squaredMag() > maxLength * maxLength) { + normalize(); + mult(maxLength); + } + return this; + } + + /** + * clamps the value of a Point elementwise between the limits given by minimum and maximum Points + * such that min.X() <= max.X() and min.Y() <= max.Y() + * + * @param min the minimum value for the dimensions of this Vector + * @param max the maximum value for the dimentions of this Vector + * @return this Vector + */ + public Vector clamp(Vector min, Vector max) { + if (min.dims() != max.dims()) { + throw new IllegalArgumentException("arguments must have the same number of dimensions"); + } + for (int i = 0; i < dims() && i < min.dims(); i++) { + if (m_vals[i] < min.m_vals[i]) m_vals[i] = min.m_vals[i]; + if (m_vals[i] > max.m_vals[i]) m_vals[i] = max.m_vals[i]; + } + return this; + } + + /** + * transforms this Point into the space of Point space this Point is also multiplied by the + * magnitude of space in the process, so if you want to avoid this, normalize space first + * effectively just a matrix transformation where space is iHat and jHat is space rotated 90 + * degreed counter-clockwise + * + * @param space the vector to the space of the given Vector + * @return this Vector + */ + public Vector toSpace(Vector space) { + double oldX = x(); + double oldY = y(); + setX(oldX * space.x() + oldY * space.y()); + setY(oldY * space.x() - oldX * space.y()); + return this; + } + + /** + * returns whether this Vector is equivelent to another one in both number of dimensions and + * values of dimensions + * + * @param p the Vector to compare to + * @return whether the two Vectors are equal + */ + public boolean equals(Vector p) { + if (p.m_vals.length != this.m_vals.length) { + return false; + } + for (int i = 0; i < m_vals.length; i++) { + if (this.m_vals[i] != p.m_vals[i]) { + return false; + } + } + return true; + } + + /** + * gets the angle from the origin to this Vector in a counter-clockwise direction + * + * @return the angle from the origin to this Vector + */ + public double angle() { + return Math.atan2(y(), x()); + } + + /** + * returns whether this Vector is within the given minimum and maximum bounds + * + * @param boundsMin the minumum values of the bounds + * @param boundsMax the maximum values of the bounds + * @return whether this Vector is within the given bounds + */ + public boolean isWithinBounds(Vector boundsMin, Vector boundsMax) { + double minX; + double maxX; + double minY; + double maxY; + + if (boundsMin.x() < boundsMax.x()) { + minX = boundsMin.x(); + maxX = boundsMax.x(); + } else { + maxX = boundsMin.x(); + minX = boundsMax.x(); + } + + if (boundsMin.y() < boundsMax.y()) { + minY = boundsMin.y(); + maxY = boundsMax.y(); + } else { + maxY = boundsMin.y(); + minY = boundsMax.y(); + } + + return minX <= x() && x() <= maxX && minY <= y() && y() <= maxY; + } +} \ No newline at end of file From c61edc65a72a2c1a5c632fb92a010cceaa1b39fe Mon Sep 17 00:00:00 2001 From: Iris Date: Sat, 16 Mar 2024 18:02:42 -0700 Subject: [PATCH 31/51] this shooter code actually works stpo mentors --- src/main/java/frc/robot/RobotContainer.java | 27 ++++++++++++++++--- .../robot/commands/shooter/SpinFlywheels.java | 1 - .../java/frc/robot/constants/RobotConfig.java | 2 +- 3 files changed, 25 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7e746319..830799a8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,7 +9,10 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.BasicDriveCommand; @@ -21,6 +24,7 @@ import frc.robot.commands.shooter.StowShooter; import frc.robot.constants.RobotConfig; import frc.robot.constants.RobotConfig.FieldElement; +import frc.robot.constants.RobotConfig.ShooterConfig; import frc.robot.constants.RobotConstants.Bindings; import frc.robot.constants.RobotConstants.DriveConstants.OIConstants; import frc.robot.subsystems.drive.Drivetrain; @@ -126,7 +130,7 @@ private void configureBindings() { .whileTrue(new PivotMove(m_shooter, 0.3)); new Trigger(() -> m_operatorController.getRawButton(Bindings.kAimAmp)) - .whileTrue(new PivotMove(m_shooter, 0.55)); + .whileTrue(new PivotMove(m_shooter, 0.8)); } private void updateInput() { @@ -142,9 +146,26 @@ private void updateInput() { // TODO: fill in placeholder commands with actual functionality private void registerCommands() { + //timeout doesn't need to be set because it is in a race group with the intake path in the .path file NamedCommands.registerCommand("intakeFromFloor", new RunIntake(m_intake, false)); - NamedCommands.registerCommand("scoreAmp", doNothing()); - NamedCommands.registerCommand("aimAndScoreSpeaker", doNothing()); + + NamedCommands.registerCommand("shootSpeaker", + new SequentialCommandGroup( + new PivotMove(m_shooter, 0.55).withTimeout(1), + new SpinFlywheels(m_shooter, FieldElement.SPEAKER).withTimeout(1.5), + new ParallelRaceGroup( + new SpinFlywheels(m_shooter, FieldElement.SPEAKER), + new Shoot(m_indexer, false)).withTimeout(3) + )); + + NamedCommands.registerCommand("shootAmp", + new SequentialCommandGroup( + new PivotMove(m_shooter, 0.3).withTimeout(1), + new SpinFlywheels(m_shooter, FieldElement.AMP).withTimeout(1.5), + new ParallelRaceGroup( + new SpinFlywheels(m_shooter, FieldElement.AMP), + new Shoot(m_indexer, false)).withTimeout(3) + )); } private Command doNothing() { diff --git a/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java b/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java index 34733d27..8d67f930 100644 --- a/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java +++ b/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java @@ -63,7 +63,6 @@ public void initialize() { default: desiredVelocity = 0; desiredAngle = Units.Degrees.of(0); - desiredVelocity = 0; break; } m_shooter.runFlywheel(desiredVelocity); diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 2c6d894b..df35e4a3 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -104,7 +104,7 @@ public static final class ShooterConfig { public static final Measure kAdjustAmountDegrees = Units.Rotations.of(0.5 / 360 * kEncoderRotsToPivotRot); - public static final double kDefaultAmpVelocity = 1500; // rpm + public static final double kDefaultAmpVelocity = 500; // rpm public static final double kDefaultTrapVelocity = 2000; // rpm public static final double kDefaultSpeakerVelocity = 4500; // rpm } From e44b0b9ce9482c13a67a7772659f04bd41dfe04f Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Sat, 16 Mar 2024 18:06:22 -0700 Subject: [PATCH 32/51] spotless --- src/main/java/frc/robot/RobotContainer.java | 41 ++++++++++----------- 1 file changed, 20 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 830799a8..a69f641e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.BasicDriveCommand; @@ -24,7 +23,6 @@ import frc.robot.commands.shooter.StowShooter; import frc.robot.constants.RobotConfig; import frc.robot.constants.RobotConfig.FieldElement; -import frc.robot.constants.RobotConfig.ShooterConfig; import frc.robot.constants.RobotConstants.Bindings; import frc.robot.constants.RobotConstants.DriveConstants.OIConstants; import frc.robot.subsystems.drive.Drivetrain; @@ -77,7 +75,7 @@ public RobotContainer() { private void configureBindings() { new Trigger(() -> m_operatorController.getRawButton(11)) - .whileTrue(new RunCommand(() -> m_shooter.setBasic(), m_shooter)); + .whileTrue(new RunCommand(() -> m_shooter.setBasic(), m_shooter)); // angle on 8-directional button m_autoAim = new POVButton(m_operatorController, 0); m_trapAim = new POVButton(m_operatorController, 90); @@ -146,26 +144,27 @@ private void updateInput() { // TODO: fill in placeholder commands with actual functionality private void registerCommands() { - //timeout doesn't need to be set because it is in a race group with the intake path in the .path file + // timeout doesn't need to be set because it is in a race group with the intake path in the + // .path file NamedCommands.registerCommand("intakeFromFloor", new RunIntake(m_intake, false)); - NamedCommands.registerCommand("shootSpeaker", - new SequentialCommandGroup( - new PivotMove(m_shooter, 0.55).withTimeout(1), - new SpinFlywheels(m_shooter, FieldElement.SPEAKER).withTimeout(1.5), - new ParallelRaceGroup( - new SpinFlywheels(m_shooter, FieldElement.SPEAKER), - new Shoot(m_indexer, false)).withTimeout(3) - )); - - NamedCommands.registerCommand("shootAmp", - new SequentialCommandGroup( - new PivotMove(m_shooter, 0.3).withTimeout(1), - new SpinFlywheels(m_shooter, FieldElement.AMP).withTimeout(1.5), - new ParallelRaceGroup( - new SpinFlywheels(m_shooter, FieldElement.AMP), - new Shoot(m_indexer, false)).withTimeout(3) - )); + NamedCommands.registerCommand( + "shootSpeaker", + new SequentialCommandGroup( + new PivotMove(m_shooter, 0.55).withTimeout(1), + new SpinFlywheels(m_shooter, FieldElement.SPEAKER).withTimeout(1.5), + new ParallelRaceGroup( + new SpinFlywheels(m_shooter, FieldElement.SPEAKER), new Shoot(m_indexer, false)) + .withTimeout(3))); + + NamedCommands.registerCommand( + "shootAmp", + new SequentialCommandGroup( + new PivotMove(m_shooter, 0.3).withTimeout(1), + new SpinFlywheels(m_shooter, FieldElement.AMP).withTimeout(1.5), + new ParallelRaceGroup( + new SpinFlywheels(m_shooter, FieldElement.AMP), new Shoot(m_indexer, false)) + .withTimeout(3))); } private Command doNothing() { From d327d306a86086a4f3a819c71a2d47c351fe1c07 Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Sun, 17 Mar 2024 21:48:03 -0700 Subject: [PATCH 33/51] refactoring --- .../robot/commands/shooter/ManualAdjust.java | 17 +++++++++-------- .../frc/robot/commands/shooter/PivotMove.java | 1 - .../robot/commands/shooter/SpinFlywheels.java | 2 +- .../frc/robot/subsystems/intake/Intake.java | 5 ----- .../frc/robot/subsystems/shooter/Shooter.java | 6 +++--- 5 files changed, 13 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/commands/shooter/ManualAdjust.java b/src/main/java/frc/robot/commands/shooter/ManualAdjust.java index 2dba9bc8..7e720abe 100644 --- a/src/main/java/frc/robot/commands/shooter/ManualAdjust.java +++ b/src/main/java/frc/robot/commands/shooter/ManualAdjust.java @@ -24,24 +24,26 @@ public ManualAdjust(Shooter shooter, AdjustType type) { @Override public void initialize() { timer.start(); + } + + @Override + public void execute() { switch (m_type) { case up: desiredAngle = m_shooter.getCurrentAngle().plus(ShooterConfig.kAdjustAmountDegrees); - m_shooter.setAngle(desiredAngle); break; case down: desiredAngle = m_shooter.getCurrentAngle().minus(ShooterConfig.kAdjustAmountDegrees); - m_shooter.setAngle(desiredAngle); break; default: desiredAngle = m_shooter.getCurrentAngle(); - m_shooter.setAngle(desiredAngle); break; } - } - @Override - public void execute() { + if (timer.get() % 10 == 0) { + m_shooter.setAngle(desiredAngle); + } + m_shooter.setFF( Math.cos(Units.rotationsToRadians(m_shooter.getCurrentAngle().magnitude())) * ShooterConfig.kAngleControlFF); @@ -49,7 +51,6 @@ public void execute() { @Override public boolean isFinished() { - return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()) - || timer.get() > ShooterConfig.kAimTimeout; + return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()); } } diff --git a/src/main/java/frc/robot/commands/shooter/PivotMove.java b/src/main/java/frc/robot/commands/shooter/PivotMove.java index 7fecf3af..6af82fbf 100644 --- a/src/main/java/frc/robot/commands/shooter/PivotMove.java +++ b/src/main/java/frc/robot/commands/shooter/PivotMove.java @@ -32,7 +32,6 @@ public boolean isFinished() { @Override public void execute() { - System.out.println("angle movement cmd"); double ff = Math.cos(m_shooter.getCurrentAngle().in(Units.Radians)) * ShooterConfig.kAngleControlFF; m_shooter.setFF(ff); diff --git a/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java b/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java index 8d67f930..f233ec71 100644 --- a/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java +++ b/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java @@ -20,7 +20,7 @@ public class SpinFlywheels extends Command { public SpinFlywheels(Shooter shooter, FieldElement type) { m_shooter = shooter; - m_vision = new Vision(); + m_vision = null; m_type = type; addRequirements(m_shooter); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index ec32536b..d662735a 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -37,9 +37,4 @@ public void run(double motorOutput) { public void stop() { m_intakeRollerMotor.stopMotor(); } - - // stops the rollers - public void stopFeedNote() { - m_intakeRollerMotor.stopMotor(); - } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index a6fe1a6f..9ccd23e6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -146,17 +146,17 @@ public void putAngleOnSmartDashboard() { public void periodic() { SmartDashboard.putNumber("shield rots", m_shieldController.getEncoder().getPosition()); - double pval = SmartDashboard.getNumber("flywheel p", 0.1); + double pval = SmartDashboard.getNumber("flywheel p", ShooterConfig.kTopFlywheelP); if (pval != m_topFlywheelPIDController.getP()) { m_topFlywheelPIDController.setP(pval); } - double ival = SmartDashboard.getNumber("flywheel i", 0.0); + double ival = SmartDashboard.getNumber("flywheel i", ShooterConfig.kTopFlywheelI); if (pval != m_topFlywheelPIDController.getI()) { m_topFlywheelPIDController.setP(ival); } - double dval = SmartDashboard.getNumber("flywheel d", 0.0); + double dval = SmartDashboard.getNumber("flywheel d", ShooterConfig.kTopFlywheelD); if (pval != m_topFlywheelPIDController.getD()) { m_topFlywheelPIDController.setP(dval); } From d1dc3c03f40921cb4887dc542fc3d0bbffabd4e4 Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Mon, 18 Mar 2024 09:17:43 -0700 Subject: [PATCH 34/51] line break sensor and automated intake/shoot commands --- src/main/java/frc/robot/RobotContainer.java | 27 ++++++++++++------- .../frc/robot/commands/intake/RunIntake.java | 23 ++++++---------- .../frc/robot/constants/RobotConstants.java | 4 +-- .../frc/robot/subsystems/intake/Intake.java | 11 -------- .../frc/robot/subsystems/shooter/Shooter.java | 19 ++++++------- 5 files changed, 36 insertions(+), 48 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a69f641e..e759e6c0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -99,17 +99,28 @@ private void configureBindings() { .whileTrue(new BasicDriveCommand(m_robotDrive, m_driverController)); // RunIntake constructor boolean is whether or not the intake should run reversed. - new Trigger(this::getIntakeButton).whileTrue(new RunIntake(m_intake, false)); - new Trigger(this::getReverseIntakeButton).whileTrue(new RunIntake(m_intake, true)); + new Trigger(this::getIntakeButton).onTrue(new RunIntake(m_intake, m_indexer, false)); + new Trigger(this::getReverseIntakeButton).whileTrue(new RunIntake(m_intake, m_indexer, true)); // just shoot on trigger new Trigger(() -> m_operatorController.getRawButton(Bindings.kShoot)) .whileTrue(new Shoot(m_indexer, false)); new Trigger(() -> m_operatorController.getRawButton(Bindings.kShootReverse)) .whileTrue(new Shoot(m_indexer, true)); + new Trigger(() -> m_operatorController.getRawButton(Bindings.kFlywheelAmp)) - .whileTrue(new SpinFlywheels(m_shooter, FieldElement.AMP)); + .whileTrue( + new SequentialCommandGroup( + new SpinFlywheels(m_shooter, FieldElement.AMP).withTimeout(1.5), + new ParallelCommandGroup( + new Shoot(m_indexer, false), new SpinFlywheels(m_shooter, FieldElement.AMP)))); + new Trigger(() -> m_operatorController.getRawButton(Bindings.kFlywheelSpeaker)) - .whileTrue(new SpinFlywheels(m_shooter, FieldElement.SPEAKER)); + .whileTrue( + new SequentialCommandGroup( + new SpinFlywheels(m_shooter, FieldElement.SPEAKER).withTimeout(1.5), + new ParallelCommandGroup( + new Shoot(m_indexer, false), + new SpinFlywheels(m_shooter, FieldElement.SPEAKER)))); m_trapAim.whileTrue(new SpinFlywheels(m_shooter, FieldElement.TRAP)); @@ -146,7 +157,7 @@ private void updateInput() { private void registerCommands() { // timeout doesn't need to be set because it is in a race group with the intake path in the // .path file - NamedCommands.registerCommand("intakeFromFloor", new RunIntake(m_intake, false)); + NamedCommands.registerCommand("intakeFromFloor", new RunIntake(m_intake, m_indexer, false)); NamedCommands.registerCommand( "shootSpeaker", @@ -167,10 +178,6 @@ private void registerCommands() { .withTimeout(3))); } - private Command doNothing() { - return Commands.none(); - } - /** * Returns true if the intake is pressed; False otherwise. * diff --git a/src/main/java/frc/robot/commands/intake/RunIntake.java b/src/main/java/frc/robot/commands/intake/RunIntake.java index eca55966..e6767af2 100644 --- a/src/main/java/frc/robot/commands/intake/RunIntake.java +++ b/src/main/java/frc/robot/commands/intake/RunIntake.java @@ -1,46 +1,39 @@ package frc.robot.commands.intake; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConfig; +import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; public class RunIntake extends Command { private final Intake m_intake; + private final Indexer m_indexer; private boolean m_reversed; - /** - * Creates a new RunIntake command, which runs the roller motor on the intake subsystem to intake - * a note - * - * @param intake The subsystem used by this command. - */ - public RunIntake(Intake intake, boolean reversed) { + public RunIntake(Intake intake, Indexer indexer, boolean reversed) { m_intake = intake; + m_indexer = indexer; m_reversed = reversed; addRequirements(intake); } - // Called when the command is initially scheduled. @Override public void initialize() { int multiplier = m_reversed ? -1 : 1; m_intake.run(RobotConfig.IntakeConfig.kDefaultSpeed * multiplier); + m_indexer.startFeedNote(m_reversed); } - // 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) { m_intake.stop(); + m_indexer.stopFeedNote(); } - // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return SmartDashboard.getBoolean("Shooter/line break", false); } } diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index a57bf0be..9be1c826 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -59,6 +59,7 @@ public final class ShooterConstants { public static final int kShieldMotorId = 18; public static final int kAngleMotorLeaderId = 13; public static final int kAngleMotorFollowerId = 14; + public static final int kLineBreakPort = 6; public static final double FlywheelDiameter = 0.0762; public static final double ShooterLength = 0.4064; public static final double Gravity = 9.81; @@ -176,9 +177,6 @@ public static final class SwerveModuleConstants { } public static final class IntakeConstants { - public static final int kLineBreakSensor = 0; - - // Roller motor ID public static final int kMotorID = 10; } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index d662735a..ef90c3c4 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -2,26 +2,15 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.RobotConstants.IntakeConstants; public class Intake extends SubsystemBase { private final CANSparkMax m_intakeRollerMotor; // Intake roller motor - private final DigitalInput m_linebreak; /** Creates a new ExampleSubsystem. */ public Intake() { m_intakeRollerMotor = new CANSparkMax(IntakeConstants.kMotorID, MotorType.kBrushless); - // TODO maybe use to terminate intake command - m_linebreak = new DigitalInput(IntakeConstants.kLineBreakSensor); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - SmartDashboard.putBoolean("Intake/linebreak sensor", m_linebreak.get()); } /** diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 9ccd23e6..e3fc6e45 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -8,6 +8,7 @@ import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController.ArbFFUnits; import edu.wpi.first.units.*; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -37,6 +38,8 @@ public class Shooter extends SubsystemBase { private CANSparkMax m_shieldController; private RelativeEncoder m_shieldEncoder; + private DigitalInput m_lineBreakSensor; + private MutableMeasure> m_shooterSpeed; private MutableMeasure m_shieldPosition; private MutableMeasure> m_targetVelocity; @@ -44,7 +47,6 @@ public class Shooter extends SubsystemBase { private MutableMeasure m_targetAngle; public Shooter() { - // Flywheel m_topFlywheelMotor = new CANSparkMax(ShooterConstants.kTopFlywheelMotorId, MotorType.kBrushless); @@ -54,6 +56,7 @@ public Shooter() { new CANSparkMax(ShooterConstants.kBottomFlywheelMotorId, MotorType.kBrushless); m_bottomFlywheelMotor.follow(m_topFlywheelMotor, true); m_bottomFlywheelEncoder = m_bottomFlywheelMotor.getEncoder(); + m_lineBreakSensor = new DigitalInput(ShooterConstants.kLineBreakPort); // shield m_shieldController = new CANSparkMax(ShooterConstants.kShieldMotorId, MotorType.kBrushless); @@ -114,9 +117,6 @@ public Shooter() { if (DriverStation.isTest()) { putAngleOnSmartDashboard(); } - - SmartDashboard.putNumber("amp multiplier", 4 / 9); - SmartDashboard.putNumber("speaker multiplier", 2 / 9); } public void putAngleOnSmartDashboard() { @@ -144,19 +144,20 @@ public void putAngleOnSmartDashboard() { @Override public void periodic() { - SmartDashboard.putNumber("shield rots", m_shieldController.getEncoder().getPosition()); + SmartDashboard.putBoolean("Shooter/line break", m_lineBreakSensor.get()); + SmartDashboard.putNumber("Shooter/shield rots", m_shieldController.getEncoder().getPosition()); - double pval = SmartDashboard.getNumber("flywheel p", ShooterConfig.kTopFlywheelP); + double pval = SmartDashboard.getNumber("fShooter/flywheel p", ShooterConfig.kTopFlywheelP); if (pval != m_topFlywheelPIDController.getP()) { m_topFlywheelPIDController.setP(pval); } - double ival = SmartDashboard.getNumber("flywheel i", ShooterConfig.kTopFlywheelI); + double ival = SmartDashboard.getNumber("Shooter/flywheel i", ShooterConfig.kTopFlywheelI); if (pval != m_topFlywheelPIDController.getI()) { m_topFlywheelPIDController.setP(ival); } - double dval = SmartDashboard.getNumber("flywheel d", ShooterConfig.kTopFlywheelD); + double dval = SmartDashboard.getNumber("Shooter/flywheel d", ShooterConfig.kTopFlywheelD); if (pval != m_topFlywheelPIDController.getD()) { m_topFlywheelPIDController.setP(dval); } @@ -173,7 +174,7 @@ public void periodic() { } SmartDashboard.putNumber( - "angle error", m_targetAngle.magnitude() - m_angleEncoder.getPosition()); + "Shooter/angle error", m_targetAngle.magnitude() - m_angleEncoder.getPosition()); } // sets the target angle the shooter should be at, called only once From fa048c3d3893b86908d1c4e861b51dc1bf316087 Mon Sep 17 00:00:00 2001 From: TurtleMeds Date: Tue, 19 Mar 2024 16:31:09 -0700 Subject: [PATCH 35/51] applied spotless --- src/main/java/frc/robot/RobotContainer.java | 30 ++--- .../frc/robot/commands/BasicDriveCommand.java | 2 +- .../commands/climber/IndividualClimb.java | 47 ++++--- .../java/frc/robot/constants/RobotConfig.java | 1 - .../frc/robot/constants/RobotConstants.java | 127 +++++++++--------- .../frc/robot/subsystems/climber/Climber.java | 11 +- .../robot/subsystems/drive/Drivetrain.java | 2 +- .../subsystems/drive/MAXSwerveModule.java | 2 +- src/main/java/frc/utils/SwerveUtils.java | 2 +- src/main/java/frc/utils/Vector.java | 2 +- 10 files changed, 110 insertions(+), 116 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 26b008b2..7a1836cf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -56,27 +56,27 @@ public RobotContainer() { } private void configureBindings() { - //new Trigger(() -> m_operatorController.getRawButton(15)).whileTrue(new Climb(m_climber, false)); + // new Trigger(() -> m_operatorController.getRawButton(15)).whileTrue(new Climb(m_climber, + // false)); - //new Trigger(() -> m_operatorController.getRawButton(16)).whileTrue(new Climb(m_climber, true)); + // new Trigger(() -> m_operatorController.getRawButton(16)).whileTrue(new Climb(m_climber, + // true)); - //down + // down new Trigger(() -> m_operatorController.getRawButton(11)) - .whileTrue(new IndividualClimb(m_climber, true, true)); + .whileTrue(new IndividualClimb(m_climber, true, true)); new Trigger(() -> m_operatorController.getRawButton(12)) - .whileTrue(new IndividualClimb(m_climber, true, false)); - //down + .whileTrue(new IndividualClimb(m_climber, true, false)); + // down new Trigger(() -> m_operatorController.getRawButton(13)) - .whileTrue(new IndividualClimb(m_climber, false, true)); + .whileTrue(new IndividualClimb(m_climber, false, true)); new Trigger(() -> m_operatorController.getRawButton(14)) - .whileTrue(new IndividualClimb(m_climber, false, false)); - - //up - new Trigger(() -> m_operatorController.getRawButton(15)) - .whileTrue(new Climb(m_climber, true)); - //down - new Trigger(() -> m_operatorController.getRawButton(16)) - .whileTrue(new Climb(m_climber, false)); + .whileTrue(new IndividualClimb(m_climber, false, false)); + + // up + new Trigger(() -> m_operatorController.getRawButton(15)).whileTrue(new Climb(m_climber, true)); + // down + new Trigger(() -> m_operatorController.getRawButton(16)).whileTrue(new Climb(m_climber, false)); } private void updateInput() { diff --git a/src/main/java/frc/robot/commands/BasicDriveCommand.java b/src/main/java/frc/robot/commands/BasicDriveCommand.java index 60cf4501..fc998f72 100644 --- a/src/main/java/frc/robot/commands/BasicDriveCommand.java +++ b/src/main/java/frc/robot/commands/BasicDriveCommand.java @@ -47,4 +47,4 @@ public void execute() { m_controller.getRightBumper(), m_controller.getAButton()); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/climber/IndividualClimb.java b/src/main/java/frc/robot/commands/climber/IndividualClimb.java index e0b79082..93c8641e 100644 --- a/src/main/java/frc/robot/commands/climber/IndividualClimb.java +++ b/src/main/java/frc/robot/commands/climber/IndividualClimb.java @@ -4,34 +4,33 @@ import frc.robot.subsystems.climber.Climber; public class IndividualClimb extends Command { - private Climber m_climber; - private boolean m_isLeader; - private boolean m_reverse; + private Climber m_climber; + private boolean m_isLeader; + private boolean m_reverse; - public IndividualClimb(Climber climber, boolean isLeader, boolean reverse) { - m_climber = climber; - m_isLeader = isLeader; - m_reverse = reverse; + public IndividualClimb(Climber climber, boolean isLeader, boolean reverse) { + m_climber = climber; + m_isLeader = isLeader; + m_reverse = reverse; - addRequirements(m_climber); - } + addRequirements(m_climber); + } - @Override - public void initialize() { - if (m_isLeader) { - m_climber.setLeader(m_reverse); - } else { - m_climber.setFollower(m_reverse); - } + @Override + public void initialize() { + if (m_isLeader) { + m_climber.setLeader(m_reverse); + } else { + m_climber.setFollower(m_reverse); } + } - @Override - public void end(boolean interrupted) { - if (m_isLeader) { - m_climber.stopLeader(); - } else { - m_climber.stopFollower(); - } + @Override + public void end(boolean interrupted) { + if (m_isLeader) { + m_climber.stopLeader(); + } else { + m_climber.stopFollower(); } - + } } diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 9940cef8..7d878746 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -3,7 +3,6 @@ import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; - import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 2335bbc3..ba5942df 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -3,11 +3,9 @@ import com.revrobotics.CANSparkBase.IdleMode; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.units.Angle; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; -import edu.wpi.first.units.Velocity; /** * Software/hardware constants (e.g. CAN IDs, gear ratios, field measurements, etc.). For software @@ -28,11 +26,11 @@ public static final class ClimberConstants { public static final double kClimberMinOutput = -1; } - public static final class NeoMotorConstants { - public static final double kFreeSpeedRpm = 5676; - } + public static final class NeoMotorConstants { + public static final double kFreeSpeedRpm = 5676; + } - public static final class DriveConstants { + public static final class DriveConstants { public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2; public static final double kFrontRightChassisAngularOffset = 0; public static final double kBackLeftChassisAngularOffset = Math.PI; @@ -69,65 +67,64 @@ public static final class DriveConstants { new Translation2d(kWheelBase.in(Units.Meters) / 2, -kTrackWidth.in(Units.Meters) / 2), new Translation2d(-kWheelBase.in(Units.Meters) / 2, kTrackWidth.in(Units.Meters) / 2), new Translation2d(-kWheelBase.in(Units.Meters) / 2, -kTrackWidth.in(Units.Meters) / 2)); - } - - - public static final class SwerveModuleConstants { - // The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T. - // This changes the drive speed of the module (a pinion gear with more teeth will result in a - // robot that drives faster). - public static final int kDrivingMotorPinionTeeth = 14; - - public static final double kMaxModuleSpeed = 1; - - // Invert the turning encoder, since the output shaft rotates in the opposite direction of - // the steering motor in the MAXSwerve Module. - public static final boolean kTurningEncoderInverted = true; - - // Calculations required for driving motor conversion factors and feed forward - public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60; - public static final double kWheelDiameterMeters = 0.0762; - public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; - // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the - // bevel pinion - public static final double kDrivingMotorReduction = - (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); - public static final double kDriveWheelFreeSpeedRps = - (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) / kDrivingMotorReduction; - - public static final double kDrivingEncoderPositionFactor = - (kWheelDiameterMeters * Math.PI) / kDrivingMotorReduction; // meters - public static final double kDrivingEncoderVelocityFactor = - ((kWheelDiameterMeters * Math.PI) / kDrivingMotorReduction) / 60.0; // meters per second - - public static final double kTurningEncoderPositionFactor = (2 * Math.PI); // radians - public static final double kTurningEncoderVelocityFactor = - (2 * Math.PI) / 60.0; // radians per second - - public static final double kTurningEncoderPositionPIDMinInput = 0; // radians - public static final double kTurningEncoderPositionPIDMaxInput = - kTurningEncoderPositionFactor; // radians - - public static final double kDrivingP = 0.04; - public static final double kDrivingI = 0; - public static final double kDrivingD = 0; - public static final double kDrivingFF = 1 / kDriveWheelFreeSpeedRps; - public static final double kDrivingMinOutput = -1; - public static final double kDrivingMaxOutput = 1; - - public static final double kTurningP = 1; - public static final double kTurningI = 0; - public static final double kTurningD = 0; - public static final double kTurningFF = 0; - public static final double kTurningMinOutput = -1; - public static final double kTurningMaxOutput = 1; - - public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake; - public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake; - - public static final int kDrivingMotorCurrentLimit = 50; // amps - public static final int kTurningMotorCurrentLimit = 20; // amps - } + } + + public static final class SwerveModuleConstants { + // The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T. + // This changes the drive speed of the module (a pinion gear with more teeth will result in a + // robot that drives faster). + public static final int kDrivingMotorPinionTeeth = 14; + + public static final double kMaxModuleSpeed = 1; + + // Invert the turning encoder, since the output shaft rotates in the opposite direction of + // the steering motor in the MAXSwerve Module. + public static final boolean kTurningEncoderInverted = true; + + // Calculations required for driving motor conversion factors and feed forward + public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60; + public static final double kWheelDiameterMeters = 0.0762; + public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; + // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the + // bevel pinion + public static final double kDrivingMotorReduction = + (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); + public static final double kDriveWheelFreeSpeedRps = + (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) / kDrivingMotorReduction; + + public static final double kDrivingEncoderPositionFactor = + (kWheelDiameterMeters * Math.PI) / kDrivingMotorReduction; // meters + public static final double kDrivingEncoderVelocityFactor = + ((kWheelDiameterMeters * Math.PI) / kDrivingMotorReduction) / 60.0; // meters per second + + public static final double kTurningEncoderPositionFactor = (2 * Math.PI); // radians + public static final double kTurningEncoderVelocityFactor = + (2 * Math.PI) / 60.0; // radians per second + + public static final double kTurningEncoderPositionPIDMinInput = 0; // radians + public static final double kTurningEncoderPositionPIDMaxInput = + kTurningEncoderPositionFactor; // radians + + public static final double kDrivingP = 0.04; + public static final double kDrivingI = 0; + public static final double kDrivingD = 0; + public static final double kDrivingFF = 1 / kDriveWheelFreeSpeedRps; + public static final double kDrivingMinOutput = -1; + public static final double kDrivingMaxOutput = 1; + + public static final double kTurningP = 1; + public static final double kTurningI = 0; + public static final double kTurningD = 0; + public static final double kTurningFF = 0; + public static final double kTurningMinOutput = -1; + public static final double kTurningMaxOutput = 1; + + public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake; + public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake; + + public static final int kDrivingMotorCurrentLimit = 50; // amps + public static final int kTurningMotorCurrentLimit = 20; // amps + } public static final class OIConstants { public static final int kDriverControllerPort = 0; diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index c80be672..4a6b1daf 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -3,7 +3,6 @@ import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; - import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.RobotConfig.ClimberConfig; @@ -33,8 +32,8 @@ public Climber() { @Override public void periodic() { SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); - if (leaderController.getEncoder().getPosition() < 0|| - leaderController.getEncoder().getPosition() > ClimberConfig.kUpperRotSoftStop) { + if (leaderController.getEncoder().getPosition() < 0 + || leaderController.getEncoder().getPosition() > ClimberConfig.kUpperRotSoftStop) { leaderController.set(0); followerController.set(0); } @@ -52,9 +51,9 @@ public void setBoth(boolean reverse) { public void setLeader(boolean reverse) { multiplier = reverse ? -1 : 1; - leaderController.set(0.7*multiplier); + leaderController.set(0.7 * multiplier); } - + public void stopLeader() { leaderController.set(0); } @@ -65,7 +64,7 @@ public void stopFollower() { public void setFollower(boolean reverse) { multiplier = reverse ? -1 : 1; - followerController.set(-0.7*multiplier); + followerController.set(-0.7 * multiplier); } public CANSparkMax getLeader() { diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index 188b71fc..b6a5f18a 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -428,4 +428,4 @@ public void zeroHeading() { m_headingOffsetRadians = getGyroAngle().in(Units.Radians); m_gyro.reset(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java index a8ac2e94..83aabc17 100644 --- a/src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java @@ -169,4 +169,4 @@ public void setDesiredState(SwerveModuleState desiredState) { public void resetEncoders() { this.m_drivingEncoder.setPosition(0); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/utils/SwerveUtils.java b/src/main/java/frc/utils/SwerveUtils.java index 7443aad2..401281f8 100644 --- a/src/main/java/frc/utils/SwerveUtils.java +++ b/src/main/java/frc/utils/SwerveUtils.java @@ -114,4 +114,4 @@ public static double WrapAngle(double _angle) { return _angle; } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/utils/Vector.java b/src/main/java/frc/utils/Vector.java index c1ed0d7a..96da268f 100644 --- a/src/main/java/frc/utils/Vector.java +++ b/src/main/java/frc/utils/Vector.java @@ -559,4 +559,4 @@ public boolean isWithinBounds(Vector boundsMin, Vector boundsMax) { return minX <= x() && x() <= maxX && minY <= y() && y() <= maxY; } -} \ No newline at end of file +} From 409e713959a99dcbc727e9fe4b0ce6ec719dc5f4 Mon Sep 17 00:00:00 2001 From: TurtleMeds Date: Tue, 19 Mar 2024 16:49:47 -0700 Subject: [PATCH 36/51] removed confusing labels on triggers --- src/main/java/frc/robot/RobotContainer.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7a1836cf..75742132 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -62,12 +62,10 @@ private void configureBindings() { // new Trigger(() -> m_operatorController.getRawButton(16)).whileTrue(new Climb(m_climber, // true)); - // down new Trigger(() -> m_operatorController.getRawButton(11)) .whileTrue(new IndividualClimb(m_climber, true, true)); new Trigger(() -> m_operatorController.getRawButton(12)) .whileTrue(new IndividualClimb(m_climber, true, false)); - // down new Trigger(() -> m_operatorController.getRawButton(13)) .whileTrue(new IndividualClimb(m_climber, false, true)); new Trigger(() -> m_operatorController.getRawButton(14)) From be98852b21db354ec515cfc3c84d7aea33070183 Mon Sep 17 00:00:00 2001 From: TurtleMeds Date: Tue, 19 Mar 2024 16:51:07 -0700 Subject: [PATCH 37/51] renamed variable for more clarity --- .../frc/robot/commands/climber/IndividualClimb.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/climber/IndividualClimb.java b/src/main/java/frc/robot/commands/climber/IndividualClimb.java index 93c8641e..1c0f9be4 100644 --- a/src/main/java/frc/robot/commands/climber/IndividualClimb.java +++ b/src/main/java/frc/robot/commands/climber/IndividualClimb.java @@ -5,12 +5,12 @@ public class IndividualClimb extends Command { private Climber m_climber; - private boolean m_isLeader; + private boolean m_isRight; private boolean m_reverse; - public IndividualClimb(Climber climber, boolean isLeader, boolean reverse) { + public IndividualClimb(Climber climber, boolean isRight, boolean reverse) { m_climber = climber; - m_isLeader = isLeader; + m_isRight = isRight; m_reverse = reverse; addRequirements(m_climber); @@ -18,7 +18,7 @@ public IndividualClimb(Climber climber, boolean isLeader, boolean reverse) { @Override public void initialize() { - if (m_isLeader) { + if (m_isRight) { m_climber.setLeader(m_reverse); } else { m_climber.setFollower(m_reverse); @@ -27,7 +27,7 @@ public void initialize() { @Override public void end(boolean interrupted) { - if (m_isLeader) { + if (m_isRight) { m_climber.stopLeader(); } else { m_climber.stopFollower(); From 25f9684f490af77060d52daa3eec64e4c3375f5a Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Wed, 20 Mar 2024 11:19:02 -0700 Subject: [PATCH 38/51] line break to indexer subsystem --- .../java/frc/robot/commands/intake/RunIntake.java | 3 +-- .../java/frc/robot/constants/RobotConstants.java | 4 ++-- .../java/frc/robot/subsystems/indexer/Indexer.java | 14 ++++++++++++++ .../java/frc/robot/subsystems/shooter/Shooter.java | 5 ----- 4 files changed, 17 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/commands/intake/RunIntake.java b/src/main/java/frc/robot/commands/intake/RunIntake.java index e6767af2..37e0856e 100644 --- a/src/main/java/frc/robot/commands/intake/RunIntake.java +++ b/src/main/java/frc/robot/commands/intake/RunIntake.java @@ -1,6 +1,5 @@ package frc.robot.commands.intake; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConfig; import frc.robot.subsystems.indexer.Indexer; @@ -34,6 +33,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return SmartDashboard.getBoolean("Shooter/line break", false); + return m_indexer.getLineBreak(); } } diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 9be1c826..bad7ca0a 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -60,8 +60,8 @@ public final class ShooterConstants { public static final int kAngleMotorLeaderId = 13; public static final int kAngleMotorFollowerId = 14; public static final int kLineBreakPort = 6; - public static final double FlywheelDiameter = 0.0762; - public static final double ShooterLength = 0.4064; + public static final double FlywheelDiameter = 0.0762; //meters + public static final double ShooterLength = 0.4064; //meters public static final double Gravity = 9.81; public static final Measure kShieldExtentionAngle = Units.Rotations.of(1); // TODO - Set to number of rotations to fully extend shield diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 8b375dda..bd023a2d 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -2,17 +2,27 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.RobotConfig; import frc.robot.constants.RobotConstants.ShooterConstants; public class Indexer extends SubsystemBase { private CANSparkMax m_shooterRollerMotor; + private DigitalInput m_lineBreakSensor; public Indexer() { // Roller m_shooterRollerMotor = new CANSparkMax(ShooterConstants.kRollerMotorLeftId, MotorType.kBrushless); + m_lineBreakSensor = new DigitalInput(ShooterConstants.kLineBreakPort); + } + + @Override + public void periodic() { + SmartDashboard.putBoolean("line break sensor", m_lineBreakSensor.get()); } // runs the rollers @@ -28,4 +38,8 @@ public void startFeedNote(boolean reverse) { public void stopFeedNote() { m_shooterRollerMotor.stopMotor(); } + + public boolean getLineBreak() { + return m_lineBreakSensor.get(); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index e3fc6e45..ee651308 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -8,7 +8,6 @@ import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController.ArbFFUnits; import edu.wpi.first.units.*; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -38,8 +37,6 @@ public class Shooter extends SubsystemBase { private CANSparkMax m_shieldController; private RelativeEncoder m_shieldEncoder; - private DigitalInput m_lineBreakSensor; - private MutableMeasure> m_shooterSpeed; private MutableMeasure m_shieldPosition; private MutableMeasure> m_targetVelocity; @@ -56,7 +53,6 @@ public Shooter() { new CANSparkMax(ShooterConstants.kBottomFlywheelMotorId, MotorType.kBrushless); m_bottomFlywheelMotor.follow(m_topFlywheelMotor, true); m_bottomFlywheelEncoder = m_bottomFlywheelMotor.getEncoder(); - m_lineBreakSensor = new DigitalInput(ShooterConstants.kLineBreakPort); // shield m_shieldController = new CANSparkMax(ShooterConstants.kShieldMotorId, MotorType.kBrushless); @@ -144,7 +140,6 @@ public void putAngleOnSmartDashboard() { @Override public void periodic() { - SmartDashboard.putBoolean("Shooter/line break", m_lineBreakSensor.get()); SmartDashboard.putNumber("Shooter/shield rots", m_shieldController.getEncoder().getPosition()); double pval = SmartDashboard.getNumber("fShooter/flywheel p", ShooterConfig.kTopFlywheelP); From cfcd8523637abb4e4bd61451595fbe4327259906 Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Wed, 20 Mar 2024 11:23:17 -0700 Subject: [PATCH 39/51] spotless --- src/main/java/frc/robot/constants/RobotConstants.java | 4 ++-- src/main/java/frc/robot/subsystems/indexer/Indexer.java | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index bad7ca0a..7dcdeebb 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -60,8 +60,8 @@ public final class ShooterConstants { public static final int kAngleMotorLeaderId = 13; public static final int kAngleMotorFollowerId = 14; public static final int kLineBreakPort = 6; - public static final double FlywheelDiameter = 0.0762; //meters - public static final double ShooterLength = 0.4064; //meters + public static final double FlywheelDiameter = 0.0762; // meters + public static final double ShooterLength = 0.4064; // meters public static final double Gravity = 9.81; public static final Measure kShieldExtentionAngle = Units.Rotations.of(1); // TODO - Set to number of rotations to fully extend shield diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index bd023a2d..9c18c538 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -2,7 +2,6 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; - import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; From 2d85d70b3b6965f55f3f02af9dc798c99535e10b Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Wed, 20 Mar 2024 17:49:52 -0700 Subject: [PATCH 40/51] only climber stuff for fewer merge conflits --- src/main/java/frc/robot/RobotContainer.java | 64 +- .../frc/robot/commands/BasicDriveCommand.java | 50 -- .../java/frc/robot/constants/RobotConfig.java | 68 --- .../frc/robot/constants/RobotConstants.java | 118 +--- .../robot/subsystems/drive/Drivetrain.java | 428 +------------ .../subsystems/drive/MAXSwerveModule.java | 172 ------ src/main/java/frc/utils/SwerveUtils.java | 117 ---- src/main/java/frc/utils/Vector.java | 562 ------------------ 8 files changed, 19 insertions(+), 1560 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/BasicDriveCommand.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java delete mode 100644 src/main/java/frc/utils/SwerveUtils.java delete mode 100644 src/main/java/frc/utils/Vector.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 75742132..ee2af2c6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,91 +1,45 @@ // 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 frc.robot; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.climber.Climb; import frc.robot.commands.climber.IndividualClimb; +import frc.robot.constants.RobotConstants.Bindings; import frc.robot.constants.RobotConstants.OIConstants; import frc.robot.subsystems.climber.Climber; -import frc.robot.subsystems.drive.Drivetrain; -import frc.utils.Vector; public class RobotContainer { private Joystick m_operatorController; - private XboxController m_driverController; private Climber m_climber; - private Drivetrain m_robotDrive; - private Vector leftInputVec; - private Vector rightInputVec; public RobotContainer() { - m_driverController = new XboxController(OIConstants.kDriverControllerPort); m_operatorController = new Joystick(OIConstants.kOperatorJoystickPort); m_climber = new Climber(); - m_robotDrive = new Drivetrain(); - - leftInputVec = new Vector(); - rightInputVec = new Vector(); - - m_robotDrive.setDefaultCommand( - // The left stick controls translation of the robot. - // Turning is controlled by the X axis of the right stick. - new RunCommand( - () -> { - // update the values of leftInputVec and rightInputVec to the values of the controller - // I'm avoiding re-instantiting Vectors to save memory - updateInput(); - m_robotDrive.drive( - leftInputVec, - rightInputVec, - m_driverController.getRightBumper(), - m_driverController.getAButton()); - }, - m_robotDrive)); configureBindings(); } private void configureBindings() { - // new Trigger(() -> m_operatorController.getRawButton(15)).whileTrue(new Climb(m_climber, - // false)); - // new Trigger(() -> m_operatorController.getRawButton(16)).whileTrue(new Climb(m_climber, - // true)); - - new Trigger(() -> m_operatorController.getRawButton(11)) + new Trigger(() -> m_operatorController.getRawButton(Bindings.kRightClimberUp)) .whileTrue(new IndividualClimb(m_climber, true, true)); - new Trigger(() -> m_operatorController.getRawButton(12)) + new Trigger(() -> m_operatorController.getRawButton(Bindings.kRightClimberDown)) .whileTrue(new IndividualClimb(m_climber, true, false)); - new Trigger(() -> m_operatorController.getRawButton(13)) + new Trigger(() -> m_operatorController.getRawButton(Bindings.kLeftClimberUp)) .whileTrue(new IndividualClimb(m_climber, false, true)); - new Trigger(() -> m_operatorController.getRawButton(14)) + new Trigger(() -> m_operatorController.getRawButton(Bindings.kLeftClimberDown)) .whileTrue(new IndividualClimb(m_climber, false, false)); - // up - new Trigger(() -> m_operatorController.getRawButton(15)).whileTrue(new Climb(m_climber, true)); - // down - new Trigger(() -> m_operatorController.getRawButton(16)).whileTrue(new Climb(m_climber, false)); - } - - private void updateInput() { - leftInputVec.setX( - MathUtil.applyDeadband(-m_driverController.getLeftY(), OIConstants.kDriveDeadband)); - leftInputVec.setY( - MathUtil.applyDeadband(-m_driverController.getLeftX(), OIConstants.kDriveDeadband)); - rightInputVec.setX( - MathUtil.applyDeadband(-m_driverController.getRightX(), OIConstants.kDriveDeadband)); - rightInputVec.setY( - MathUtil.applyDeadband(-m_driverController.getRightY(), OIConstants.kDriveDeadband)); + new Trigger(() -> m_operatorController.getRawButton(Bindings.kBothClimbersUp)) + .whileTrue(new Climb(m_climber, true)); + new Trigger(() -> m_operatorController.getRawButton(Bindings.kBothClimbersDown)) + .whileTrue(new Climb(m_climber, false)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/BasicDriveCommand.java b/src/main/java/frc/robot/commands/BasicDriveCommand.java deleted file mode 100644 index fc998f72..00000000 --- a/src/main/java/frc/robot/commands/BasicDriveCommand.java +++ /dev/null @@ -1,50 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.RobotConstants.OIConstants; -import frc.robot.subsystems.drive.Drivetrain; -import frc.utils.Vector; - -public class BasicDriveCommand extends Command { - private Drivetrain m_drive; - private XboxController m_controller; - private double m_multiplier; - - public BasicDriveCommand(Drivetrain drive, XboxController controller) { - this.m_drive = drive; - this.m_controller = controller; - - this.m_multiplier = 1; - - addRequirements(this.m_drive); - } - - @Override - public void execute() { - if (this.m_controller.getLeftTriggerAxis() != 0) { - this.m_multiplier = 2; - SmartDashboard.putBoolean("slow mode", false); - } else if (this.m_controller.getRightTriggerAxis() != 0) { - this.m_multiplier = 0.5; - SmartDashboard.putBoolean("slow mode", true); - } - - Vector lStickPos = - new Vector( - MathUtil.applyDeadband(-m_controller.getLeftY(), OIConstants.kDriveDeadband), - MathUtil.applyDeadband(-m_controller.getLeftX(), OIConstants.kDriveDeadband)); - Vector rStickPos = - new Vector( - MathUtil.applyDeadband(-m_controller.getRightX(), OIConstants.kDriveDeadband), - MathUtil.applyDeadband(-m_controller.getRightY(), OIConstants.kDriveDeadband)); - - m_drive.drive( - lStickPos.mult(m_multiplier), - rStickPos, - m_controller.getRightBumper(), - m_controller.getAButton()); - } -} diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 7d878746..44f36fa6 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -1,13 +1,8 @@ package frc.robot.constants; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; -import frc.robot.constants.RobotConstants.DriveConstants; -import frc.robot.constants.RobotConstants.SwerveModuleConstants; /** * Software config settings (e.g. max speed, PID values). For hardware constants @see @@ -23,67 +18,4 @@ public static final class ClimberConfig { public static final Measure buddyClimbExtensionDiff = Units.Meters.of(Units.Inches.of(5).in(Units.Meters)); } - - public static class DriveConfig { - public static class TranslateConfig { - public static final String kPKey = "Vision Translate P"; - public static final String kIKey = "Vision Translate I"; - public static final String kDKey = "Vision Translate D"; - public static final double kP = 0.0; - public static final double kI = 0.0; - public static final double kD = 0.0; - public static final double kTolerance = 1.0; - public static final double minIntegral = 0; - public static final double maxIntegral = 2; - } - - public static class TurnConfig { - public static final String kPKey = "Vision Turn P"; - public static final String kIKey = "Vision Turn I"; - public static final String kDKey = "Vision Turn D"; - public static final double kP = 0.0; - public static final double kI = 0.0; - public static final double kD = 0.0; - public static final double kTolerance = 1.0; - public static final double minIntegral = 0; - public static final double maxIntegral = 8; - } - - public static final String kSlewRateTranslationMagOutput = "translation magnitude output"; - public static final String kSlewRateTranslationDirRadOutput = "translation dir rad"; - - public static final HolonomicPathFollowerConfig kPathFollowerConfig = - new HolonomicPathFollowerConfig( - new PIDConstants( - SwerveModuleConstants.kDrivingP, - SwerveModuleConstants.kDrivingI, - SwerveModuleConstants.kDrivingD), - new PIDConstants( - SwerveModuleConstants.kTurningP, - SwerveModuleConstants.kTurningI, - SwerveModuleConstants.kTurningD), - SwerveModuleConstants.kMaxModuleSpeed, - DriveConstants.kWheelBaseRadius.in(Units.Meters), - new ReplanningConfig()); - - // 4.45 m/s max speed - public static final double kMaxSpeedBase = 4.8; - public static final double kMaxSpeedScaleFactor = 0.9; - public static final double kMaxSpeedMetersPerSecond = kMaxSpeedBase * kMaxSpeedScaleFactor; - - public static final double kMaxAngularSpeedBase = Math.PI; - public static final double kMaxAngularSpeedScaleFactor = 0.7; - public static final double kMaxAngularSpeed = - kMaxAngularSpeedBase * kMaxAngularSpeedScaleFactor; // radians per second - - public static final double kFrontLeftChassisAngularOffset = 0.0; - public static final double kFrontRightChassisAngularOffset = 0.0; - public static final double kBackLeftChassisAngularOffset = 0.0; - public static final double kBackRightChassisAngularOffset = 0.0; - // scaling factor for the alternative turning mode - public static final int altTurnSmoothing = 20; - public static final double HIGH_DIRECTION_SLEW_RATE = 500; - public static final double MIN_ANGLE_SLEW_RATE = 0.45 * Math.PI; - public static final double MAX_ANGLE_SLEW_RATE = 0.85 * Math.PI; - } } diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index ba5942df..90b4ee89 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -1,17 +1,19 @@ package frc.robot.constants; -import com.revrobotics.CANSparkBase.IdleMode; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.units.Distance; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.Units; - /** * Software/hardware constants (e.g. CAN IDs, gear ratios, field measurements, etc.). For software * configs @see RobotConfig */ public final class RobotConstants { + public static final class Bindings { + public static final int kLeftClimberUp = 11; + public static final int kLeftClimberDown = 12; + public static final int kRightClimberUp = 13; + public static final int kRightClimberDown = 14; + public static final int kBothClimbersUp = 11; + public static final int kBothClimbersDown = 11; + } + public static final class ClimberConstants { public static final int kClimberLeaderID = 11; public static final int kClimberFollowerID = 12; @@ -30,110 +32,8 @@ public static final class NeoMotorConstants { public static final double kFreeSpeedRpm = 5676; } - public static final class DriveConstants { - public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2; - public static final double kFrontRightChassisAngularOffset = 0; - public static final double kBackLeftChassisAngularOffset = Math.PI; - public static final double kBackRightChassisAngularOffset = Math.PI / 2; - - public static final double kDriveDeadband = 0.06; - - public static final int kFrontLeftDrivingCanId = 2; - public static final int kFrontLeftTurningCanId = 1; - - public static final int kFrontRightDrivingCanId = 6; - public static final int kFrontRightTurningCanId = 5; - - public static final int kRearLeftDrivingCanId = 4; - public static final int kRearLeftTurningCanId = 3; - - public static final int kRearRightDrivingCanId = 8; - public static final int kRearRightTurningCanId = 7; - - public static final int kGyroId = 9; - - // Chassis configuration - public static final Measure kTrackWidth = Units.Inches.of(22.5); - - // Distance between centers of right and left wheels on robot - public static final Measure kWheelBase = Units.Inches.of(22.5); - - public static final Measure kWheelBaseRadius = Units.Meters.of(0.404); - - // Distance between front and back wheels on robot - public static final SwerveDriveKinematics kDriveKinematics = - new SwerveDriveKinematics( - new Translation2d(kWheelBase.in(Units.Meters) / 2, kTrackWidth.in(Units.Meters) / 2), - new Translation2d(kWheelBase.in(Units.Meters) / 2, -kTrackWidth.in(Units.Meters) / 2), - new Translation2d(-kWheelBase.in(Units.Meters) / 2, kTrackWidth.in(Units.Meters) / 2), - new Translation2d(-kWheelBase.in(Units.Meters) / 2, -kTrackWidth.in(Units.Meters) / 2)); - } - - public static final class SwerveModuleConstants { - // The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T. - // This changes the drive speed of the module (a pinion gear with more teeth will result in a - // robot that drives faster). - public static final int kDrivingMotorPinionTeeth = 14; - - public static final double kMaxModuleSpeed = 1; - - // Invert the turning encoder, since the output shaft rotates in the opposite direction of - // the steering motor in the MAXSwerve Module. - public static final boolean kTurningEncoderInverted = true; - - // Calculations required for driving motor conversion factors and feed forward - public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60; - public static final double kWheelDiameterMeters = 0.0762; - public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; - // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the - // bevel pinion - public static final double kDrivingMotorReduction = - (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); - public static final double kDriveWheelFreeSpeedRps = - (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) / kDrivingMotorReduction; - - public static final double kDrivingEncoderPositionFactor = - (kWheelDiameterMeters * Math.PI) / kDrivingMotorReduction; // meters - public static final double kDrivingEncoderVelocityFactor = - ((kWheelDiameterMeters * Math.PI) / kDrivingMotorReduction) / 60.0; // meters per second - - public static final double kTurningEncoderPositionFactor = (2 * Math.PI); // radians - public static final double kTurningEncoderVelocityFactor = - (2 * Math.PI) / 60.0; // radians per second - - public static final double kTurningEncoderPositionPIDMinInput = 0; // radians - public static final double kTurningEncoderPositionPIDMaxInput = - kTurningEncoderPositionFactor; // radians - - public static final double kDrivingP = 0.04; - public static final double kDrivingI = 0; - public static final double kDrivingD = 0; - public static final double kDrivingFF = 1 / kDriveWheelFreeSpeedRps; - public static final double kDrivingMinOutput = -1; - public static final double kDrivingMaxOutput = 1; - - public static final double kTurningP = 1; - public static final double kTurningI = 0; - public static final double kTurningD = 0; - public static final double kTurningFF = 0; - public static final double kTurningMinOutput = -1; - public static final double kTurningMaxOutput = 1; - - public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake; - public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake; - - public static final int kDrivingMotorCurrentLimit = 50; // amps - public static final int kTurningMotorCurrentLimit = 20; // amps - } - public static final class OIConstants { public static final int kDriverControllerPort = 0; public static final int kOperatorJoystickPort = 1; - - public static final double kDriveDeadband = 0.06; - public static final double kMagnitudeDeadband = 0.06; - public static final double kDirectionSlewRate = 10; // radians per second - public static final double kMagnitudeSlewRate = 90; // percent per second (1 = 100%) - public static final double kRotationalSlewRate = 90; // percent per second (1 = 100%) } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index b6a5f18a..9d1b0c7a 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -1,431 +1,5 @@ package frc.robot.subsystems.drive; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.units.*; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.RobotConfig; -import frc.robot.constants.RobotConfig.DriveConfig; -import frc.robot.constants.RobotConstants.DriveConstants; -import frc.robot.constants.RobotConstants.OIConstants; -import frc.utils.SwerveUtils; -import frc.utils.Vector; -/** an object representing the Drivetrain of a swerve drive frc robot */ -public class Drivetrain extends SubsystemBase { - // Create MAXSwerveModules - private final MAXSwerveModule m_frontLeft; - private final MAXSwerveModule m_frontRight; - private final MAXSwerveModule m_rearLeft; - private final MAXSwerveModule m_rearRight; - - private Pigeon2 m_gyro; - - private final PowerDistribution m_powerDistribution; - - private double m_prevAngleRadians; - private double m_rightAngGoalRadians; - private double m_turnDirRadians; - - // Slew rate filter variables for controlling lateral acceleration - private double m_currentRotationRadians; - private double m_currentTranslationDirRadians; - private double m_currentTranslationMag; - - private double m_headingOffsetRadians; - - private SlewRateLimiter m_magLimiter; - private SlewRateLimiter m_rotLimiter; - private Vector spdCommanded; - - private Timer m_timer; - private double m_prevSlewRateTime; - - private MutableMeasure m_heading; - - // Odometry class for tracking robot pose - SwerveDriveOdometry m_odometry; - private Pose2d m_pose; - private ChassisSpeeds m_relativeSpeeds; - - private SwerveModulePosition[] m_swerveModulePositions; - - /** constructs a new Drivetrain object */ - public Drivetrain() { - m_frontLeft = - new MAXSwerveModule( - DriveConstants.kFrontLeftDrivingCanId, - DriveConstants.kFrontLeftTurningCanId, - DriveConstants.kFrontLeftChassisAngularOffset); - - m_frontRight = - new MAXSwerveModule( - DriveConstants.kFrontRightDrivingCanId, - DriveConstants.kFrontRightTurningCanId, - DriveConstants.kFrontRightChassisAngularOffset); - - m_rearLeft = - new MAXSwerveModule( - DriveConstants.kRearLeftDrivingCanId, - DriveConstants.kRearLeftTurningCanId, - DriveConstants.kBackLeftChassisAngularOffset); - - m_rearRight = - new MAXSwerveModule( - DriveConstants.kRearRightDrivingCanId, - DriveConstants.kRearRightTurningCanId, - DriveConstants.kBackRightChassisAngularOffset); - - // TODO: initialize this to where we place the robot on the field, will get from auto chosen - // from Smart Dashboard - m_pose = new Pose2d(); - - m_swerveModulePositions = - new SwerveModulePosition[] { - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_rearLeft.getPosition(), - m_rearRight.getPosition() - }; - - m_gyro = new Pigeon2(DriveConstants.kGyroId); - m_gyro.reset(); - - m_heading = MutableMeasure.ofBaseUnits(m_gyro.getAngle(), Units.Degrees); - - m_timer = new Timer(); - - m_powerDistribution = new PowerDistribution(); - - m_magLimiter = new SlewRateLimiter(OIConstants.kMagnitudeSlewRate); - m_rotLimiter = new SlewRateLimiter(OIConstants.kRotationalSlewRate); - spdCommanded = new Vector(); - - m_timer.start(); - m_prevSlewRateTime = m_timer.get(); - - m_odometry = - new SwerveDriveOdometry( - DriveConstants.kDriveKinematics, - Rotation2d.fromRadians(-getGyroAngle().in(Units.Radians)), - m_swerveModulePositions, - m_pose); - - configureAutoBuilder(); - - m_powerDistribution.clearStickyFaults(); - SmartDashboard.putNumber("driveVelocity", 0); - } - - /** configures the pathplanner AutoBuilder */ - private void configureAutoBuilder() { - AutoBuilder.configureHolonomic( - this::getPose, - this::resetOdometry, - this::getSpeeds, - this::driveChassisSpeeds, - RobotConfig.DriveConfig.kPathFollowerConfig, - this::allianceCheck, - this); - } - - /** - * returns the current speed of the drivetrain - * - * @return the current speed of the drivetrain - */ - public ChassisSpeeds getSpeeds() { - return m_relativeSpeeds; - } - - /** stops the drivetrain's movement */ - public void stop() { - move(Vector.Origin, 0); - } - - /** runs the periodic functionality of the drivetrain */ - @Override - public void periodic() { - m_odometry.update(m_gyro.getRotation2d(), m_swerveModulePositions); - double ang = getGyroAngle().in(Units.Radians); - SmartDashboard.putNumber("delta heading", ang - m_prevAngleRadians); - - m_prevAngleRadians = ang; - m_relativeSpeeds = getRobotRelativeSpeeds(); - m_pose = m_odometry.getPoseMeters(); - - SmartDashboard.putNumber("heading", ang - m_headingOffsetRadians); - - SmartDashboard.putNumber("right stick angle", m_rightAngGoalRadians); - SmartDashboard.putNumber("turn direction", m_turnDirRadians); - } - - /** - * Resets the pose estimator to the specified pose. - * - * @param pose The pose to which to set the estimator. - */ - public void resetOdometry(Pose2d pose) { - m_odometry.resetPosition( - Rotation2d.fromRadians(-getGyroAngle().in(Units.Radians)), - new SwerveModulePosition[] { - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_rearLeft.getPosition(), - m_rearRight.getPosition(), - }, - pose); - } - - /** - * drives the drivatrain using the given inputs the magnitude of the joystick components shouldn't - * be > 1 (x^2 + y^2 <= 1) - * - * @param xSpeed the x-pos of the left joystick (-1, 1) - * @param ySpeed the y-pos of the left joystick (-1, 1) - * @param xRot the x-pos of the right joystick (-1, 1) - * @param yRot the x-pos of the right joystick (-1, 1) - * @param altDrive whether or not to use the alternative turning mode - * @param centerGyro whether or not to reset the gyro position to the current rotation - */ - public void drive(Vector spdVec, Vector rotVec, boolean altDrive, boolean centerGyro) { - if (centerGyro) zeroHeading(); - if (altDrive) { - altDrive(spdVec, rotVec); - } else { - mainDrive(spdVec, rotVec.x()); - } - } - - /** - * moves the divetrain based on the given ChassisSpeeds - * - * @param spds the target speeds of the drivetrain chassis - */ - public void driveChassisSpeeds(ChassisSpeeds spds) { - Vector spd = new Vector(spds.vxMetersPerSecond, spds.vyMetersPerSecond); - spdCommanded = spd; - double angVel = spds.omegaRadiansPerSecond; - move(spd, angVel); - } - - /** - * moves the drivetrain using the main turning mode - * - * @param xSpeed the proportion of the robot's max velocity to move in the x direction - * @param ySpeed the proportion of the robot's max velocity to move in the y direction - * @param xRot the speed to rotate with (-1, 1) - */ - public void mainDrive(Vector spdVec, double xRot) { - double rot = xRot * DriveConfig.kMaxAngularSpeed; - move(spdVec, rot); - } - - /** - * gets the value of the robot's gyro as a Measure - * - * @see Measure - * @return the angle of the robot gyro - */ - public Measure getGyroAngle() { - return m_heading.mut_replace(m_gyro.getAngle(), Units.Degrees); - } - - /** - * moves the drivetrain using the alternative turning mode - * - * @param xSpeed the proportion of the robot's max velocity to move in the x direction - * @param ySpeed the proportion of the robot's max velocity to move in the y direction - * @param xRot the x component of the direction vector to point towards - * @param yRot the y component of the direction vector to point towards - */ - public void altDrive(Vector spdVec, Vector rotVec) { - double rot = 0; - m_rightAngGoalRadians = rotVec.angle(); - if (rotVec.squaredMag() > 0) { - double stickAng = m_rightAngGoalRadians; - // gets the difference in angle, then uses mod to make sure its from -PI rad to PI rad - rot = altTurnSmooth(stickAng); - } - m_turnDirRadians = rot; - move(spdVec, rot); - } - - /** - * returns the current speed of the robot from it's reference frame - * - * @return the current speed of the robot from it's reference frame - */ - public ChassisSpeeds getRobotRelativeSpeeds() { - return DriveConstants.kDriveKinematics.toChassisSpeeds( - new SwerveModuleState[] { - m_frontLeft.getState(), - m_frontRight.getState(), - m_rearLeft.getState(), - m_rearRight.getState() - }); - } - - /** - * applies smoothing to the turning input of altDrive - * - * @param stickAng the given angle of the driver turning stick - * @return the commanded rotation based on the rotation input - */ - private double altTurnSmooth(double stickAng) { - return Math.tanh( - ((getGyroAngle().in(Units.Radians) + stickAng + Math.PI) % (2 * Math.PI) - Math.PI) - / DriveConfig.altTurnSmoothing) - * DriveConfig.kMaxAngularSpeed; - } - - /** - * returns the current position of the robot on the field - * - * @return the current position of the robot on the field - */ - private Pose2d getPose() { - Pose2d pose = m_odometry.getPoseMeters(); - return pose; - } - - /** - * moves the drivetrain using the given values - * - * @param xSpeed the proportion of the robot's max velocity to move in the x direction - * @param ySpeed the proportion of the robot's max velocity to move in the y direction - * @param rot the angular velocity to rotate the drivetrain in radians/s - */ - public void move(Vector spdVec, double rot) { - move(spdVec, rot, true); - } - - /** - * moves the drivetrain using the given values - * - * @param xSpeed the proportion of the robot's max velocity to move in the x direction - * @param ySpeed the proportion of the robot's max velocity to move in the y direction - * @param rot the angular velocity to rotate the drivetrain in radians/s - * @param rateLimit whether or not to use slew rate limiting - */ - private void move(Vector spdVec, double rot, boolean rateLimit) { - m_currentRotationRadians = rot; - - spdCommanded.setX(spdVec.x()); - spdCommanded.setY(spdVec.y()); - - if (rateLimit) { - limitDirectionSlewRate(spdCommanded); - m_currentRotationRadians = m_rotLimiter.calculate(rot); - SmartDashboard.putNumber(DriveConfig.kSlewRateTranslationMagOutput, spdCommanded.mag()); - SmartDashboard.putNumber(DriveConfig.kSlewRateTranslationDirRadOutput, spdCommanded.angle()); - } - - // Adjust input based on max speed - spdCommanded.mult(DriveConfig.kMaxSpeedMetersPerSecond); - - double rotDelivered = m_currentRotationRadians * DriveConfig.kMaxAngularSpeed; - - var swerveModuleStates = - DriveConstants.kDriveKinematics.toSwerveModuleStates( - ChassisSpeeds.fromFieldRelativeSpeeds( - spdCommanded.x(), - spdCommanded.y(), - rotDelivered, - Rotation2d.fromDegrees(-m_gyro.getAngle()))); - SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, DriveConfig.kMaxSpeedMetersPerSecond); - m_frontLeft.setDesiredState(swerveModuleStates[0]); - m_frontRight.setDesiredState(swerveModuleStates[1]); - m_rearLeft.setDesiredState(swerveModuleStates[2]); - m_rearRight.setDesiredState(swerveModuleStates[3]); - } - - /** - * applies slewrate limiting to the given control vector - * - * @param spdVec the vector which represents the commanded speed of the drivetrain - * @return the slew rate limited Vector for controlling the drivetrain - */ - private void limitDirectionSlewRate(Vector spdVec) { - // Convert XY to polar for rate limiting - double inputTranslationDir = spdVec.angle(); - double inputTranslationMag = spdVec.mag(); - - // Calculate the direction slew rate based on an estimate of the lateral acceleration - double directionSlewRate; - // if very close to zero but not exactly zero, there is no in division by zero due to floating - // point precision errors - if (m_currentTranslationMag != 0) { - // set lower rate of change/slew rate for higher translation speeds - directionSlewRate = Math.abs(OIConstants.kDirectionSlewRate / m_currentTranslationMag); - } else { - directionSlewRate = DriveConfig.HIGH_DIRECTION_SLEW_RATE; - } - - double currentTime = m_timer.get(); - double elapsedTime = currentTime - m_prevSlewRateTime; - - double angleDif = - SwerveUtils.AngleDifference(inputTranslationDir, m_currentTranslationDirRadians); - - if (angleDif < DriveConfig.MIN_ANGLE_SLEW_RATE) { - m_currentTranslationDirRadians = - SwerveUtils.StepTowardsCircular( - m_currentTranslationDirRadians, inputTranslationDir, directionSlewRate * elapsedTime); - m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); - SmartDashboard.putNumber("translation magnitude output", inputTranslationMag); - } else if (angleDif > DriveConfig.MAX_ANGLE_SLEW_RATE) { - if (m_currentTranslationMag > 1e-4) { - m_currentTranslationMag = m_magLimiter.calculate(0.0); - } else { - m_currentTranslationDirRadians = - SwerveUtils.WrapAngle(m_currentTranslationDirRadians + Math.PI); - m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); - } - } else { - m_currentTranslationDirRadians = - SwerveUtils.StepTowardsCircular( - m_currentTranslationDirRadians, inputTranslationDir, directionSlewRate * elapsedTime); - - m_currentTranslationMag = m_magLimiter.calculate(0.0); - - m_prevSlewRateTime = currentTime; - } - - spdVec.setX(m_currentTranslationMag); - spdVec.setY(0); - spdVec.rot(m_currentTranslationDirRadians); - } - - /** - * checks whether pathplanner paths should be flipped based on the current alliance - * - * @return whether pathplanner paths should be flipped - */ - private boolean allianceCheck() { - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - } - - /** Zeroes the heading of the robot. */ - public void zeroHeading() { - m_headingOffsetRadians = getGyroAngle().in(Units.Radians); - m_gyro.reset(); - } -} +public class Drivetrain extends SubsystemBase {} diff --git a/src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java deleted file mode 100644 index 83aabc17..00000000 --- a/src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java +++ /dev/null @@ -1,172 +0,0 @@ -// 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 frc.robot.subsystems.drive; - -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAbsoluteEncoder.Type; -import com.revrobotics.SparkPIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.constants.RobotConstants.SwerveModuleConstants; - -public class MAXSwerveModule { - private final CANSparkMax m_drivingSparkMax; - private final CANSparkMax m_turningSparkMax; - - private final RelativeEncoder m_drivingEncoder; - private final AbsoluteEncoder m_turningEncoder; - - private final SparkPIDController m_drivingPIDController; - private final SparkPIDController m_turningPIDController; - - private double m_chassisAngularOffset = 0; - private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d()); - - /** - * Constructs a MAXSwerveModule and configures the driving and turning motor, encoder, and PID - * controller. This configuration is specific to the REV MAXSwerve Module built with NEOs, SPARKS - * MAX, and a Through Bore Encoder. - */ - public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) { - this.m_drivingSparkMax = new CANSparkMax(drivingCANId, MotorType.kBrushless); - this.m_turningSparkMax = new CANSparkMax(turningCANId, MotorType.kBrushless); - - // Factory reset, so we get the SPARKS MAX to a known state before configuring - // them. This is useful in case a SPARK MAX is swapped out. - this.m_drivingSparkMax.restoreFactoryDefaults(); - this.m_turningSparkMax.restoreFactoryDefaults(); - - // Setup encoders and PID controllers for the driving and turning SPARKS MAX. - this.m_drivingEncoder = this.m_drivingSparkMax.getEncoder(); - this.m_turningEncoder = this.m_turningSparkMax.getAbsoluteEncoder(Type.kDutyCycle); - this.m_drivingPIDController = this.m_drivingSparkMax.getPIDController(); - this.m_turningPIDController = this.m_turningSparkMax.getPIDController(); - this.m_drivingPIDController.setFeedbackDevice(this.m_drivingEncoder); - this.m_turningPIDController.setFeedbackDevice(this.m_turningEncoder); - - // Apply position and velocity conversion factors for the driving encoder. The - // native units for position and velocity are rotations and RPM, respectively, - // but we want meters and meters per second to use with WPILib's swerve APIs. - this.m_drivingEncoder.setPositionConversionFactor( - SwerveModuleConstants.kDrivingEncoderPositionFactor); - this.m_drivingEncoder.setVelocityConversionFactor( - SwerveModuleConstants.kDrivingEncoderVelocityFactor); - - // Apply position and velocity conversion factors for the turning encoder. We - // want these in radians and radians per second to use with WPILib's swerve - // APIs. - this.m_turningEncoder.setPositionConversionFactor( - SwerveModuleConstants.kTurningEncoderPositionFactor); - this.m_turningEncoder.setVelocityConversionFactor( - SwerveModuleConstants.kTurningEncoderVelocityFactor); - - // Invert the turning encoder, since the output shaft rotates in the opposite direction of - // the steering motor in the MAXSwerve Module. - this.m_turningEncoder.setInverted(SwerveModuleConstants.kTurningEncoderInverted); - - // Enable PID wrap around for the turning motor. This will allow the PID - // controller to go through 0 to get to the setpoint i.e. going from 350 degrees - // to 10 degrees will go through 0 rather than the other direction which is a - // longer route. - this.m_turningPIDController.setPositionPIDWrappingEnabled(true); - this.m_turningPIDController.setPositionPIDWrappingMinInput( - SwerveModuleConstants.kTurningEncoderPositionPIDMinInput); - this.m_turningPIDController.setPositionPIDWrappingMaxInput( - SwerveModuleConstants.kTurningEncoderPositionPIDMaxInput); - - // Set the PID gains for the driving motor. Note these are example gains, and you - // may need to tune them for your own robot! - this.m_drivingPIDController.setP(SwerveModuleConstants.kDrivingP); - this.m_drivingPIDController.setI(SwerveModuleConstants.kDrivingI); - this.m_drivingPIDController.setD(SwerveModuleConstants.kDrivingD); - this.m_drivingPIDController.setFF(SwerveModuleConstants.kDrivingFF); - this.m_drivingPIDController.setOutputRange( - SwerveModuleConstants.kDrivingMinOutput, SwerveModuleConstants.kDrivingMaxOutput); - - // Set the PID gains for the turning motor. Note these are example gains, and you - // may need to tune them for your own robot! - this.m_turningPIDController.setP(SwerveModuleConstants.kTurningP); - this.m_turningPIDController.setI(SwerveModuleConstants.kTurningI); - this.m_turningPIDController.setD(SwerveModuleConstants.kTurningD); - this.m_turningPIDController.setFF(SwerveModuleConstants.kTurningFF); - this.m_turningPIDController.setOutputRange( - SwerveModuleConstants.kTurningMinOutput, SwerveModuleConstants.kTurningMaxOutput); - - this.m_drivingSparkMax.setIdleMode(SwerveModuleConstants.kDrivingMotorIdleMode); - this.m_turningSparkMax.setIdleMode(SwerveModuleConstants.kTurningMotorIdleMode); - this.m_drivingSparkMax.setSmartCurrentLimit(SwerveModuleConstants.kDrivingMotorCurrentLimit); - this.m_turningSparkMax.setSmartCurrentLimit(SwerveModuleConstants.kTurningMotorCurrentLimit); - - // Save the SPARK MAX configurations. If a SPARK MAX browns out during - // operation, it will maintain the above configurations. - this.m_drivingSparkMax.burnFlash(); - this.m_turningSparkMax.burnFlash(); - - this.m_chassisAngularOffset = chassisAngularOffset; - this.m_desiredState.angle = new Rotation2d(this.m_turningEncoder.getPosition()); - this.m_drivingEncoder.setPosition(0); - } - - /** - * Returns the current state of the module. - * - * @return The current state of the module. - */ - public SwerveModuleState getState() { - // Apply chassis angular offset to the encoder position to get the position - // relative to the chassis. - return new SwerveModuleState( - this.m_drivingEncoder.getVelocity(), - new Rotation2d(this.m_turningEncoder.getPosition() - this.m_chassisAngularOffset)); - } - - /** - * Returns the current position of the module. - * - * @return The current position of the module. - */ - public SwerveModulePosition getPosition() { - // Apply chassis angular offset to the encoder position to get the position - // relative to the chassis. - return new SwerveModulePosition( - this.m_drivingEncoder.getPosition(), - new Rotation2d(this.m_turningEncoder.getPosition() - this.m_chassisAngularOffset)); - } - - /** - * Sets the desired state for the module. - * - * @param desiredState Desired state with speed and angle. - */ - public void setDesiredState(SwerveModuleState desiredState) { - // Apply chassis angular offset to the desired state. - SwerveModuleState correctedDesiredState = new SwerveModuleState(); - correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond; - correctedDesiredState.angle = - desiredState.angle.plus(Rotation2d.fromRadians(this.m_chassisAngularOffset)); - - // Optimize the reference state to avoid spinning further than 90 degrees. - SwerveModuleState optimizedDesiredState = - SwerveModuleState.optimize( - correctedDesiredState, new Rotation2d(this.m_turningEncoder.getPosition())); - - // Command driving and turning SPARKS MAX towards their respective setpoints. - this.m_drivingPIDController.setReference( - optimizedDesiredState.speedMetersPerSecond, CANSparkMax.ControlType.kVelocity); - this.m_turningPIDController.setReference( - optimizedDesiredState.angle.getRadians(), CANSparkMax.ControlType.kPosition); - - this.m_desiredState = desiredState; - } - - /** Zeroes all the SwerveModule encoders. */ - public void resetEncoders() { - this.m_drivingEncoder.setPosition(0); - } -} diff --git a/src/main/java/frc/utils/SwerveUtils.java b/src/main/java/frc/utils/SwerveUtils.java deleted file mode 100644 index 401281f8..00000000 --- a/src/main/java/frc/utils/SwerveUtils.java +++ /dev/null @@ -1,117 +0,0 @@ -package frc.utils; - -public class SwerveUtils { - - /** - * Steps a value towards a target with a specified step size. - * - * @param _current The current or starting value. Can be positive or negative. - * @param _target The target value the algorithm will step towards. Can be positive or negative. - * @param _stepsize The maximum step size that can be taken. - * @return The new value for {@code _current} after performing the specified step towards the - * specified target. - */ - public static double StepTowards(double _current, double _target, double _stepsize) { - if (Math.abs(_current - _target) <= _stepsize) { - return _target; - } else if (_target < _current) { - return _current - _stepsize; - } else { - return _current + _stepsize; - } - } - - /** - * compares to doubles with the given tolerance and returns if they are approximately equal - * - * @param a the first double to compare - * @param b the second double to compare - * @param tol the maximum difference between doubles to still be considered equal - * @return whether the doubles are approximately equal - */ - public static final boolean approxEqual(double a, double b, double tol) { - return Math.abs(a - b) <= tol; - } - - /** - * compares two doubles and returns whether they are equal within a small tolerance - * - * @param a the first double to compare - * @param b the second double to compare - * @return whether the doubles are approximately equal - */ - public static final boolean approxEqual(double a, double b) { - return approxEqual(a, b, 1e-6); - } - - /** - * Steps a value (angle) towards a target (angle) taking the shortest path with a specified step - * size. - * - * @param _current The current or starting angle (in radians). Can lie outside the 0 to 2*PI - * range. - * @param _target The target angle (in radians) the algorithm will step towards. Can lie outside - * the 0 to 2*PI range. - * @param _stepsize The maximum step size that can be taken (in radians). - * @return The new angle (in radians) for {@code _current} after performing the specified step - * towards the specified target. This value will always lie in the range 0 to 2*PI - * (exclusive). - */ - public static double StepTowardsCircular(double _current, double _target, double _stepsize) { - _current = WrapAngle(_current); - _target = WrapAngle(_target); - - double stepDirection = Math.signum(_target - _current); - double difference = Math.abs(_current - _target); - - if (difference <= _stepsize) { - return _target; - } else if (difference > Math.PI) { // does the system need to wrap over eventually? - // handle the special case where you can reach the target in one step while also wrapping - if (_current + 2 * Math.PI - _target < _stepsize - || _target + 2 * Math.PI - _current < _stepsize) { - return _target; - } else { - return WrapAngle( - _current - stepDirection * _stepsize); // this will handle wrapping gracefully - } - } else { - return _current + stepDirection * _stepsize; - } - } - - /** - * Finds the (unsigned) minimum difference between two angles including calculating across 0. - * - * @param _angleA An angle (in radians). - * @param _angleB An angle (in radians). - * @return The (unsigned) minimum difference between the two angles (in radians). - */ - public static double AngleDifference(double _angleA, double _angleB) { - double difference = Math.abs(_angleA - _angleB); - return difference > Math.PI ? (2 * Math.PI) - difference : difference; - } - - /** - * Wraps an angle until it lies within the range from 0 to 2*PI (exclusive). - * - * @param _angle The angle (in radians) to wrap. Can be positive or negative and can lie multiple - * wraps outside the output range. - * @return An angle (in radians) from 0 and 2*PI (exclusive). - */ - public static double WrapAngle(double _angle) { - double twoPi = 2 * Math.PI; - - if (_angle - == twoPi) { // Handle this case separately to avoid floating point errors with the floor - // after the division in the case below - return 0.0; - } else if (_angle > twoPi) { - return _angle - twoPi * Math.floor(_angle / twoPi); - } else if (_angle < 0.0) { - return _angle + twoPi * (Math.floor((-_angle) / twoPi) + 1); - } else { - return _angle; - } - } -} diff --git a/src/main/java/frc/utils/Vector.java b/src/main/java/frc/utils/Vector.java deleted file mode 100644 index 96da268f..00000000 --- a/src/main/java/frc/utils/Vector.java +++ /dev/null @@ -1,562 +0,0 @@ -package frc.utils; - -// a class representing either a vector or a Point in space in 2 or more dimensions with double -// precision -// with operations for manipulating them - -public class Vector { - public static final Vector Origin = new Vector(0, 0); - - // the array of values for the location of the Point, - // from lowest dimension to highest, - // ie. x-value is vals[0], y-value is vals[1], etc. - protected double[] m_vals; - - /** constructs a new 2d Vector at (0,0) */ - public Vector() { - this(0, 0); - } - - /** - * constructs a new 2d Vector at (x, y) - * - * @param x the x value for the Vector - * @param y the y value for the Vector - */ - public Vector(double x, double y) { - m_vals = new double[2]; - m_vals[0] = x; - m_vals[1] = y; - } - - /** - * constructs a new 3d Vector at (x, y, z) - * - * @param x the x value for the Vector - * @param y the y value for the Vector - * @param z the z value for the Vector - */ - public Vector(double x, double y, double z) { - m_vals = new double[3]; - m_vals[0] = x; - m_vals[1] = y; - m_vals[2] = z; - } - - /** - * constructs a new n-dimensional Vector at (vals[0], vals[1], ..., vals[n]) where n is the final - * element of vals - * - * @param vals the dimensions to use for the new Vector - */ - public Vector(double[] vals) { - if (vals.length < 2) { - throw new IllegalArgumentException("dimension counts less than 2 not supported"); - } - this.m_vals = vals; - } - - /** - * constructs a new n-dimensional Vector where all dimensions start as 0 - * - * @param dimensions the number of dimensions to construct the Vector with - */ - public Vector(int dimensions) { - if (dimensions < 2) { - throw new IllegalArgumentException("dimension counts less than 2 not supported"); - } - m_vals = new double[dimensions]; - } - - /** - * returns a Vector which is a copy of this one - * - * @return a copy of this Vector - */ - public Vector copy() { - Vector newP = new Vector(m_vals.length); - for (int i = 0; i < m_vals.length; i++) { - newP.set(i, m_vals[i]); - } - return newP; - } - - /** - * returns the number of dimensions of this Vector - * - * @return the number of dimensions of this Vector - */ - public int dims() { - return m_vals.length; - } - - @Override - /** returns a String representation of this Vector in the format (x, y, ..., n) */ - public String toString() { - String s = "(" + m_vals[0]; - for (int i = 1; i < m_vals.length; i++) { - s += "," + m_vals[i]; - } - return s + ")"; - } - - /** - * returns a new Vector using the String representation of a Vector as returned by - * Vector.toString() - * - * @param data the String representation of a vector - * @return the new Vector - */ - public static Vector fromString(String data) { - data = data.substring(1, data.length() - 1); - String[] valStrings = data.split(","); - double[] vals = new double[valStrings.length]; - for (int i = 0; i < valStrings.length; i++) vals[i] = Double.parseDouble(valStrings[i]); - return new Vector(vals); - } - - /** - * returns the magnitude of this Vector - * - * @return the magnitude of this Vector - */ - public double mag() { - double n = 0; - for (double d : m_vals) { - n += d * d; - } - return Math.sqrt(n); - } - - /** - * returns the magnitude of this Vector squared more quickly than mag() - * - * @return the magnitude of this Vector squared - */ - public double squaredMag() { - double n = 0; - for (double d : m_vals) { - n += d * d; - } - return n; - } - - /** - * returns the X component of this Vector - * - * @return the X component of this Vector - */ - public double x() { - return m_vals[0]; - } - - /** - * sets the X component of this Vector to the given value - * - * @param n the new value for the X component of this Vector - */ - public void setX(double n) { - m_vals[0] = n; - } - - /** - * returns the Y component of this Vector - * - * @return the Y component of this Vector - */ - public double y() { - return m_vals[1]; - } - - /** - * sets the Y component of this Vector to the given value - * - * @param n the new value for the Y component of this Vector - */ - public void setY(double n) { - m_vals[1] = n; - } - - /** - * returns the Z component of this Vector - * - * @return the Z component of this Vector - */ - public double z() { - if (m_vals.length < 2) - throw new IllegalStateException("z-value requires a point with at least 3 dimensions"); - return m_vals[2]; - } - - /** - * sets the Z component of this Vector to the given value - * - * @param n the new value for the Z component of this Vector - */ - public void setZ(double n) { - if (m_vals.length < 2) - throw new IllegalStateException("z-value requires a point with at least 3 dimensions"); - m_vals[2] = n; - } - - /** - * returns the value of dimension dim of this Vector where X is dimension 0, Y is dimension 1, and - * so on - * - * @param dim the dimension to be returned - * @return the value of dimension dim - */ - public double get(int dim) { - if (m_vals.length <= dim) { - return 0; - } - return m_vals[dim]; - } - - /** - * sets the value of dimension dim of this Vector to n - * - * @param dim the dimension to be set - * @param n the value to set dim to - */ - public void set(int dim, double n) { - m_vals[dim] = n; - } - - /** - * returns the distance between this Vector and Vector p - * - * @param p the Vector to get the distance to - * @return the distance between this Vector and Vector p - */ - public double dist(Vector p) { - if (p.m_vals.length != this.m_vals.length) { - throw new IllegalArgumentException( - "points to compare must have the same number of dimensions"); - } - return p.copy().sub(this).mag(); - } - - /** - * sets the magnitude of this Vector to 1 while maintaining the relative proportions of each - * dimension - * - * @return this Vector - */ - public Vector normalize() { - div(mag()); - return this; - } - - /** - * divides all the dimensions of this Vector by the given value - * - * @param d the number to divide this Vector by - * @return this Vector - */ - public Vector div(double d) { - for (int i = 0; i < m_vals.length; i++) { - m_vals[i] /= d; - } - return this; - } - - /** - * divides all the dimensions of this Vector by the given double - * - * @param d the number to multiply by - * @return this Vector - */ - public Vector mult(double d) { - for (int i = 0; i < m_vals.length; i++) { - m_vals[i] *= d; - } - return this; - } - - /** - * divides all the dimensions of this Vector by the given long value - * - * @param d the number to multiply by - * @return this Vector - */ - public Vector mult(long l) { - for (int i = 0; i < m_vals.length; i++) { - m_vals[i] *= l; - } - return this; - } - - /** - * adds all the shared dimensions of another vaector to this one - * - * @param p the Vector to be added to this one - * @return this Vector - */ - public Vector add(Vector p) { - if (p.m_vals.length > this.m_vals.length) { - throw new IllegalArgumentException( - "points to add must have the same number of dimensions or less"); - } - for (int i = 0; i < Math.min(m_vals.length, p.m_vals.length); i++) { - this.m_vals[i] += p.m_vals[i]; - } - return this; - } - - /** - * adds the given values to the x and y dimensions of this Vector - * - * @param x the value to add to the x dimension of this Vector - * @param y the value to add to the y dimension of this Vector - * @return this Vector - */ - public Vector add(double x, double y) { - m_vals[0] += x; - m_vals[1] += y; - return this; - } - - /** - * subtracts the shared dimensions of another Vector from this Vector - * - * @param p the Vector to subtract from this one - * @return this Vector - */ - public Vector sub(Vector p) { - for (int i = 0; i < Math.min(m_vals.length, p.m_vals.length); i++) { - this.m_vals[i] -= p.m_vals[i]; - } - return this; - } - - /** - * subtracts the given x and y values from the x and y dimensions of this Vector - * - * @param x the value to subtract from the x dimension of this Vector - * @param y the value to subtract from the y value of this Vector - * @return this Vector - */ - public Vector sub(double x, double y) { - m_vals[0] -= x; - m_vals[1] -= y; - return this; - } - - /** - * returns the dot product between this Vector and another if one vector is normalized, this can - * be thought of as getting the distance along that vector as an axis if both vectors are - * normalized this can be used to get the cosine of the angle between the two vectors - * - * @param p the Vector to get the dot product from - * @return this Vector - */ - public double dot(Vector p) { - if (p.m_vals.length != this.m_vals.length) { - throw new IllegalArgumentException( - "dot product requires the same number of dimensions between points"); - } - double sum = 0; - for (int i = 0; i < m_vals.length; i++) { - sum += this.m_vals[i] * p.m_vals[i]; - } - return sum; - } - - /** - * returns the dot product between this Vector and another using only the X and Y dimensions if - * one vector is normalized, this can be thought of as getting the distance along that vector as - * an axis if both vectors are normalized this can be used to get the cosine of the angle between - * the two vectors - * - * @param p the Vector to get the dot product from - * @return this Vector - */ - public double dot2d(Vector p) { - double sum = 0; - for (int i = 0; i < 2; i++) { - sum += this.m_vals[i] * p.m_vals[i]; - } - return sum; - } - - /** - * sets all of the dimensions of this Vector to their absolute value - * - * @return this Vector - */ - public Vector abs() { - for (int i = 0; i < m_vals.length; i++) { - m_vals[i] = Math.abs(m_vals[i]); - } - return this; - } - - /** - * applies a 2d linear transformation to this Vector, where the transformed location of (1, 0) is - * iHatLoc, and the transformed location of (0, 1) is jHatLoc, and is also equivalent to - * multiplying the matrix with iHatLoc and jHatLoc as it's columns by this Vector - * - * @param iHatLoc the transformed location of the x axis basis vector - * @param jHatLoc the transformed location of the y axis basis vector - * @return this Vector - */ - public Vector matrixTransform(Vector iHatLoc, Vector jHatLoc) { - double newX = y() * jHatLoc.x() + x() * iHatLoc.x(); - double newY = y() * jHatLoc.y() + x() * iHatLoc.y(); - setX(newX); - setY(newY); - return this; - } - - /** - * returns the Vector 90 degrees counter-clockwise from this one - * - * @return the Vector 90 degrees counter-clockwise from this one - */ - public Vector getPerpendicular() { - return new Vector(-y(), x()); - } - - /** - * rotates this Vector the given number of radians around the origin - * - * @param theta the number of radians to rotate this Vector around the origin - * @return this Vector - */ - public Vector rot(double theta) { - double prevX = x(); - double prevY = y(); - setX(Math.cos(theta) * prevX - Math.sin(theta) * prevY); - setY(Math.cos(theta) * prevY + Math.sin(theta) * prevX); - return this; - } - - /** - * restricts this Point to a maximum length, setting it to that length it it it longer, then - * returns itself - * - * @param maxLength the maximum length to be clamped to - * @return this Vector - */ - public Vector clampLength(double maxLength) { - return clampLength(0, maxLength); - } - - /** - * restricts this Point to a maximum length, setting it to that length it it it longer, then - * returns itself - * - * @param maxLength the maximum length to be clamped to - * @return this Vector - */ - public Vector clampLength(double minLength, double maxLength) { - if (minLength > maxLength) throw new IllegalArgumentException(); - if (this.squaredMag() < minLength * minLength) { - normalize(); - mult(minLength); - return this; - } - if (this.squaredMag() > maxLength * maxLength) { - normalize(); - mult(maxLength); - } - return this; - } - - /** - * clamps the value of a Point elementwise between the limits given by minimum and maximum Points - * such that min.X() <= max.X() and min.Y() <= max.Y() - * - * @param min the minimum value for the dimensions of this Vector - * @param max the maximum value for the dimentions of this Vector - * @return this Vector - */ - public Vector clamp(Vector min, Vector max) { - if (min.dims() != max.dims()) { - throw new IllegalArgumentException("arguments must have the same number of dimensions"); - } - for (int i = 0; i < dims() && i < min.dims(); i++) { - if (m_vals[i] < min.m_vals[i]) m_vals[i] = min.m_vals[i]; - if (m_vals[i] > max.m_vals[i]) m_vals[i] = max.m_vals[i]; - } - return this; - } - - /** - * transforms this Point into the space of Point space this Point is also multiplied by the - * magnitude of space in the process, so if you want to avoid this, normalize space first - * effectively just a matrix transformation where space is iHat and jHat is space rotated 90 - * degreed counter-clockwise - * - * @param space the vector to the space of the given Vector - * @return this Vector - */ - public Vector toSpace(Vector space) { - double oldX = x(); - double oldY = y(); - setX(oldX * space.x() + oldY * space.y()); - setY(oldY * space.x() - oldX * space.y()); - return this; - } - - /** - * returns whether this Vector is equivelent to another one in both number of dimensions and - * values of dimensions - * - * @param p the Vector to compare to - * @return whether the two Vectors are equal - */ - public boolean equals(Vector p) { - if (p.m_vals.length != this.m_vals.length) { - return false; - } - for (int i = 0; i < m_vals.length; i++) { - if (this.m_vals[i] != p.m_vals[i]) { - return false; - } - } - return true; - } - - /** - * gets the angle from the origin to this Vector in a counter-clockwise direction - * - * @return the angle from the origin to this Vector - */ - public double angle() { - return Math.atan2(y(), x()); - } - - /** - * returns whether this Vector is within the given minimum and maximum bounds - * - * @param boundsMin the minumum values of the bounds - * @param boundsMax the maximum values of the bounds - * @return whether this Vector is within the given bounds - */ - public boolean isWithinBounds(Vector boundsMin, Vector boundsMax) { - double minX; - double maxX; - double minY; - double maxY; - - if (boundsMin.x() < boundsMax.x()) { - minX = boundsMin.x(); - maxX = boundsMax.x(); - } else { - maxX = boundsMin.x(); - minX = boundsMax.x(); - } - - if (boundsMin.y() < boundsMax.y()) { - minY = boundsMin.y(); - maxY = boundsMax.y(); - } else { - maxY = boundsMin.y(); - minY = boundsMax.y(); - } - - return minX <= x() && x() <= maxX && minY <= y() && y() <= maxY; - } -} From d2be4d4d5f850eb12b4599ba472685878c50f175 Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Wed, 20 Mar 2024 18:09:28 -0700 Subject: [PATCH 41/51] resolved dev conflicts --- src/main/java/frc/robot/RobotContainer.java | 14 ++++++-------- src/main/java/frc/robot/constants/RobotConfig.java | 4 +--- .../java/frc/robot/constants/RobotConstants.java | 11 +++++------ .../frc/robot/subsystems/drive/Drivetrain.java | 1 - .../java/frc/robot/subsystems/intake/Intake.java | 11 ----------- 5 files changed, 12 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b7864ae6..4c817bf5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,10 +12,9 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.BasicDriveCommand; import frc.robot.commands.climber.Climb; import frc.robot.commands.climber.IndividualClimb; -import frc.robot.subsystems.climber.Climber; -import frc.robot.commands.BasicDriveCommand; import frc.robot.commands.intake.RunIntake; import frc.robot.commands.shooter.ActuateShield; import frc.robot.commands.shooter.Aim; @@ -24,13 +23,13 @@ import frc.robot.constants.RobotConfig.FieldElement; import frc.robot.constants.RobotConstants.Bindings; import frc.robot.constants.RobotConstants.DriveConstants.OIConstants; +import frc.robot.subsystems.climber.Climber; import frc.robot.subsystems.drive.Drivetrain; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; import frc.utils.Vector; - public class RobotContainer { private Joystick m_operatorController; @@ -114,8 +113,8 @@ private void configureBindings() { // extend shield new Trigger(() -> m_operatorController.getRawButton(Bindings.kRetractShield)) .onTrue(new ActuateShield(m_shooter, true)); - - new Trigger(() -> m_operatorController.getRawButton(Bindings.kRightClimberUp)) + + new Trigger(() -> m_operatorController.getRawButton(Bindings.kRightClimberUp)) .whileTrue(new IndividualClimb(m_climber, true, true)); new Trigger(() -> m_operatorController.getRawButton(Bindings.kRightClimberDown)) .whileTrue(new IndividualClimb(m_climber, true, false)); @@ -161,7 +160,7 @@ private Command doNothing() { * @see RobotConfig.IntakeConfig.Bindings.kIntakeNote */ public boolean getIntakeButton() { - return m_operatorController.getRawButton(RobotConfig.IntakeConfig.Bindings.kIntakeNoteButtonID); + return m_operatorController.getRawButton(Bindings.kIntakeNoteButtonID); } /** @@ -170,8 +169,7 @@ public boolean getIntakeButton() { * @see RobotConfig.IntakeConfig.Bindings.kReverseIntakeButtonID */ public boolean getReverseIntakeButton() { - return m_operatorController.getRawButton( - RobotConfig.IntakeConfig.Bindings.kReverseIntakeButtonID); + return m_operatorController.getRawButton(Bindings.kReverseIntakeButtonID); } public boolean triggerPressed() { diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index db64fcc5..8be3d569 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -1,6 +1,5 @@ package frc.robot.constants; - import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; @@ -12,7 +11,6 @@ import frc.robot.constants.RobotConstants.DriveConstants; import frc.robot.constants.RobotConstants.DriveConstants.SwerveModuleConstants; - /** * Software config settings (e.g. max speed, PID values). For hardware constants @see * RobotConstants" @@ -29,7 +27,7 @@ public enum FieldElement { AMP, TRAP } - + public static final class ClimberConfig { public static final double kDefaultSpeed = 0.4; public static final double kStallInput = 0.02; diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index a32d7638..553fe44e 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -23,14 +23,15 @@ public final class Bindings { public static final int kAimSpeaker = 3; public static final int kShoot = 1; public static final int kShootReverse = 7; - public static final int kAimTrap = 2; - public static final int kStowShooter = 14; + public static final int kIntakeNoteButtonID = 2; + public static final int kReverseIntakeButtonID = 8; + public static final int kStowShooter = 6; public static final int kToggleFlywheel = 5; public static final int kRetractShield = 10; public static final int kExtendShield = 9; public static final int kManualAdjustDown = 18; public static final int kManualAdjustUp = 19; - + public static final int kLeftClimberUp = 11; public static final int kLeftClimberDown = 12; public static final int kRightClimberUp = 13; @@ -51,19 +52,17 @@ public static final class VisionConstants { new Transform3d( new Translation3d(camChassisXOffset, camChassisYOffset, camChassisZOffset), new Rotation3d(0, 0, 0)); - } public static final class NeoMotorConstants { public static final double kFreeSpeedRpm = 5676; } - public static final class OIConstants { public static final int kDriverControllerPort = 0; public static final int kOperatorJoystickPort = 1; } - + public static final class ClimberConstants { public static final int kClimberLeaderID = 10; public static final int kClimberFollowerID = 11; diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index 66fa5bcf..fdcbb366 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -23,7 +23,6 @@ import frc.utils.SwerveUtils; import frc.utils.Vector; - /** an object representing the Drivetrain of a swerve drive frc robot */ public class Drivetrain extends SubsystemBase { // Create MAXSwerveModules diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index d662735a..ef90c3c4 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -2,26 +2,15 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.RobotConstants.IntakeConstants; public class Intake extends SubsystemBase { private final CANSparkMax m_intakeRollerMotor; // Intake roller motor - private final DigitalInput m_linebreak; /** Creates a new ExampleSubsystem. */ public Intake() { m_intakeRollerMotor = new CANSparkMax(IntakeConstants.kMotorID, MotorType.kBrushless); - // TODO maybe use to terminate intake command - m_linebreak = new DigitalInput(IntakeConstants.kLineBreakSensor); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - SmartDashboard.putBoolean("Intake/linebreak sensor", m_linebreak.get()); } /** From 0edbb81146556375a27a1d9d8b9d90f4a4a08238 Mon Sep 17 00:00:00 2001 From: iris Date: Thu, 21 Mar 2024 07:59:32 -0700 Subject: [PATCH 42/51] drive practice changes 3/19 --- src/main/java/frc/robot/RobotContainer.java | 5 ++--- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 4 ---- 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e759e6c0..cca66692 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.POVButton; @@ -66,7 +67,7 @@ public RobotContainer() { autoChooser = AutoBuilder.buildAutoChooser(); configureBindings(); - autoChooser.setDefaultOption("Leave Top", AutoBuilder.buildAuto("LeaveFromTop")); + autoChooser.setDefaultOption("do nothing", new PrintCommand("nothing")); SmartDashboard.putData("Auto Chooser", autoChooser); /*m_shooter.setDefaultCommand( @@ -74,8 +75,6 @@ public RobotContainer() { } private void configureBindings() { - new Trigger(() -> m_operatorController.getRawButton(11)) - .whileTrue(new RunCommand(() -> m_shooter.setBasic(), m_shooter)); // angle on 8-directional button m_autoAim = new POVButton(m_operatorController, 0); m_trapAim = new POVButton(m_operatorController, 90); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index e3fc6e45..7e428b67 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -194,10 +194,6 @@ public void setFF(double ff) { ArbFFUnits.kPercentOut); } - public void setBasic() { - m_angleMotorLeader.set(SmartDashboard.getNumber("angle pos", 0.1)); - } - public Measure getCurrentAngle() { return m_shooterAngle.mut_replace(m_angleEncoder.getPosition(), Units.Revolutions); } From bbe3eb75505d1bf3f2f79641e81bb703609d1379 Mon Sep 17 00:00:00 2001 From: Iris Date: Thu, 21 Mar 2024 13:56:45 -0700 Subject: [PATCH 43/51] Script to delete pathplanner folder on robot --- clear_pathplanner_folder.bat | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 clear_pathplanner_folder.bat diff --git a/clear_pathplanner_folder.bat b/clear_pathplanner_folder.bat new file mode 100644 index 00000000..2820e824 --- /dev/null +++ b/clear_pathplanner_folder.bat @@ -0,0 +1,11 @@ +@echo off + +set user="admin" +set hostname="roboRIO-8248-frc.local" +set autos_path="/home/lvuser/deploy/pathplanner" + +echo Connecting to %user%@%hostname% +echo -------------------------------------- +ssh %user%@%hostname% "rm -rf %autos_path%;echo %autos_path% has been cleared" +echo -------------------------------------- +pause \ No newline at end of file From 61f7b066b2668a7fd71c1cb4e97fed83b6eaf759 Mon Sep 17 00:00:00 2001 From: Irishumanoid <95321751+Irishumanoid@users.noreply.github.com> Date: Fri, 22 Mar 2024 16:20:23 -0700 Subject: [PATCH 44/51] applied spotless, resolved conflicts with dev after shooter pr --- src/main/java/frc/robot/RobotContainer.java | 4 +--- src/main/java/frc/robot/constants/RobotConstants.java | 3 +-- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index af12e201..e52a071a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.POVButton; @@ -19,7 +18,6 @@ import frc.robot.commands.climber.Climb; import frc.robot.commands.climber.IndividualClimb; import frc.robot.commands.intake.RunIntake; -import frc.robot.commands.shooter.ActuateShield; import frc.robot.commands.shooter.PivotMove; import frc.robot.commands.shooter.Shoot; import frc.robot.commands.shooter.SpinFlywheels; @@ -138,7 +136,7 @@ private void configureBindings() { .whileTrue(new Climb(m_climber, true)); new Trigger(() -> m_operatorController.getRawButton(Bindings.kBothClimbersDown)) .whileTrue(new Climb(m_climber, false)); - + new Trigger(() -> m_operatorController.getRawButton(Bindings.kStowShooter)) .whileTrue(new StowShooter(m_shooter)); diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 6bfb6fb7..b61f7782 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -25,7 +25,7 @@ public final class Bindings { public static final int kShootReverse = 7; public static final int kIntakeNoteButtonID = 2; public static final int kReverseIntakeButtonID = 6; - + public static final int kStowShooter = 8; public static final int kAimAmp = 9; public static final int kAimSpeaker = 10; @@ -36,7 +36,6 @@ public final class Bindings { public static final int kRightClimberDown = 14; public static final int kBothClimbersUp = 15; public static final int kBothClimbersDown = 16; - } public static final class VisionConstants { From 6bba5e4e73f86931707486ad9592825e78cf248e Mon Sep 17 00:00:00 2001 From: iris Date: Sat, 23 Mar 2024 12:03:08 -0700 Subject: [PATCH 45/51] climber changes --- src/main/java/frc/robot/Robot.java | 4 ++++ src/main/java/frc/robot/commands/climber/Climb.java | 5 +++++ .../java/frc/robot/commands/climber/IndividualClimb.java | 5 +++++ src/main/java/frc/robot/constants/RobotConstants.java | 4 ++-- src/main/java/frc/robot/subsystems/climber/Climber.java | 4 ++-- 5 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b68462c8..d83885dc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,6 +5,8 @@ package frc.robot; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -16,11 +18,13 @@ public class Robot extends TimedRobot { @Override public void robotInit() { m_robotContainer = new RobotContainer(); + SmartDashboard.putNumber("Match Time Left", 0); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + SmartDashboard.putNumber("Match Time Left", Timer.getMatchTime()); } @Override diff --git a/src/main/java/frc/robot/commands/climber/Climb.java b/src/main/java/frc/robot/commands/climber/Climb.java index 81b1d4a7..25717a83 100644 --- a/src/main/java/frc/robot/commands/climber/Climb.java +++ b/src/main/java/frc/robot/commands/climber/Climb.java @@ -20,6 +20,11 @@ public void initialize() { m_climber.setBoth(m_reverse); } + @Override + public void execute() { + System.out.println("both climb"); + } + @Override public void end(boolean interrupted) { m_climber.stopFollower(); diff --git a/src/main/java/frc/robot/commands/climber/IndividualClimb.java b/src/main/java/frc/robot/commands/climber/IndividualClimb.java index 1c0f9be4..3b5385f6 100644 --- a/src/main/java/frc/robot/commands/climber/IndividualClimb.java +++ b/src/main/java/frc/robot/commands/climber/IndividualClimb.java @@ -25,6 +25,11 @@ public void initialize() { } } + @Override + public void execute() { + System.out.println("one climb"); + } + @Override public void end(boolean interrupted) { if (m_isRight) { diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index b61f7782..49392f1f 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -62,8 +62,8 @@ public static final class OIConstants { } public static final class ClimberConstants { - public static final int kClimberLeaderID = 10; - public static final int kClimberFollowerID = 11; + public static final int kClimberLeaderID = 11; + public static final int kClimberFollowerID = 12; public static final double kClimberP = 0.1; public static final double kClimberI = 0; diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 4a6b1daf..7d2c984b 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -45,7 +45,7 @@ public double getLeaderEncoderPosition() { public void setBoth(boolean reverse) { multiplier = reverse ? -1 : 1; - leaderController.set(-0.5 * multiplier); + leaderController.set(0.5 * multiplier); followerController.set(0.5 * multiplier); } @@ -64,7 +64,7 @@ public void stopFollower() { public void setFollower(boolean reverse) { multiplier = reverse ? -1 : 1; - followerController.set(-0.7 * multiplier); + followerController.set(0.7 * multiplier); } public CANSparkMax getLeader() { From 19903ca9941fc182601a057fd76c6842102c7a94 Mon Sep 17 00:00:00 2001 From: iris Date: Sat, 23 Mar 2024 13:07:44 -0700 Subject: [PATCH 46/51] limit switches et al --- .../frc/robot/commands/climber/Climb.java | 6 +- .../commands/climber/IndividualClimb.java | 8 +- .../frc/robot/subsystems/climber/Climber.java | 76 +++++++++++-------- .../frc/robot/subsystems/shooter/Shooter.java | 20 ----- 4 files changed, 53 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/commands/climber/Climb.java b/src/main/java/frc/robot/commands/climber/Climb.java index 25717a83..70abcd31 100644 --- a/src/main/java/frc/robot/commands/climber/Climb.java +++ b/src/main/java/frc/robot/commands/climber/Climb.java @@ -27,13 +27,13 @@ public void execute() { @Override public void end(boolean interrupted) { - m_climber.stopFollower(); - m_climber.stopLeader(); + m_climber.stopRight(); + m_climber.stopLeft(); } @Override public boolean isFinished() { - return m_climber.getLeaderEncoderPosition() + return m_climber.getLeftEncoderPosition() > ClimberConfig.kUpperRotSoftStop - ClimberConfig.kStopMargin; } } diff --git a/src/main/java/frc/robot/commands/climber/IndividualClimb.java b/src/main/java/frc/robot/commands/climber/IndividualClimb.java index 3b5385f6..40fc28cf 100644 --- a/src/main/java/frc/robot/commands/climber/IndividualClimb.java +++ b/src/main/java/frc/robot/commands/climber/IndividualClimb.java @@ -19,9 +19,9 @@ public IndividualClimb(Climber climber, boolean isRight, boolean reverse) { @Override public void initialize() { if (m_isRight) { - m_climber.setLeader(m_reverse); + m_climber.setLeft(m_reverse); } else { - m_climber.setFollower(m_reverse); + m_climber.setRight(m_reverse); } } @@ -33,9 +33,9 @@ public void execute() { @Override public void end(boolean interrupted) { if (m_isRight) { - m_climber.stopLeader(); + m_climber.stopLeft(); } else { - m_climber.stopFollower(); + m_climber.stopRight(); } } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 7d2c984b..237d7134 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -3,6 +3,8 @@ import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.RobotConfig.ClimberConfig; @@ -10,68 +12,82 @@ public class Climber extends SubsystemBase { - private CANSparkMax leaderController; - private CANSparkMax followerController; + private CANSparkMax leftController; + private CANSparkMax rightController; private int multiplier; + private DigitalInput m_limSwitchLeft; + private DigitalInput m_limSwitchRight; public Climber() { - leaderController = new CANSparkMax(ClimberConstants.kClimberLeaderID, MotorType.kBrushless); - followerController = new CANSparkMax(ClimberConstants.kClimberFollowerID, MotorType.kBrushless); + leftController = new CANSparkMax(ClimberConstants.kClimberLeaderID, MotorType.kBrushless); + rightController = new CANSparkMax(ClimberConstants.kClimberFollowerID, MotorType.kBrushless); + + leftController.setIdleMode(IdleMode.kBrake); + rightController.setIdleMode(IdleMode.kBrake); - leaderController.setIdleMode(IdleMode.kBrake); - followerController.setIdleMode(IdleMode.kBrake); + m_limSwitchLeft = new DigitalInput(8); + m_limSwitchRight = new DigitalInput(9); multiplier = 1; - leaderController.getEncoder().setPosition(0); - followerController.getEncoder().setPosition(0); + leftController.getEncoder().setPosition(0); + rightController.getEncoder().setPosition(0); - SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); + SmartDashboard.putNumber("climber encoder rots", leftController.getEncoder().getPosition()); } @Override public void periodic() { - SmartDashboard.putNumber("climber encoder rots", leaderController.getEncoder().getPosition()); - if (leaderController.getEncoder().getPosition() < 0 - || leaderController.getEncoder().getPosition() > ClimberConfig.kUpperRotSoftStop) { - leaderController.set(0); - followerController.set(0); + SmartDashboard.putNumber("climber encoder rots", leftController.getEncoder().getPosition()); + if (leftController.getEncoder().getPosition() < 0 + || leftController.getEncoder().getPosition() > ClimberConfig.kUpperRotSoftStop) { + leftController.set(0); + rightController.set(0); + } + + if (m_limSwitchLeft.get()) { + leftController.set(0); + } + + if (m_limSwitchRight.get()) { + rightController.set(0); } } - public double getLeaderEncoderPosition() { - return leaderController.getEncoder().getPosition(); + public double getLeftEncoderPosition() { + return leftController.getEncoder().getPosition(); } + public void setBoth(boolean reverse) { multiplier = reverse ? -1 : 1; - leaderController.set(0.5 * multiplier); - followerController.set(0.5 * multiplier); + leftController.set(0.5 * multiplier); + rightController.set(0.5 * multiplier); } - public void setLeader(boolean reverse) { + public void setLeft(boolean reverse) { multiplier = reverse ? -1 : 1; - leaderController.set(0.7 * multiplier); + leftController.set(0.7 * multiplier); } - public void stopLeader() { - leaderController.set(0); + public void stopLeft() { + leftController.set(0); } - public void stopFollower() { - followerController.set(0); + public void stopRight() { + rightController.set(0); } - public void setFollower(boolean reverse) { + public void setRight(boolean reverse) { multiplier = reverse ? -1 : 1; - followerController.set(0.7 * multiplier); + rightController.set(0.7 * multiplier); } - public CANSparkMax getLeader() { - return leaderController; + public CANSparkMax getLeft() { + return leftController; } - public CANSparkMax getFollower() { - return followerController; + public CANSparkMax getRight() { + return rightController; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 8e15df4b..542ab781 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -140,26 +140,6 @@ public void putAngleOnSmartDashboard() { @Override public void periodic() { - SmartDashboard.putNumber("Shooter/shield rots", m_shieldController.getEncoder().getPosition()); - - double pval = SmartDashboard.getNumber("fShooter/flywheel p", ShooterConfig.kTopFlywheelP); - if (pval != m_topFlywheelPIDController.getP()) { - m_topFlywheelPIDController.setP(pval); - } - - double ival = SmartDashboard.getNumber("Shooter/flywheel i", ShooterConfig.kTopFlywheelI); - if (pval != m_topFlywheelPIDController.getI()) { - m_topFlywheelPIDController.setP(ival); - } - - double dval = SmartDashboard.getNumber("Shooter/flywheel d", ShooterConfig.kTopFlywheelD); - if (pval != m_topFlywheelPIDController.getD()) { - m_topFlywheelPIDController.setP(dval); - } - - SmartDashboard.putNumber("Shooter/top flywheel output", m_topFlywheelMotor.getAppliedOutput()); - SmartDashboard.putNumber( - "Shooter/bottom flywheel output", m_bottomFlywheelMotor.getAppliedOutput()); double flywheelRPM = SmartDashboard.getNumber("Shooter/Flywheel RPM", m_topFlywheelEncoder.getVelocity()); SmartDashboard.putNumber("Shooter/Flywheel RPM", m_topFlywheelEncoder.getVelocity()); From 2f8b5c0e08358a002ebf12265272eed02464dc4b Mon Sep 17 00:00:00 2001 From: iris Date: Sat, 23 Mar 2024 14:20:07 -0700 Subject: [PATCH 47/51] Fixed climber --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../frc/robot/commands/intake/RunIntake.java | 2 +- .../frc/robot/subsystems/climber/Climber.java | 17 ++++++++++------- 3 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e52a071a..248fbf7d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -98,7 +98,7 @@ private void configureBindings() { .whileTrue(new BasicDriveCommand(m_robotDrive, m_driverController)); // RunIntake constructor boolean is whether or not the intake should run reversed. - new Trigger(this::getIntakeButton).onTrue(new RunIntake(m_intake, m_indexer, false)); + new Trigger(this::getIntakeButton).whileTrue(new RunIntake(m_intake, m_indexer, false)); new Trigger(this::getReverseIntakeButton).whileTrue(new RunIntake(m_intake, m_indexer, true)); // just shoot on trigger new Trigger(() -> m_operatorController.getRawButton(Bindings.kShoot)) @@ -144,7 +144,7 @@ private void configureBindings() { .whileTrue(new PivotMove(m_shooter, 0.3)); new Trigger(() -> m_operatorController.getRawButton(Bindings.kAimAmp)) - .whileTrue(new PivotMove(m_shooter, 0.8)); + .whileTrue(new PivotMove(m_shooter, 0.69)); } private void updateInput() { diff --git a/src/main/java/frc/robot/commands/intake/RunIntake.java b/src/main/java/frc/robot/commands/intake/RunIntake.java index 37e0856e..c75ce0d7 100644 --- a/src/main/java/frc/robot/commands/intake/RunIntake.java +++ b/src/main/java/frc/robot/commands/intake/RunIntake.java @@ -33,6 +33,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return m_indexer.getLineBreak(); + return false; } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 237d7134..e234366b 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -25,8 +25,8 @@ public Climber() { leftController.setIdleMode(IdleMode.kBrake); rightController.setIdleMode(IdleMode.kBrake); - m_limSwitchLeft = new DigitalInput(8); - m_limSwitchRight = new DigitalInput(9); + m_limSwitchLeft = new DigitalInput(9); + m_limSwitchRight = new DigitalInput(8); multiplier = 1; @@ -38,20 +38,23 @@ public Climber() { @Override public void periodic() { + SmartDashboard.putBoolean("left lim switch", m_limSwitchLeft.get()); + SmartDashboard.putBoolean("right lim switch", m_limSwitchRight.get()); + SmartDashboard.putNumber("climber encoder rots", leftController.getEncoder().getPosition()); if (leftController.getEncoder().getPosition() < 0 || leftController.getEncoder().getPosition() > ClimberConfig.kUpperRotSoftStop) { - leftController.set(0); - rightController.set(0); + //leftController.set(0); + //rightController.set(0); } - if (m_limSwitchLeft.get()) { + /*if (!m_limSwitchLeft.get()) { leftController.set(0); } - if (m_limSwitchRight.get()) { + if (!m_limSwitchRight.get()) { rightController.set(0); - } + }*/ } public double getLeftEncoderPosition() { From d5e1ac273296dc8c16bb18808fa59b59f723f16e Mon Sep 17 00:00:00 2001 From: iris Date: Sat, 23 Mar 2024 14:40:19 -0700 Subject: [PATCH 48/51] applied spotless --- src/main/java/frc/robot/constants/RobotConfig.java | 2 +- src/main/java/frc/robot/subsystems/climber/Climber.java | 6 ++---- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 82f091ba..96f26a9a 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -164,7 +164,7 @@ public static class TurnConfig { new ReplanningConfig()); // 4.45 m/s max speed - public static final double kMaxSpeedBase = 6; + public static final double kMaxSpeedBase = 9; public static final double kMaxSpeedScaleFactor = 0.9; public static final double kMaxSpeedMetersPerSecond = kMaxSpeedBase * kMaxSpeedScaleFactor; diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index e234366b..d6934bbd 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -3,7 +3,6 @@ import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; - import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -44,8 +43,8 @@ public void periodic() { SmartDashboard.putNumber("climber encoder rots", leftController.getEncoder().getPosition()); if (leftController.getEncoder().getPosition() < 0 || leftController.getEncoder().getPosition() > ClimberConfig.kUpperRotSoftStop) { - //leftController.set(0); - //rightController.set(0); + // leftController.set(0); + // rightController.set(0); } /*if (!m_limSwitchLeft.get()) { @@ -61,7 +60,6 @@ public double getLeftEncoderPosition() { return leftController.getEncoder().getPosition(); } - public void setBoth(boolean reverse) { multiplier = reverse ? -1 : 1; leftController.set(0.5 * multiplier); From 798b7e7672cfa788f6f05226f3b291f903061580 Mon Sep 17 00:00:00 2001 From: iris Date: Sat, 23 Mar 2024 18:01:31 -0700 Subject: [PATCH 49/51] climber tuning --- src/main/java/frc/robot/commands/climber/Climb.java | 5 ----- .../java/frc/robot/commands/climber/IndividualClimb.java | 5 ----- src/main/java/frc/robot/subsystems/climber/Climber.java | 8 ++++---- 3 files changed, 4 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/commands/climber/Climb.java b/src/main/java/frc/robot/commands/climber/Climb.java index 70abcd31..01c333f5 100644 --- a/src/main/java/frc/robot/commands/climber/Climb.java +++ b/src/main/java/frc/robot/commands/climber/Climb.java @@ -20,11 +20,6 @@ public void initialize() { m_climber.setBoth(m_reverse); } - @Override - public void execute() { - System.out.println("both climb"); - } - @Override public void end(boolean interrupted) { m_climber.stopRight(); diff --git a/src/main/java/frc/robot/commands/climber/IndividualClimb.java b/src/main/java/frc/robot/commands/climber/IndividualClimb.java index 40fc28cf..c250d6c0 100644 --- a/src/main/java/frc/robot/commands/climber/IndividualClimb.java +++ b/src/main/java/frc/robot/commands/climber/IndividualClimb.java @@ -25,11 +25,6 @@ public void initialize() { } } - @Override - public void execute() { - System.out.println("one climb"); - } - @Override public void end(boolean interrupted) { if (m_isRight) { diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index d6934bbd..ff38788e 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -62,13 +62,13 @@ public double getLeftEncoderPosition() { public void setBoth(boolean reverse) { multiplier = reverse ? -1 : 1; - leftController.set(0.5 * multiplier); - rightController.set(0.5 * multiplier); + leftController.set(0.9 * multiplier); + rightController.set(-0.9 * multiplier); } public void setLeft(boolean reverse) { multiplier = reverse ? -1 : 1; - leftController.set(0.7 * multiplier); + leftController.set(0.9 * multiplier); } public void stopLeft() { @@ -81,7 +81,7 @@ public void stopRight() { public void setRight(boolean reverse) { multiplier = reverse ? -1 : 1; - rightController.set(0.7 * multiplier); + rightController.set(0.9 * multiplier); } public CANSparkMax getLeft() { From 28537f903487d58def9bc0d9a134ca92779d1f44 Mon Sep 17 00:00:00 2001 From: iris Date: Sat, 12 Oct 2024 15:01:52 -0700 Subject: [PATCH 50/51] working --- .../commands/VisionTranslateCommand.java | 67 ----------- .../frc/robot/commands/VisionTurnCommand.java | 61 ---------- .../robot/commands/shooter/SpinFlywheels.java | 21 +--- .../frc/robot/subsystems/vision/Vision.java | 110 ------------------ vendordeps/photonlib.json | 57 --------- 5 files changed, 3 insertions(+), 313 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/VisionTranslateCommand.java delete mode 100644 src/main/java/frc/robot/commands/VisionTurnCommand.java delete mode 100644 src/main/java/frc/robot/subsystems/vision/Vision.java delete mode 100644 vendordeps/photonlib.json diff --git a/src/main/java/frc/robot/commands/VisionTranslateCommand.java b/src/main/java/frc/robot/commands/VisionTranslateCommand.java deleted file mode 100644 index 5ad3c56c..00000000 --- a/src/main/java/frc/robot/commands/VisionTranslateCommand.java +++ /dev/null @@ -1,67 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.RobotConfig.DriveConfig.TranslateConfig; -import frc.robot.constants.RobotConstants.DriveConstants; -import frc.robot.subsystems.drive.Drivetrain; -import frc.robot.subsystems.vision.Vision; -import frc.utils.Vector; - -public class VisionTranslateCommand extends Command { - - private Vision vision; - private Drivetrain drive; - private XboxController controller; - - private PIDController forwardController; - - public VisionTranslateCommand(Vision vision, Drivetrain drive, XboxController controller) { - this.vision = vision; - this.drive = drive; - - this.controller = controller; - SmartDashboard.putNumber(TranslateConfig.kPKey, TranslateConfig.kP); - SmartDashboard.putNumber(TranslateConfig.kIKey, TranslateConfig.kI); - SmartDashboard.putNumber(TranslateConfig.kDKey, TranslateConfig.kD); - forwardController = - new PIDController(TranslateConfig.kP, TranslateConfig.kI, TranslateConfig.kD); - - addRequirements(vision, drive); - - forwardController.setIntegratorRange(TranslateConfig.minIntegral, TranslateConfig.maxIntegral); - } - - @Override - public void execute() { - double forwardSpeed = 0.0; - - if (vision.getHasTarget()) { - double range = vision.getDistToTarget(); - - forwardSpeed = forwardController.calculate(range, 0); - } - - drive.drive( - new Vector( - MathUtil.applyDeadband(controller.getLeftX(), DriveConstants.kDriveDeadband), - MathUtil.applyDeadband(controller.getRightX(), DriveConstants.kDriveDeadband)), - new Vector(MathUtil.applyDeadband(forwardSpeed, DriveConstants.kDriveDeadband), 0), - false, - false); - } - - @Override - public boolean isFinished() { - forwardController.setTolerance(TranslateConfig.kTolerance); - return forwardController.atSetpoint(); - } - - @Override - public void end(boolean interrupted) { - drive.drive(new Vector(0, 0), new Vector(0, 0), false, false); - } -} diff --git a/src/main/java/frc/robot/commands/VisionTurnCommand.java b/src/main/java/frc/robot/commands/VisionTurnCommand.java deleted file mode 100644 index 08a65cf8..00000000 --- a/src/main/java/frc/robot/commands/VisionTurnCommand.java +++ /dev/null @@ -1,61 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.RobotConfig.DriveConfig.TurnConfig; -import frc.robot.constants.RobotConstants.DriveConstants; -import frc.robot.subsystems.drive.Drivetrain; -import frc.robot.subsystems.vision.Vision; -import frc.utils.Vector; - -public class VisionTurnCommand extends Command { - - private Vision vision; - private Drivetrain drive; - private XboxController controller; - - private PIDController turnController; - - public VisionTurnCommand(Vision vision, Drivetrain drive, XboxController controller) { - this.vision = vision; - this.drive = drive; - this.controller = controller; - - addRequirements(vision, drive); - - turnController = new PIDController(TurnConfig.kP, TurnConfig.kI, TurnConfig.kD); - - // set a limit on overshoot compensation - turnController.setIntegratorRange( - TurnConfig.minIntegral, Math.toRadians(TurnConfig.maxIntegral)); - } - - @Override - public void execute() { - double rotationSpeed = 0.0; - - if (vision.getHasTarget()) { - rotationSpeed = turnController.calculate(vision.getBestTarget().getYaw(), 0); - } - - drive.drive( - new Vector( - MathUtil.applyDeadband(controller.getLeftX(), DriveConstants.kDriveDeadband), - MathUtil.applyDeadband(controller.getLeftY(), DriveConstants.kDriveDeadband)), - new Vector(MathUtil.applyDeadband(rotationSpeed, DriveConstants.kDriveDeadband), 0), - false, - false); - } - - public boolean isFinished() { - turnController.setTolerance(TurnConfig.kTolerance); - return turnController.atSetpoint(); - } - - @Override - public void end(boolean interrupted) { - drive.drive(new Vector(0, 0), new Vector(0, 0), false, false); - } -} diff --git a/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java b/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java index f233ec71..bc1b71f4 100644 --- a/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java +++ b/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java @@ -9,44 +9,29 @@ import frc.robot.constants.RobotConfig.FieldElement; import frc.robot.constants.RobotConfig.ShooterConfig; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.vision.Vision; public class SpinFlywheels extends Command { private final Shooter m_shooter; - private final Vision m_vision; private final FieldElement m_type; private double desiredVelocity; private Measure desiredAngle; public SpinFlywheels(Shooter shooter, FieldElement type) { m_shooter = shooter; - m_vision = null; m_type = type; addRequirements(m_shooter); } - public SpinFlywheels(Shooter shooter, Vision eyes) { + public SpinFlywheels(Shooter shooter) { m_shooter = shooter; - m_vision = eyes; m_type = null; - addRequirements(m_shooter, m_vision); + addRequirements(m_shooter); } @Override public void initialize() { - if (m_type == null) { - if (m_vision.getHasTarget()) { - double desiredAngle = - Units.Degrees.of(m_vision.getBestTarget().getPitch()).in(Units.Radians); - Measure> desiredVelocity = - m_shooter.calculateVelocity( - m_vision.getDistToTarget() * Math.atan(desiredAngle), - Units.Radians.of(desiredAngle)); - m_shooter.runFlywheel(m_shooter.convertToRPM(desiredVelocity.magnitude())); - } - } else { switch (m_type) { case AMP: desiredAngle = ShooterConfig.kAmpAngle; @@ -67,7 +52,7 @@ public void initialize() { } m_shooter.runFlywheel(desiredVelocity); } - } + @Override public void execute() { diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java deleted file mode 100644 index df3b98ee..00000000 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ /dev/null @@ -1,110 +0,0 @@ -package frc.robot.subsystems.vision; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.RobotConstants.VisionConstants; -import java.util.Optional; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.PhotonUtils; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -public class Vision extends SubsystemBase { - - private PhotonCamera camera; - private boolean hasTarget; - private PhotonPipelineResult result; - - private AprilTagFieldLayout aprilTagFieldLayout; - // if there's a pose estimator in the drivetrain subsystem, update it with this estimator - private PhotonPoseEstimator poseEstimator; - - public Vision() { - camera = new PhotonCamera("picam"); - - aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - - poseEstimator = - new PhotonPoseEstimator( - aprilTagFieldLayout, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - camera, - VisionConstants.robotToCam); - } - - @Override - public void periodic() { - PhotonPipelineResult result = camera.getLatestResult(); - hasTarget = result.hasTargets(); - if (hasTarget) { - this.result = result; - } - Optional currentEstPose = getEstimatedGlobalPose(); - if (currentEstPose.isPresent()) { - SmartDashboard.putNumber("vision/estimated x pos", currentEstPose.get().estimatedPose.getX()); - SmartDashboard.putNumber("vision/estimated y pos", currentEstPose.get().estimatedPose.getY()); - SmartDashboard.putNumber("vision/estimated z pos", currentEstPose.get().estimatedPose.getZ()); - } - } - - // Pose functions - - public Optional getEstimatedGlobalPose() { - if (poseEstimator != null) { - return poseEstimator.update(); - } - return null; - } - - public Pose2d getEstimatedPose2d() { - Optional estPose = poseEstimator.update(); - if (estPose.isPresent()) { - return estPoseToPose2d(estPose.get()); - } - return null; - } - - public Pose2d estPoseToPose2d(EstimatedRobotPose est) { // Converts estimated pose to pose 2d - return new Pose2d( - est.estimatedPose.getX(), - est.estimatedPose.getY(), - new Rotation2d( - est.estimatedPose.getRotation().getX(), est.estimatedPose.getRotation().getY())); - } - - public PhotonTrackedTarget getBestTarget() { - if (hasTarget) { - return result.getBestTarget(); - } else { - return null; - } - } - - public double getDistToTarget() { - return PhotonUtils.calculateDistanceToTargetMeters( - VisionConstants.kCameraHeight, - VisionConstants.kTargetHeight, - VisionConstants.kCameraPitchRadians, - Units.degreesToRadians(getBestTarget().getPitch())); - } - - public boolean getHasTarget() { - return hasTarget; - } - - public PhotonCamera getCam() { - return camera; - } - - public PhotonPoseEstimator getPoseEstimator() { - return poseEstimator; - } -} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json deleted file mode 100644 index 8b1044d3..00000000 --- a/vendordeps/photonlib.json +++ /dev/null @@ -1,57 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2024.2.6", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2024", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2024.2.6", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2024.2.6", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2024.2.6" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2024.2.6" - } - ] -} \ No newline at end of file From 364dc41b8abf88f02ca24320fd3d54f6f4eb8f9a Mon Sep 17 00:00:00 2001 From: iris Date: Sat, 12 Oct 2024 17:04:21 -0700 Subject: [PATCH 51/51] girls gen changes --- src/main/java/frc/robot/RobotContainer.java | 11 +++-- .../robot/commands/shooter/SpinFlywheels.java | 41 +++++++++---------- 2 files changed, 27 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 248fbf7d..588ed041 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -70,7 +70,12 @@ public RobotContainer() { autoChooser = AutoBuilder.buildAutoChooser(); configureBindings(); - autoChooser.setDefaultOption("Leave Top", AutoBuilder.buildAuto("LeaveFromTop")); + autoChooser.setDefaultOption("shoot and leave ", + new SequentialCommandGroup( + NamedCommands.getCommand("shootSpeaker"), + new RunCommand(() -> m_robotDrive.drive(new Vector(-0.2, 0), new Vector(), false, false), m_robotDrive).withTimeout(3))); + + autoChooser.addOption("Leave Top", AutoBuilder.buildAuto("LeaveFromTop")); SmartDashboard.putData("Auto Chooser", autoChooser); } @@ -167,7 +172,7 @@ private void registerCommands() { NamedCommands.registerCommand( "shootSpeaker", new SequentialCommandGroup( - new PivotMove(m_shooter, 0.55).withTimeout(1), + new PivotMove(m_shooter, 0.3).withTimeout(1), new SpinFlywheels(m_shooter, FieldElement.SPEAKER).withTimeout(1.5), new ParallelRaceGroup( new SpinFlywheels(m_shooter, FieldElement.SPEAKER), new Shoot(m_indexer, false)) @@ -176,7 +181,7 @@ private void registerCommands() { NamedCommands.registerCommand( "shootAmp", new SequentialCommandGroup( - new PivotMove(m_shooter, 0.3).withTimeout(1), + new PivotMove(m_shooter, 0.69).withTimeout(1), new SpinFlywheels(m_shooter, FieldElement.AMP).withTimeout(1.5), new ParallelRaceGroup( new SpinFlywheels(m_shooter, FieldElement.AMP), new Shoot(m_indexer, false)) diff --git a/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java b/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java index bc1b71f4..3154a05c 100644 --- a/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java +++ b/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java @@ -1,10 +1,8 @@ package frc.robot.commands.shooter; import edu.wpi.first.units.Angle; -import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; -import edu.wpi.first.units.Velocity; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConfig.FieldElement; import frc.robot.constants.RobotConfig.ShooterConfig; @@ -32,27 +30,26 @@ public SpinFlywheels(Shooter shooter) { @Override public void initialize() { - switch (m_type) { - case AMP: - desiredAngle = ShooterConfig.kAmpAngle; - desiredVelocity = ShooterConfig.kDefaultAmpVelocity; - break; - case SPEAKER: - desiredAngle = ShooterConfig.kSpeakerAngle; - desiredVelocity = ShooterConfig.kDefaultSpeakerVelocity; - break; - case TRAP: - desiredAngle = ShooterConfig.kTrapAngle; - desiredVelocity = ShooterConfig.kDefaultTrapVelocity; - break; - default: - desiredVelocity = 0; - desiredAngle = Units.Degrees.of(0); - break; - } - m_shooter.runFlywheel(desiredVelocity); + switch (m_type) { + case AMP: + desiredAngle = ShooterConfig.kAmpAngle; + desiredVelocity = ShooterConfig.kDefaultAmpVelocity; + break; + case SPEAKER: + desiredAngle = ShooterConfig.kSpeakerAngle; + desiredVelocity = ShooterConfig.kDefaultSpeakerVelocity; + break; + case TRAP: + desiredAngle = ShooterConfig.kTrapAngle; + desiredVelocity = ShooterConfig.kDefaultTrapVelocity; + break; + default: + desiredVelocity = 0; + desiredAngle = Units.Degrees.of(0); + break; } - + m_shooter.runFlywheel(desiredVelocity); + } @Override public void execute() {