From 2ef7ae27882c1b0f007abdc78301e1e4b32efc4b Mon Sep 17 00:00:00 2001 From: SR1899 Date: Mon, 26 Feb 2024 17:15:07 -0800 Subject: [PATCH 01/24] feat: LED subsystem --- src/main/java/frc/robot/Constants.java | 9 +++- .../frc/robot/subsystems/LEDSubsystem.java | 43 +++++++++++++++++++ 2 files changed, 51 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/subsystems/LEDSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6260daa..bd59a56 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -118,7 +118,8 @@ public static final class IntakeConstants { public static final class ShooterConstants { public static final int kTopShooterMotorPort = 35; public static final int kBottomShooterMotorPort = 20; - } + } + public static class ClimberConstants { public final static int leftForwardChannel = 0; public final static int rightForwardChannel = 0; @@ -148,4 +149,10 @@ public static final class VisionConstants { public static final double kFieldLength = 16.54175; } + public static final class LEDConstants{ + //TODO: find plz + public static final int kLEDPort = 0; + public static final int kLEDLength = 10; + } + } diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java new file mode 100644 index 0000000..f228c4b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -0,0 +1,43 @@ +// 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; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.LEDConstants; + +public class LEDSubsystem extends SubsystemBase { + private final AddressableLED m_LED = new AddressableLED(LEDConstants.kLEDPort); + private final AddressableLEDBuffer m_LEDBuffer = new AddressableLEDBuffer(LEDConstants.kLEDLength); + + /** Creates a new {@link LEDSubsystem}. */ + public LEDSubsystem() { + m_LED.setLength(LEDConstants.kLEDLength); // 29 + m_LED.setData(m_LEDBuffer); + m_LED.start(); + + // Yellow when bot turns on. + setLED(50, 50, 0); + + SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); + } + + /** + * Sets the rgb value for all LEDs. + * + * @param r Red 0-255 + * @param g Green 0-255 + * @param b Blue 0-255 + */ + public void setLED(int r, int g, int b) { + for (var i = 0; i < LEDConstants.kLEDLength; i++) { + m_LEDBuffer.setRGB(i, r, g, b); + } + m_LED.setData(m_LEDBuffer); + SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); + } +} \ No newline at end of file From 2eb69bd163d418914aee9cbe0ad62151156d9f55 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Mon, 26 Feb 2024 17:18:23 -0800 Subject: [PATCH 02/24] Changed LED port constant --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bd59a56..819df9f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -151,7 +151,7 @@ public static final class VisionConstants { public static final class LEDConstants{ //TODO: find plz - public static final int kLEDPort = 0; + public static final int kLEDPort = 9; public static final int kLEDLength = 10; } From f8bb56e1b6c624d9cbe37446d47482e29d46a4a7 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Fri, 1 Mar 2024 18:10:33 -0800 Subject: [PATCH 03/24] WIP: added a new command and method for odometry --- src/main/java/frc/robot/Constants.java | 1 - src/main/java/frc/robot/commands/SetLED.java | 47 +++++++++++++++++++ .../frc/robot/subsystems/DriveSubsystem.java | 3 ++ 3 files changed, 50 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/SetLED.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 819df9f..42b9172 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -100,7 +100,6 @@ public static final class IntakeConstants { public static final int kArmMotorID = 39; public static final int kArmEncoderChannel = 0; - // In degrees // In degrees public static final double kIntakeLoweredAngle = 0; public static final double kIntakeRaisedAngle = 194; diff --git a/src/main/java/frc/robot/commands/SetLED.java b/src/main/java/frc/robot/commands/SetLED.java new file mode 100644 index 0000000..85258bd --- /dev/null +++ b/src/main/java/frc/robot/commands/SetLED.java @@ -0,0 +1,47 @@ +// 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.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.LEDSubsystem; +import frc.robot.subsystems.VisionSubsystem; +import frc.robot.subsystems.VisionSubsystem.Measurement; + +public class SetLED extends Command { + private LEDSubsystem m_LedSubsystem; + private DriveSubsystem m_driveSubsystem = new DriveSubsystem(); + private Pose2d m_odometry = m_driveSubsystem.getOdometry(); + + /** Creates a new SetLED. */ + public SetLED(LEDSubsystem LED) { + m_LedSubsystem = LED; + addRequirements(m_LedSubsystem); + + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + + if () + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index dc822ef..50e3588 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -194,6 +194,9 @@ public void resetOdometry(Pose2d pose) { }, pose); } + public Pose2d getOdometry(){ + return m_poseEstimator.getEstimatedPosition(); + } /** Zeroes the heading of the robot. */ public void zeroHeading() { From ba563501d59b54bd650ca37481be4268ec0669bb Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Sun, 3 Mar 2024 16:36:09 -0800 Subject: [PATCH 04/24] feat: basic code for setting the LED color based on robot positions fro shooting --- src/main/java/frc/robot/Constants.java | 46 ++++++++++++---- src/main/java/frc/robot/commands/SetLED.java | 58 ++++++++++++++++---- 2 files changed, 84 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 42b9172..f07e917 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -117,17 +117,43 @@ public static final class IntakeConstants { public static final class ShooterConstants { public static final int kTopShooterMotorPort = 35; public static final int kBottomShooterMotorPort = 20; + + /** + * Created as 'good' points for the setLED command + * Coordinates are in meter based on (0,0) being the source for the red alliance + */ + //TODO: verify values, values were eyeballed using pathplanner + public static final double kTopShootX = 0.75; + public static final double kTopShootY = 6.7; + public static final double kTopShootAngle = 120; + + public static final double kMiddleShootX = 1.4; + public static final double kMiddleShootY = 5.6; + public static final double kMiddleShootAngle = 180; + + public static final double kBottomShootX = 0.75; + public static final double kBottomShootY = 4.4; + public static final double kBottomShootAngle = 240; + + /** + * the tolerance for the possible shooting positions at which the isk goes in + */ + // TODO:update maybe (currently arbitrary) + public static final double kPositionTolerance = 0.5; + public static final double kAngleTolerance = 10; + } public static class ClimberConstants { - public final static int leftForwardChannel = 0; - public final static int rightForwardChannel = 0; - public final static int leftReverseChannel = 1; - public final static int rightReverseChannel = 1; - - public final static double minPressure = 50.0; - public final static double maxPressure = 120.0; - } + public final static int leftForwardChannel = 0; + public final static int rightForwardChannel = 0; + public final static int leftReverseChannel = 1; + public final static int rightReverseChannel = 1; + + public final static double minPressure = 50.0; + public final static double maxPressure = 120.0; + } + public static final class VisionConstants { // TODO: Update cam pose relative to center of bot public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); @@ -148,8 +174,8 @@ public static final class VisionConstants { public static final double kFieldLength = 16.54175; } - public static final class LEDConstants{ - //TODO: find plz + public static final class LEDConstants { + // TODO: find plz public static final int kLEDPort = 9; public static final int kLEDLength = 10; } diff --git a/src/main/java/frc/robot/commands/SetLED.java b/src/main/java/frc/robot/commands/SetLED.java index 85258bd..2fdf30b 100644 --- a/src/main/java/frc/robot/commands/SetLED.java +++ b/src/main/java/frc/robot/commands/SetLED.java @@ -8,18 +8,26 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.LEDSubsystem; -import frc.robot.subsystems.VisionSubsystem; -import frc.robot.subsystems.VisionSubsystem.Measurement; +import frc.robot.Constants.ShooterConstants;; public class SetLED extends Command { - private LEDSubsystem m_LedSubsystem; + private LEDSubsystem m_ledSubsystem; + private DriveSubsystem m_driveSubsystem = new DriveSubsystem(); + private Pose2d m_odometry = m_driveSubsystem.getOdometry(); - + + private double[] m_pointsX = { ShooterConstants.kTopShootX, ShooterConstants.kMiddleShootX, + ShooterConstants.kBottomShootX }; + private double[] m_pointsY = { ShooterConstants.kTopShootY, ShooterConstants.kMiddleShootY, + ShooterConstants.kBottomShootY }; + private double[] m_angles = { ShooterConstants.kTopShootAngle, ShooterConstants.kMiddleShootAngle, + ShooterConstants.kBottomShootAngle }; + /** Creates a new SetLED. */ public SetLED(LEDSubsystem LED) { - m_LedSubsystem = LED; - addRequirements(m_LedSubsystem); + m_ledSubsystem = LED; + addRequirements(m_ledSubsystem); // Use addRequirements() here to declare subsystem dependencies. } @@ -27,17 +35,47 @@ public SetLED(LEDSubsystem LED) { // Called when the command is initially scheduled. @Override public void initialize() { - - if () + + for (int i = 0; i < 3; i++) { + boolean validX = false; + boolean validY = false; + boolean validA = false; + double X = m_pointsX[i]; + double Y = m_pointsY[i]; + double A = m_angles[i]; + if (X + ShooterConstants.kPositionTolerance > m_odometry.getX() + && m_odometry.getX() > X - ShooterConstants.kPositionTolerance) { + validX = true; + } + if (Y + ShooterConstants.kPositionTolerance > m_odometry.getY() + && m_odometry.getY() > Y - ShooterConstants.kPositionTolerance) { + validY = true; + break; + } + if (A + ShooterConstants.kAngleTolerance > m_odometry.getRotation().getDegrees() + && m_odometry.getRotation().getDegrees() > A - ShooterConstants.kAngleTolerance) { + validA = true; + break; + } + if (validX && validY && validA) { + m_ledSubsystem.setLED(0, 255, 0); + break; + } else { + m_ledSubsystem.setLED(255, 0, 0); + } + } + } // 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 - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + } // Returns true when the command should end. @Override From 20d6f40b0a78b638db9b8f69c4d4a719e1b36c86 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Mon, 4 Mar 2024 17:30:28 -0800 Subject: [PATCH 05/24] Feat: Blue LED Lights when Robot is in an Intake Position --- src/main/java/frc/robot/Constants.java | 26 ++++++++++ src/main/java/frc/robot/commands/SetLED.java | 54 +++++++++++++++++--- 2 files changed, 73 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f07e917..c8b9cc5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -110,6 +110,32 @@ public static final class IntakeConstants { public static final double kIntakeSpeed = 3.0; + + /** Intake Coordninates */ + // TODO: MAKE THIS AN ACTUALY VALID VALUE + TUNE VALUES + public static final double kIntakeDist = 999; + public static final double kIntakeError = 999; + public static final double KIntakeAngleTolerance = 1; + public static final double KIntakePositionTolerance = 1; + + public static final double KLeftRingX = 2.9; + public static final double KTopLeftRingY = 7; + + public static final double KMiddleLeftRingY = 5.5; + + public static final double KBottomLeftRingY = 4; + + public static final double KRightRingX = 8.3; + + public static final double KTopRightRingY = 7.43; + + public static final double KSecondTopRightRingY = 5.8; + + public static final double KMiddleRightRingY = 4; + + public static final double KSecondBottomRightRingY = 2.42; + + public static final double KBottomRightRingY = 0.75; // TODO: Tune distance sensor threshold for detecting note public static final double kDistanceSensorThreshold = 10; } diff --git a/src/main/java/frc/robot/commands/SetLED.java b/src/main/java/frc/robot/commands/SetLED.java index 2fdf30b..f45d3a3 100644 --- a/src/main/java/frc/robot/commands/SetLED.java +++ b/src/main/java/frc/robot/commands/SetLED.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.LEDSubsystem; +import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.ShooterConstants;; public class SetLED extends Command { @@ -18,9 +19,23 @@ public class SetLED extends Command { private Pose2d m_odometry = m_driveSubsystem.getOdometry(); private double[] m_pointsX = { ShooterConstants.kTopShootX, ShooterConstants.kMiddleShootX, - ShooterConstants.kBottomShootX }; + ShooterConstants.kBottomShootX, + //Intake Positions + IntakeConstants.KLeftRingX, + IntakeConstants.KRightRingX, + }; private double[] m_pointsY = { ShooterConstants.kTopShootY, ShooterConstants.kMiddleShootY, - ShooterConstants.kBottomShootY }; + ShooterConstants.kBottomShootY, IntakeConstants.KTopLeftRingY, + //Intake Positions + IntakeConstants.KTopLeftRingY, + IntakeConstants.KMiddleLeftRingY, + IntakeConstants.KBottomLeftRingY, + IntakeConstants.KTopRightRingY, + IntakeConstants.KSecondTopRightRingY, + IntakeConstants.KMiddleRightRingY, + IntakeConstants.KSecondBottomRightRingY, + IntakeConstants.KBottomRightRingY + }; private double[] m_angles = { ShooterConstants.kTopShootAngle, ShooterConstants.kMiddleShootAngle, ShooterConstants.kBottomShootAngle }; @@ -35,7 +50,8 @@ public SetLED(LEDSubsystem LED) { // Called when the command is initially scheduled. @Override public void initialize() { - + double robotX = m_odometry.getX(); + double robotY = m_odometry.getY(); for (int i = 0; i < 3; i++) { boolean validX = false; boolean validY = false; @@ -43,12 +59,12 @@ public void initialize() { double X = m_pointsX[i]; double Y = m_pointsY[i]; double A = m_angles[i]; - if (X + ShooterConstants.kPositionTolerance > m_odometry.getX() - && m_odometry.getX() > X - ShooterConstants.kPositionTolerance) { + if (X + ShooterConstants.kPositionTolerance > robotX + && robotX > X - ShooterConstants.kPositionTolerance) { validX = true; } - if (Y + ShooterConstants.kPositionTolerance > m_odometry.getY() - && m_odometry.getY() > Y - ShooterConstants.kPositionTolerance) { + if (Y + ShooterConstants.kPositionTolerance > robotY + && robotY > Y - ShooterConstants.kPositionTolerance) { validY = true; break; } @@ -65,6 +81,30 @@ public void initialize() { } } + //Lighting for Intake Positions + //Run through the next 8 positions on the list + for(int i = 3; i < 11; i++){ + //Figures out the rings x value (there is only 2 independent values per side) + double X; + if(i <= 5){ + X = m_pointsX[3]; + }else{ + X = m_pointsX[4]; + } + //Calculate distance to point + double dist = Math.hypot(robotX -X,robotY - m_pointsY[i]); + //Check if within an acceptable distance + if(dist >= IntakeConstants.kIntakeDist - IntakeConstants.KIntakePositionTolerance && + dist <= IntakeConstants.kIntakeDist + IntakeConstants.KIntakePositionTolerance ){ + //Estimates the proper angle + double estimateAngle = Math.toDegrees(Math.atan2(robotY - m_pointsY[i],robotX - X)); + //Checks if the angle is within an acceptable measure + if(Math.abs(m_odometry.getRotation().getDegrees() - estimateAngle) <= IntakeConstants.KIntakeAngleTolerance){ + m_ledSubsystem.setLED(0,0, 255); + } + } + } + } // Called every time the scheduler runs while the command is scheduled. From 01658bcfd1c40d3b21f06ee5197e58654e7edf1e Mon Sep 17 00:00:00 2001 From: SR1899 Date: Wed, 13 Mar 2024 18:59:24 -0700 Subject: [PATCH 06/24] LED william --- src/main/java/frc/robot/RobotContainer.java | 3 + src/main/java/frc/robot/commands/SetLED.java | 125 ------------------ .../frc/robot/subsystems/IntakeSubsystem.java | 15 +++ .../frc/robot/subsystems/LEDSubsystem.java | 16 ++- 4 files changed, 32 insertions(+), 127 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/SetLED.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index daaa2fc..965463f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -32,6 +32,7 @@ import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.IntakeSubsystem.ArmPosition; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.ShooterSubsystem.ShootSpeed; @@ -50,6 +51,7 @@ public class RobotContainer { private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); + private final LEDSubsystem m_ledSubsystem = new LEDSubsystem(); private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort); @@ -95,6 +97,7 @@ public RobotContainer() { // NoteOuttakeCommand(m_intakeSubsystem)))); m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement); + m_intakeSubsystem.addConsumer(m_ledSubsystem::addIntakeStatus); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); diff --git a/src/main/java/frc/robot/commands/SetLED.java b/src/main/java/frc/robot/commands/SetLED.java deleted file mode 100644 index f45d3a3..0000000 --- a/src/main/java/frc/robot/commands/SetLED.java +++ /dev/null @@ -1,125 +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.commands; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.LEDSubsystem; -import frc.robot.Constants.IntakeConstants; -import frc.robot.Constants.ShooterConstants;; - -public class SetLED extends Command { - private LEDSubsystem m_ledSubsystem; - - private DriveSubsystem m_driveSubsystem = new DriveSubsystem(); - - private Pose2d m_odometry = m_driveSubsystem.getOdometry(); - - private double[] m_pointsX = { ShooterConstants.kTopShootX, ShooterConstants.kMiddleShootX, - ShooterConstants.kBottomShootX, - //Intake Positions - IntakeConstants.KLeftRingX, - IntakeConstants.KRightRingX, - }; - private double[] m_pointsY = { ShooterConstants.kTopShootY, ShooterConstants.kMiddleShootY, - ShooterConstants.kBottomShootY, IntakeConstants.KTopLeftRingY, - //Intake Positions - IntakeConstants.KTopLeftRingY, - IntakeConstants.KMiddleLeftRingY, - IntakeConstants.KBottomLeftRingY, - IntakeConstants.KTopRightRingY, - IntakeConstants.KSecondTopRightRingY, - IntakeConstants.KMiddleRightRingY, - IntakeConstants.KSecondBottomRightRingY, - IntakeConstants.KBottomRightRingY - }; - private double[] m_angles = { ShooterConstants.kTopShootAngle, ShooterConstants.kMiddleShootAngle, - ShooterConstants.kBottomShootAngle }; - - /** Creates a new SetLED. */ - public SetLED(LEDSubsystem LED) { - m_ledSubsystem = LED; - addRequirements(m_ledSubsystem); - - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - double robotX = m_odometry.getX(); - double robotY = m_odometry.getY(); - for (int i = 0; i < 3; i++) { - boolean validX = false; - boolean validY = false; - boolean validA = false; - double X = m_pointsX[i]; - double Y = m_pointsY[i]; - double A = m_angles[i]; - if (X + ShooterConstants.kPositionTolerance > robotX - && robotX > X - ShooterConstants.kPositionTolerance) { - validX = true; - } - if (Y + ShooterConstants.kPositionTolerance > robotY - && robotY > Y - ShooterConstants.kPositionTolerance) { - validY = true; - break; - } - if (A + ShooterConstants.kAngleTolerance > m_odometry.getRotation().getDegrees() - && m_odometry.getRotation().getDegrees() > A - ShooterConstants.kAngleTolerance) { - validA = true; - break; - } - if (validX && validY && validA) { - m_ledSubsystem.setLED(0, 255, 0); - break; - } else { - m_ledSubsystem.setLED(255, 0, 0); - } - } - - //Lighting for Intake Positions - //Run through the next 8 positions on the list - for(int i = 3; i < 11; i++){ - //Figures out the rings x value (there is only 2 independent values per side) - double X; - if(i <= 5){ - X = m_pointsX[3]; - }else{ - X = m_pointsX[4]; - } - //Calculate distance to point - double dist = Math.hypot(robotX -X,robotY - m_pointsY[i]); - //Check if within an acceptable distance - if(dist >= IntakeConstants.kIntakeDist - IntakeConstants.KIntakePositionTolerance && - dist <= IntakeConstants.kIntakeDist + IntakeConstants.KIntakePositionTolerance ){ - //Estimates the proper angle - double estimateAngle = Math.toDegrees(Math.atan2(robotY - m_pointsY[i],robotX - X)); - //Checks if the angle is within an acceptable measure - if(Math.abs(m_odometry.getRotation().getDegrees() - estimateAngle) <= IntakeConstants.KIntakeAngleTolerance){ - m_ledSubsystem.setLED(0,0, 255); - } - } - } - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index e630d6d..9da64d1 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -4,6 +4,10 @@ package frc.robot.subsystems; +import java.util.ArrayList; +import java.util.List; +import java.util.function.Consumer; + import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -16,6 +20,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; +import frc.robot.subsystems.VisionSubsystem.Measurement; import frc.robot.Constants.IntakeConstants; public class IntakeSubsystem extends SubsystemBase { @@ -35,6 +40,8 @@ public class IntakeSubsystem extends SubsystemBase { private double m_intakeSpeed = 0; private double m_armSetpoint = IntakeConstants.kIntakeRaisedAngle; + private List> m_consumerList = new ArrayList<>(3); + /** Creates a new IntakeSubsystem */ public IntakeSubsystem() { if (m_distanceSensorToggle) m_distanceSensor.setAutomaticMode(true); @@ -74,6 +81,10 @@ public void setArmPosition(ArmPosition position) { } m_armPID.setSetpoint(m_armSetpoint); + + for (Consumer consumer : m_consumerList) { + consumer.accept(position); + } } public double getArmPosition() { @@ -96,6 +107,10 @@ public void stopIntake() { m_intakeSpeed = 0; } + public void addConsumer(Consumer consumer) { + m_consumerList.add(consumer); + } + /** * Gets distance from Rev 2m sensor */ diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index f228c4b..c5cbc55 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.LEDConstants; +import frc.robot.subsystems.IntakeSubsystem.ArmPosition; public class LEDSubsystem extends SubsystemBase { private final AddressableLED m_LED = new AddressableLED(LEDConstants.kLEDPort); @@ -20,8 +21,8 @@ public LEDSubsystem() { m_LED.setData(m_LEDBuffer); m_LED.start(); - // Yellow when bot turns on. - setLED(50, 50, 0); + // Blue when bot turns on. + setLED(0, 0, 255); SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); } @@ -40,4 +41,15 @@ public void setLED(int r, int g, int b) { m_LED.setData(m_LEDBuffer); SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); } + + public void addIntakeStatus(ArmPosition s){ + switch (s){ + case Extended: + setLED(0, 255, 255); + break; + case Retracted: + setLED(0, 250, 0); + break; + } + } } \ No newline at end of file From 6afeeaec501b43cced9a64806f7a688cb8403cbe Mon Sep 17 00:00:00 2001 From: ArnoUUU Date: Thu, 14 Mar 2024 22:46:34 -0700 Subject: [PATCH 07/24] feat: added shooter functionality and fixed naming --- src/main/java/frc/robot/Constants.java | 4 +-- .../frc/robot/subsystems/LEDSubsystem.java | 35 +++++++++++++++++-- .../robot/subsystems/ShooterSubsystem.java | 11 ++++-- 3 files changed, 43 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d504383..0c6c903 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -144,8 +144,8 @@ public static final class IntakeConstants { public static final class ShooterConstants { public static final int kTopShooterMotorPort = 20; public static final int kBottomShooterMotorPort = 35; - public static final double kShooterSpeedTop = 0.8; - public static final double kShooterSpeedBottom = 0.9; + public static final double kTopShooterSpeed = 0.8; + public static final double kBottomShooterSpeed = 0.9; public static final double kPreShooterSpeed = 0.4; public static final double kShooterOff = 0; diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index c5cbc55..f98a13c 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -9,13 +9,15 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.LEDConstants; +import frc.robot.Constants.ShooterConstants; import frc.robot.subsystems.IntakeSubsystem.ArmPosition; +import frc.robot.subsystems.ShooterSubsystem.ShootSpeed; public class LEDSubsystem extends SubsystemBase { private final AddressableLED m_LED = new AddressableLED(LEDConstants.kLEDPort); private final AddressableLEDBuffer m_LEDBuffer = new AddressableLEDBuffer(LEDConstants.kLEDLength); - /** Creates a new {@link LEDSubsystem}. */ + /** Creates a new {@link LEDSubsystem}. */ public LEDSubsystem() { m_LED.setLength(LEDConstants.kLEDLength); // 29 m_LED.setData(m_LEDBuffer); @@ -42,8 +44,8 @@ public void setLED(int r, int g, int b) { SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); } - public void addIntakeStatus(ArmPosition s){ - switch (s){ + public void addIntakeStatus(ArmPosition s) { + switch (s) { case Extended: setLED(0, 255, 255); break; @@ -52,4 +54,31 @@ public void addIntakeStatus(ArmPosition s){ break; } } + +//TODO: potentially streamline + public void addShooterStatus(ShootSpeed s, double tspeed, double bspeed) { + switch (s) { + case Shooting: + if (tspeed > ShooterConstants.kTopShooterSpeed && bspeed > ShooterConstants.kBottomShooterSpeed) { + setLED(0, 255, 255); + } else if (tspeed > ShooterConstants.kPreShooterSpeed && bspeed > ShooterConstants.kPreShooterSpeed) { + setLED(255, 0, 170); + } else { + setLED(255, 0, 0); + } + break; + + case Halfway: + if (tspeed > ShooterConstants.kPreShooterSpeed && bspeed > ShooterConstants.kPreShooterSpeed) { + setLED(255, 0, 170); + } else { + setLED(255, 0, 0); + } + break; + default: + setLED(255, 0, 0); + break; + + } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 3e0ccb9..8f4f82d 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -5,6 +5,9 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkBase.IdleMode; + +import java.util.function.Consumer; + import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -17,6 +20,8 @@ public class ShooterSubsystem extends SubsystemBase { CANSparkFlex m_bottom = new CANSparkFlex(ShooterConstants.kBottomShooterMotorPort, MotorType.kBrushless); CANSparkFlex m_top = new CANSparkFlex(ShooterConstants.kTopShooterMotorPort, MotorType.kBrushless); + private Consumer consumer; + private double m_topSpeed = 0; private double m_bottomSpeed = 0; @@ -39,8 +44,8 @@ public void reset() { public void setShootingSpeed(ShootSpeed speed) { switch (speed) { case Shooting: - m_topSpeed = ShooterConstants.kShooterSpeedTop; - m_bottomSpeed = ShooterConstants.kShooterSpeedBottom; + m_topSpeed = ShooterConstants.kTopShooterSpeed; + m_bottomSpeed = ShooterConstants.kBottomShooterSpeed; break; case Halfway: m_topSpeed = ShooterConstants.kPreShooterSpeed; @@ -51,6 +56,8 @@ public void setShootingSpeed(ShootSpeed speed) { m_bottomSpeed = 0.0; break; } + + consumer.accept(speed); } public double returnCurrentSpeed() { From d03cb869562b0ede5e8cbf05f87a40ad80c781da Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Fri, 15 Mar 2024 16:45:17 -0700 Subject: [PATCH 08/24] Revert "LED william" And then we're going to refactor based on commands This reverts commit 01658bcfd1c40d3b21f06ee5197e58654e7edf1e. --- src/main/java/frc/robot/RobotContainer.java | 3 - src/main/java/frc/robot/commands/SetLED.java | 125 ++++++++++++++++++ .../frc/robot/subsystems/IntakeSubsystem.java | 15 --- .../frc/robot/subsystems/LEDSubsystem.java | 45 +------ 4 files changed, 127 insertions(+), 61 deletions(-) create mode 100644 src/main/java/frc/robot/commands/SetLED.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index af63cc1..21cfc10 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,7 +36,6 @@ import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.IntakeSubsystem.ArmPosition; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.ShooterSubsystem.ShootSpeed; @@ -55,7 +54,6 @@ public class RobotContainer { private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); // private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); - private final LEDSubsystem m_ledSubsystem = new LEDSubsystem(); private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort); @@ -115,7 +113,6 @@ public RobotContainer() { // NoteOuttakeCommand(m_intakeSubsystem)))); m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement); - m_intakeSubsystem.addConsumer(m_ledSubsystem::addIntakeStatus); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); diff --git a/src/main/java/frc/robot/commands/SetLED.java b/src/main/java/frc/robot/commands/SetLED.java new file mode 100644 index 0000000..f45d3a3 --- /dev/null +++ b/src/main/java/frc/robot/commands/SetLED.java @@ -0,0 +1,125 @@ +// 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.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.LEDSubsystem; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.ShooterConstants;; + +public class SetLED extends Command { + private LEDSubsystem m_ledSubsystem; + + private DriveSubsystem m_driveSubsystem = new DriveSubsystem(); + + private Pose2d m_odometry = m_driveSubsystem.getOdometry(); + + private double[] m_pointsX = { ShooterConstants.kTopShootX, ShooterConstants.kMiddleShootX, + ShooterConstants.kBottomShootX, + //Intake Positions + IntakeConstants.KLeftRingX, + IntakeConstants.KRightRingX, + }; + private double[] m_pointsY = { ShooterConstants.kTopShootY, ShooterConstants.kMiddleShootY, + ShooterConstants.kBottomShootY, IntakeConstants.KTopLeftRingY, + //Intake Positions + IntakeConstants.KTopLeftRingY, + IntakeConstants.KMiddleLeftRingY, + IntakeConstants.KBottomLeftRingY, + IntakeConstants.KTopRightRingY, + IntakeConstants.KSecondTopRightRingY, + IntakeConstants.KMiddleRightRingY, + IntakeConstants.KSecondBottomRightRingY, + IntakeConstants.KBottomRightRingY + }; + private double[] m_angles = { ShooterConstants.kTopShootAngle, ShooterConstants.kMiddleShootAngle, + ShooterConstants.kBottomShootAngle }; + + /** Creates a new SetLED. */ + public SetLED(LEDSubsystem LED) { + m_ledSubsystem = LED; + addRequirements(m_ledSubsystem); + + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + double robotX = m_odometry.getX(); + double robotY = m_odometry.getY(); + for (int i = 0; i < 3; i++) { + boolean validX = false; + boolean validY = false; + boolean validA = false; + double X = m_pointsX[i]; + double Y = m_pointsY[i]; + double A = m_angles[i]; + if (X + ShooterConstants.kPositionTolerance > robotX + && robotX > X - ShooterConstants.kPositionTolerance) { + validX = true; + } + if (Y + ShooterConstants.kPositionTolerance > robotY + && robotY > Y - ShooterConstants.kPositionTolerance) { + validY = true; + break; + } + if (A + ShooterConstants.kAngleTolerance > m_odometry.getRotation().getDegrees() + && m_odometry.getRotation().getDegrees() > A - ShooterConstants.kAngleTolerance) { + validA = true; + break; + } + if (validX && validY && validA) { + m_ledSubsystem.setLED(0, 255, 0); + break; + } else { + m_ledSubsystem.setLED(255, 0, 0); + } + } + + //Lighting for Intake Positions + //Run through the next 8 positions on the list + for(int i = 3; i < 11; i++){ + //Figures out the rings x value (there is only 2 independent values per side) + double X; + if(i <= 5){ + X = m_pointsX[3]; + }else{ + X = m_pointsX[4]; + } + //Calculate distance to point + double dist = Math.hypot(robotX -X,robotY - m_pointsY[i]); + //Check if within an acceptable distance + if(dist >= IntakeConstants.kIntakeDist - IntakeConstants.KIntakePositionTolerance && + dist <= IntakeConstants.kIntakeDist + IntakeConstants.KIntakePositionTolerance ){ + //Estimates the proper angle + double estimateAngle = Math.toDegrees(Math.atan2(robotY - m_pointsY[i],robotX - X)); + //Checks if the angle is within an acceptable measure + if(Math.abs(m_odometry.getRotation().getDegrees() - estimateAngle) <= IntakeConstants.KIntakeAngleTolerance){ + m_ledSubsystem.setLED(0,0, 255); + } + } + } + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 1dbfc8c..1049fcf 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -4,10 +4,6 @@ package frc.robot.subsystems; -import java.util.ArrayList; -import java.util.List; -import java.util.function.Consumer; - import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -20,7 +16,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; -import frc.robot.subsystems.VisionSubsystem.Measurement; import frc.robot.Constants.IntakeConstants; public class IntakeSubsystem extends SubsystemBase { @@ -40,8 +35,6 @@ public class IntakeSubsystem extends SubsystemBase { private double m_intakeSpeed = 0; private double m_armSetpoint = IntakeConstants.kIntakeRaisedAngle; - private List> m_consumerList = new ArrayList<>(3); - /** Creates a new IntakeSubsystem */ public IntakeSubsystem() { if (m_distanceSensorToggle) m_distanceSensor.setAutomaticMode(true); @@ -81,10 +74,6 @@ public void setArmPosition(ArmPosition position) { } m_armPID.setSetpoint(m_armSetpoint); - - for (Consumer consumer : m_consumerList) { - consumer.accept(position); - } } public double getArmPosition() { @@ -107,10 +96,6 @@ public void stopIntake() { m_intakeSpeed = 0; } - public void addConsumer(Consumer consumer) { - m_consumerList.add(consumer); - } - /** * Gets distance from Rev 2m sensor */ diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index f98a13c..8f4f94a 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -9,9 +9,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.LEDConstants; -import frc.robot.Constants.ShooterConstants; -import frc.robot.subsystems.IntakeSubsystem.ArmPosition; -import frc.robot.subsystems.ShooterSubsystem.ShootSpeed; public class LEDSubsystem extends SubsystemBase { private final AddressableLED m_LED = new AddressableLED(LEDConstants.kLEDPort); @@ -23,8 +20,8 @@ public LEDSubsystem() { m_LED.setData(m_LEDBuffer); m_LED.start(); - // Blue when bot turns on. - setLED(0, 0, 255); + // Yellow when bot turns on. + setLED(50, 50, 0); SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); } @@ -43,42 +40,4 @@ public void setLED(int r, int g, int b) { m_LED.setData(m_LEDBuffer); SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); } - - public void addIntakeStatus(ArmPosition s) { - switch (s) { - case Extended: - setLED(0, 255, 255); - break; - case Retracted: - setLED(0, 250, 0); - break; - } - } - -//TODO: potentially streamline - public void addShooterStatus(ShootSpeed s, double tspeed, double bspeed) { - switch (s) { - case Shooting: - if (tspeed > ShooterConstants.kTopShooterSpeed && bspeed > ShooterConstants.kBottomShooterSpeed) { - setLED(0, 255, 255); - } else if (tspeed > ShooterConstants.kPreShooterSpeed && bspeed > ShooterConstants.kPreShooterSpeed) { - setLED(255, 0, 170); - } else { - setLED(255, 0, 0); - } - break; - - case Halfway: - if (tspeed > ShooterConstants.kPreShooterSpeed && bspeed > ShooterConstants.kPreShooterSpeed) { - setLED(255, 0, 170); - } else { - setLED(255, 0, 0); - } - break; - default: - setLED(255, 0, 0); - break; - - } - } } \ No newline at end of file From cc052eecadf481621b0e3fca5217e89127dd3f45 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Fri, 15 Mar 2024 19:12:17 -0700 Subject: [PATCH 09/24] LED stuff --- src/main/java/frc/robot/RobotContainer.java | 7 + .../frc/robot/commands/DefaultLEDCommand.java | 79 +++++++++++ src/main/java/frc/robot/commands/SetLED.java | 125 ------------------ .../frc/robot/subsystems/LEDSubsystem.java | 27 ++++ 4 files changed, 113 insertions(+), 125 deletions(-) create mode 100644 src/main/java/frc/robot/commands/DefaultLEDCommand.java delete mode 100644 src/main/java/frc/robot/commands/SetLED.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 21cfc10..24e24d1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,7 @@ import frc.robot.Constants.DriveConstants; import frc.robot.Constants.IOConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.commands.DefaultLEDCommand; import frc.robot.commands.IntakeArmPositionCommand; import frc.robot.commands.NoteIntakeCommand; import frc.robot.commands.NoteOuttakeCommand; @@ -36,6 +37,7 @@ import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.IntakeSubsystem.ArmPosition; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.ShooterSubsystem.ShootSpeed; @@ -54,6 +56,7 @@ public class RobotContainer { private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); // private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); + private final LEDSubsystem m_ledSubsystem = new LEDSubsystem(); private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort); @@ -120,6 +123,10 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); + m_ledSubsystem.setDefaultCommand( + new DefaultLEDCommand(m_ledSubsystem, m_intakeSubsystem, m_shooterSubsystem) + ); + m_robotDrive.setDefaultCommand( new RunCommand( () -> m_robotDrive.drive( diff --git a/src/main/java/frc/robot/commands/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/DefaultLEDCommand.java new file mode 100644 index 0000000..79c0fb9 --- /dev/null +++ b/src/main/java/frc/robot/commands/DefaultLEDCommand.java @@ -0,0 +1,79 @@ +// 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.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.IntakeConstants; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LEDSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +public class DefaultLEDCommand extends Command { + private LEDSubsystem m_ledSubsystem; + private IntakeSubsystem m_intakeSubsystem; + private ShooterSubsystem m_shooterSubsystem; + + private double armSetpoint; + private double shootSpeed; + private int[] rgb = new int[3]; + + /** Creates a new SetLED. */ + public DefaultLEDCommand(LEDSubsystem LED, IntakeSubsystem intake, ShooterSubsystem shooter) { + m_ledSubsystem = LED; + m_intakeSubsystem = intake; + m_shooterSubsystem = shooter; + addRequirements(m_ledSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // default color is invalid so the subsystem will set it to rainbow + rgb[0] = 256; + rgb[1] = 256; + rgb[2] = 256; + + armSetpoint = m_intakeSubsystem.getArmPosition(); + if (armSetpoint == IntakeConstants.kIntakeLoweredAngle) { + rgb[0] = 255; + rgb[1] = 165; + rgb[2] = 0; + } else if (armSetpoint == IntakeConstants.kIntakeRaisedAngle) { + rgb[0] = 0; + rgb[1] = 0; + rgb[2] = 255; + } + + shootSpeed = m_shooterSubsystem.returnCurrentSpeed(); + if (shootSpeed > 500 && shootSpeed < 1899 * 2) {// if charging up + rgb[0] = 150; + rgb[1] = 150; + rgb[2] = 0; + } else if (shootSpeed > 1899 * 2) {// if the shooter is ready to shoot + rgb[0] = 0; + rgb[1] = 255; + rgb[2] = 0; + } + + m_ledSubsystem.setLED(rgb[0], rgb[1], rgb[2]); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/SetLED.java b/src/main/java/frc/robot/commands/SetLED.java deleted file mode 100644 index f45d3a3..0000000 --- a/src/main/java/frc/robot/commands/SetLED.java +++ /dev/null @@ -1,125 +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.commands; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.LEDSubsystem; -import frc.robot.Constants.IntakeConstants; -import frc.robot.Constants.ShooterConstants;; - -public class SetLED extends Command { - private LEDSubsystem m_ledSubsystem; - - private DriveSubsystem m_driveSubsystem = new DriveSubsystem(); - - private Pose2d m_odometry = m_driveSubsystem.getOdometry(); - - private double[] m_pointsX = { ShooterConstants.kTopShootX, ShooterConstants.kMiddleShootX, - ShooterConstants.kBottomShootX, - //Intake Positions - IntakeConstants.KLeftRingX, - IntakeConstants.KRightRingX, - }; - private double[] m_pointsY = { ShooterConstants.kTopShootY, ShooterConstants.kMiddleShootY, - ShooterConstants.kBottomShootY, IntakeConstants.KTopLeftRingY, - //Intake Positions - IntakeConstants.KTopLeftRingY, - IntakeConstants.KMiddleLeftRingY, - IntakeConstants.KBottomLeftRingY, - IntakeConstants.KTopRightRingY, - IntakeConstants.KSecondTopRightRingY, - IntakeConstants.KMiddleRightRingY, - IntakeConstants.KSecondBottomRightRingY, - IntakeConstants.KBottomRightRingY - }; - private double[] m_angles = { ShooterConstants.kTopShootAngle, ShooterConstants.kMiddleShootAngle, - ShooterConstants.kBottomShootAngle }; - - /** Creates a new SetLED. */ - public SetLED(LEDSubsystem LED) { - m_ledSubsystem = LED; - addRequirements(m_ledSubsystem); - - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - double robotX = m_odometry.getX(); - double robotY = m_odometry.getY(); - for (int i = 0; i < 3; i++) { - boolean validX = false; - boolean validY = false; - boolean validA = false; - double X = m_pointsX[i]; - double Y = m_pointsY[i]; - double A = m_angles[i]; - if (X + ShooterConstants.kPositionTolerance > robotX - && robotX > X - ShooterConstants.kPositionTolerance) { - validX = true; - } - if (Y + ShooterConstants.kPositionTolerance > robotY - && robotY > Y - ShooterConstants.kPositionTolerance) { - validY = true; - break; - } - if (A + ShooterConstants.kAngleTolerance > m_odometry.getRotation().getDegrees() - && m_odometry.getRotation().getDegrees() > A - ShooterConstants.kAngleTolerance) { - validA = true; - break; - } - if (validX && validY && validA) { - m_ledSubsystem.setLED(0, 255, 0); - break; - } else { - m_ledSubsystem.setLED(255, 0, 0); - } - } - - //Lighting for Intake Positions - //Run through the next 8 positions on the list - for(int i = 3; i < 11; i++){ - //Figures out the rings x value (there is only 2 independent values per side) - double X; - if(i <= 5){ - X = m_pointsX[3]; - }else{ - X = m_pointsX[4]; - } - //Calculate distance to point - double dist = Math.hypot(robotX -X,robotY - m_pointsY[i]); - //Check if within an acceptable distance - if(dist >= IntakeConstants.kIntakeDist - IntakeConstants.KIntakePositionTolerance && - dist <= IntakeConstants.kIntakeDist + IntakeConstants.KIntakePositionTolerance ){ - //Estimates the proper angle - double estimateAngle = Math.toDegrees(Math.atan2(robotY - m_pointsY[i],robotX - X)); - //Checks if the angle is within an acceptable measure - if(Math.abs(m_odometry.getRotation().getDegrees() - estimateAngle) <= IntakeConstants.KIntakeAngleTolerance){ - m_ledSubsystem.setLED(0,0, 255); - } - } - } - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 8f4f94a..ff0d08d 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import frc.robot.Constants.LEDConstants; public class LEDSubsystem extends SubsystemBase { @@ -34,10 +35,36 @@ public LEDSubsystem() { * @param b Blue 0-255 */ public void setLED(int r, int g, int b) { + + // If we get an invalid value just set it to a rainbow + if (r > 255 || b > 255 || g > 255) { + rainbow(); + return; + } + for (var i = 0; i < LEDConstants.kLEDLength; i++) { m_LEDBuffer.setRGB(i, r, g, b); } m_LED.setData(m_LEDBuffer); SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); } + + private void rainbow() { + int m_rainbowFirstPixelHue = 0; + // For every pixel + for (var i = 0; i < m_LEDBuffer.getLength(); i++) { + // Calculate the hue - hue is easier for rainbows because the color + // shape is a circle so only one value needs to precess + final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_LEDBuffer.getLength())) % 180; + // Set the value + m_LEDBuffer.setHSV(i, hue, 255, 128); + } + // Increase by to make the rainbow "move" + m_rainbowFirstPixelHue += 3; + // Check bounds + m_rainbowFirstPixelHue %= 180; + + m_LED.setData(m_LEDBuffer); + SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); + } } \ No newline at end of file From db7d31a83ed2fb0e118d97ed37d05478f9d52cf6 Mon Sep 17 00:00:00 2001 From: Programming Date: Mon, 18 Mar 2024 17:03:20 -0700 Subject: [PATCH 10/24] removed consumer LED --- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 8f4f82d..81e62ae 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -6,8 +6,6 @@ import com.revrobotics.CANSparkBase.IdleMode; -import java.util.function.Consumer; - import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -20,8 +18,6 @@ public class ShooterSubsystem extends SubsystemBase { CANSparkFlex m_bottom = new CANSparkFlex(ShooterConstants.kBottomShooterMotorPort, MotorType.kBrushless); CANSparkFlex m_top = new CANSparkFlex(ShooterConstants.kTopShooterMotorPort, MotorType.kBrushless); - private Consumer consumer; - private double m_topSpeed = 0; private double m_bottomSpeed = 0; @@ -56,8 +52,6 @@ public void setShootingSpeed(ShootSpeed speed) { m_bottomSpeed = 0.0; break; } - - consumer.accept(speed); } public double returnCurrentSpeed() { From f3e1067fd55d61f78a285d02fb41ea283bbeafe9 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Mon, 18 Mar 2024 17:42:35 -0700 Subject: [PATCH 11/24] fix: checks distance sensor toggle before reading value --- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 1049fcf..c52e849 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -100,10 +100,13 @@ public void stopIntake() { * Gets distance from Rev 2m sensor */ private double getDistanceSensor() { - if (m_distanceSensor.getRange() == -1) { - m_distanceSensorToggle = false; + if(m_distanceSensorToggle){ + if (m_distanceSensor.getRange() == -1) { + m_distanceSensorToggle = false; + } + return m_distanceSensor.getRange(); } - return m_distanceSensor.getRange(); + return -1; } public boolean getDistanceSensorToggle() { From 4992c074d5a34c5a25973e66dafe57e4c68f32a3 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Mon, 18 Mar 2024 17:45:30 -0700 Subject: [PATCH 12/24] chore: deleted unused constants --- src/main/java/frc/robot/Constants.java | 26 -------------------------- 1 file changed, 26 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0c6c903..45222a0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -111,32 +111,6 @@ public static final class IntakeConstants { public static final double kIntakeSpeed = 0.5; - - /** Intake Coordninates */ - // TODO: MAKE THIS AN ACTUALY VALID VALUE + TUNE VALUES - public static final double kIntakeDist = 999; - public static final double kIntakeError = 999; - public static final double KIntakeAngleTolerance = 1; - public static final double KIntakePositionTolerance = 1; - - public static final double KLeftRingX = 2.9; - public static final double KTopLeftRingY = 7; - - public static final double KMiddleLeftRingY = 5.5; - - public static final double KBottomLeftRingY = 4; - - public static final double KRightRingX = 8.3; - - public static final double KTopRightRingY = 7.43; - - public static final double KSecondTopRightRingY = 5.8; - - public static final double KMiddleRightRingY = 4; - - public static final double KSecondBottomRightRingY = 2.42; - - public static final double KBottomRightRingY = 0.75; // TODO: Tune distance sensor threshold for detecting note public static final double kDistanceSensorThreshold = 10; } From a8dc6f6a89a8767a978a9ad8369b7f6d8c320aa0 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Mon, 18 Mar 2024 17:47:42 -0700 Subject: [PATCH 13/24] chore: updated kLEDLength --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 45222a0..f39dc18 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -160,7 +160,7 @@ public static final class VisionConstants { public static final class LEDConstants { // TODO: find plz public static final int kLEDPort = 9; - public static final int kLEDLength = 10; + public static final int kLEDLength = 44; } } From a0d6586a9e110c939b5157ce711b33b98a7458e5 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Mon, 18 Mar 2024 17:49:16 -0700 Subject: [PATCH 14/24] Check for note when using LED, slowed down rainbow LED --- src/main/java/frc/robot/commands/DefaultLEDCommand.java | 4 ++-- src/main/java/frc/robot/subsystems/LEDSubsystem.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/DefaultLEDCommand.java index 79c0fb9..6d837de 100644 --- a/src/main/java/frc/robot/commands/DefaultLEDCommand.java +++ b/src/main/java/frc/robot/commands/DefaultLEDCommand.java @@ -38,9 +38,9 @@ public void initialize() { armSetpoint = m_intakeSubsystem.getArmPosition(); if (armSetpoint == IntakeConstants.kIntakeLoweredAngle) { rgb[0] = 255; - rgb[1] = 165; + rgb[1] = 0; rgb[2] = 0; - } else if (armSetpoint == IntakeConstants.kIntakeRaisedAngle) { + } else if (armSetpoint == IntakeConstants.kIntakeRaisedAngle && m_intakeSubsystem.haveNote()) { rgb[0] = 0; rgb[1] = 0; rgb[2] = 255; diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index ff0d08d..3fd93f1 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -14,6 +14,7 @@ public class LEDSubsystem extends SubsystemBase { private final AddressableLED m_LED = new AddressableLED(LEDConstants.kLEDPort); private final AddressableLEDBuffer m_LEDBuffer = new AddressableLEDBuffer(LEDConstants.kLEDLength); + private int m_rainbowFirstPixelHue = 0; /** Creates a new {@link LEDSubsystem}. */ public LEDSubsystem() { @@ -50,7 +51,6 @@ public void setLED(int r, int g, int b) { } private void rainbow() { - int m_rainbowFirstPixelHue = 0; // For every pixel for (var i = 0; i < m_LEDBuffer.getLength(); i++) { // Calculate the hue - hue is easier for rainbows because the color @@ -60,7 +60,7 @@ private void rainbow() { m_LEDBuffer.setHSV(i, hue, 255, 128); } // Increase by to make the rainbow "move" - m_rainbowFirstPixelHue += 3; + m_rainbowFirstPixelHue += 1; // Check bounds m_rainbowFirstPixelHue %= 180; From 746c1d380a8dfec3dfc057a08cb7b19bd9cb9d33 Mon Sep 17 00:00:00 2001 From: ArnoUUU Date: Tue, 19 Mar 2024 23:10:54 -0700 Subject: [PATCH 15/24] Merge branch 'main' into LED Should be ready for testing --- .../autos/0-Anywhere-Leave-StraightBack.auto | 25 ++++++++ ...s.auto => 1-AmpSide-Leave-Delayed-3s.auto} | 0 ...s.auto => 1-AmpSide-Leave-Delayed-8s.auto} | 0 ...ght-Leave-4s.auto => 1-AmpSide-Leave.auto} | 0 ...nter-Stays-8s.auto => 2-Center-Stays.auto} | 0 ...ght-Leave.auto => 2-SourceSide-Leave.auto} | 0 .../pathplanner/autos/3-Center-Stays.auto | 32 ++++------- .../pathplanner/autos/4-Center-Stays.auto | 57 +++++++++---------- .../pathplanner/paths/LeftNote-Center.path | 6 +- .../pathplanner/paths/Straight-Back.path | 52 +++++++++++++++++ src/main/java/frc/robot/Constants.java | 11 ++-- src/main/java/frc/robot/RobotContainer.java | 57 +++++++++++-------- .../frc/robot/subsystems/LEDSubsystem.java | 1 - .../robot/subsystems/ShooterSubsystem.java | 12 ++-- 14 files changed, 164 insertions(+), 89 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto rename src/main/deploy/pathplanner/autos/{1-Left-Leave-Delayed-6s.auto => 1-AmpSide-Leave-Delayed-3s.auto} (100%) rename src/main/deploy/pathplanner/autos/{1-Left-Leave-Delayed-12s.auto => 1-AmpSide-Leave-Delayed-8s.auto} (100%) rename src/main/deploy/pathplanner/autos/{1-Right-Leave-4s.auto => 1-AmpSide-Leave.auto} (100%) rename src/main/deploy/pathplanner/autos/{2-Center-Stays-8s.auto => 2-Center-Stays.auto} (100%) rename src/main/deploy/pathplanner/autos/{2-Right-Leave.auto => 2-SourceSide-Leave.auto} (100%) create mode 100644 src/main/deploy/pathplanner/paths/Straight-Back.path diff --git a/src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto b/src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto new file mode 100644 index 0000000..398d77b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.55744644822623, + "y": 6.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Straight-Back" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1-Left-Leave-Delayed-6s.auto b/src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-3s.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/1-Left-Leave-Delayed-6s.auto rename to src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-3s.auto diff --git a/src/main/deploy/pathplanner/autos/1-Left-Leave-Delayed-12s.auto b/src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-8s.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/1-Left-Leave-Delayed-12s.auto rename to src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-8s.auto diff --git a/src/main/deploy/pathplanner/autos/1-Right-Leave-4s.auto b/src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/1-Right-Leave-4s.auto rename to src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto diff --git a/src/main/deploy/pathplanner/autos/2-Center-Stays-8s.auto b/src/main/deploy/pathplanner/autos/2-Center-Stays.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/2-Center-Stays-8s.auto rename to src/main/deploy/pathplanner/autos/2-Center-Stays.auto diff --git a/src/main/deploy/pathplanner/autos/2-Right-Leave.auto b/src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/2-Right-Leave.auto rename to src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto diff --git a/src/main/deploy/pathplanner/autos/3-Center-Stays.auto b/src/main/deploy/pathplanner/autos/3-Center-Stays.auto index fe3a087..3c0c573 100644 --- a/src/main/deploy/pathplanner/autos/3-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/3-Center-Stays.auto @@ -22,16 +22,9 @@ "data": { "commands": [ { - "type": "sequential", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "CenterNote-Center" - } - } - ] + "pathName": "CenterNote-Center" } }, { @@ -39,6 +32,12 @@ "data": { "name": "Intake" } + }, + { + "type": "named", + "data": { + "name": "Spin up Shooter" + } } ] } @@ -52,7 +51,7 @@ { "type": "named", "data": { - "name": "Shoot" + "name": "Outtake" } }, { @@ -60,16 +59,9 @@ "data": { "commands": [ { - "type": "sequential", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "LeftNote-Center" - } - } - ] + "pathName": "LeftNote-Center" } }, { @@ -90,7 +82,7 @@ { "type": "named", "data": { - "name": "Shoot" + "name": "Outtake" } } ] diff --git a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto index 8ac0f0a..56651e6 100644 --- a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto @@ -17,21 +17,26 @@ "name": "Shoot" } }, + { + "type": "named", + "data": { + "name": "Intake out" + } + }, + { + "type": "deadline", + "data": { + "commands": [] + } + }, { "type": "deadline", "data": { "commands": [ { - "type": "sequential", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "CenterNote-Center" - } - } - ] + "pathName": "CenterNote-Center" } }, { @@ -39,6 +44,12 @@ "data": { "name": "Intake" } + }, + { + "type": "named", + "data": { + "name": "Spin up Shooter" + } } ] } @@ -52,7 +63,7 @@ { "type": "named", "data": { - "name": "Shoot" + "name": "Outtake" } }, { @@ -60,16 +71,9 @@ "data": { "commands": [ { - "type": "sequential", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "LeftNote-Center" - } - } - ] + "pathName": "LeftNote-Center" } }, { @@ -90,7 +94,7 @@ { "type": "named", "data": { - "name": "Shoot" + "name": "Outtake" } }, { @@ -98,16 +102,9 @@ "data": { "commands": [ { - "type": "sequential", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "RightNote-Center" - } - } - ] + "pathName": "RightNote-Center" } }, { @@ -128,7 +125,7 @@ { "type": "named", "data": { - "name": "Shoot" + "name": "Outtake" } } ] diff --git a/src/main/deploy/pathplanner/paths/LeftNote-Center.path b/src/main/deploy/pathplanner/paths/LeftNote-Center.path index d87c8f2..51473cb 100644 --- a/src/main/deploy/pathplanner/paths/LeftNote-Center.path +++ b/src/main/deploy/pathplanner/paths/LeftNote-Center.path @@ -16,15 +16,15 @@ }, { "anchor": { - "x": 2.9026396514331507, + "x": 2.75, "y": 6.5 }, "prevControl": { - "x": 2.913008302826397, + "x": 2.760368651393246, "y": 5.421660255102424 }, "nextControl": { - "x": 2.8927727086223416, + "x": 2.740133057189191, "y": 7.526162052324105 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Straight-Back.path b/src/main/deploy/pathplanner/paths/Straight-Back.path new file mode 100644 index 0000000..2eddfae --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight-Back.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.55744644822623, + "y": 6.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.557446448226229, + "y": 6.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f39dc18..b573764 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -106,8 +106,7 @@ public static final class IntakeConstants { public static final double kIntakeAmpScoringAngle = -93; // 193 - 100 (previous angle) /** Encoder offset in rotations */ - //public static final double kArmEncoderOffset = 0.3415; - public static final double kArmEncoderOffset = 0.504; + public static final double kArmEncoderOffset = 0.715; public static final double kIntakeSpeed = 0.5; @@ -118,13 +117,13 @@ public static final class IntakeConstants { public static final class ShooterConstants { public static final int kTopShooterMotorPort = 20; public static final int kBottomShooterMotorPort = 35; - public static final double kTopShooterSpeed = 0.8; - public static final double kBottomShooterSpeed = 0.9; - public static final double kPreShooterSpeed = 0.4; + public static final double kShooterSpeedTop = 0.75; + public static final double kShooterSpeedBottom = 1; + public static final double kPrepShooterSpeed = 0.6; public static final double kShooterOff = 0; public static final double kShooterOffTime = 0.04; - public static final double kShooterOnTime = 1.9; + public static final double kShooterOnTime = 1; } public static class ClimberConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 24e24d1..056da24 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,7 +11,6 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; @@ -20,7 +19,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.RepeatCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -54,7 +52,7 @@ public class RobotContainer { private final DriveSubsystem m_robotDrive = new DriveSubsystem(); private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); - // private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); + private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); private final LEDSubsystem m_ledSubsystem = new LEDSubsystem(); @@ -70,7 +68,7 @@ public RobotContainer() { NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), - new ParallelDeadlineGroup(new WaitCommand(0.8), new NoteOuttakeCommand(m_intakeSubsystem)), + new ParallelDeadlineGroup(new WaitCommand(0.25), new NoteOuttakeCommand(m_intakeSubsystem)), new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime))); NamedCommands.registerCommand("Intake", @@ -79,12 +77,24 @@ public RobotContainer() { new NoteIntakeCommand(m_intakeSubsystem), new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted))); - NamedCommands.registerCommand("Pre-Speed - 30%", - new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Halfway, 0.01)); + NamedCommands.registerCommand("Prep-Speed - 60%", + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Prep, 0.01)); + + NamedCommands.registerCommand("Spin up Shooter", + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, 0.01)); + + NamedCommands.registerCommand("Spin down Shooter", + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.01)); + + NamedCommands.registerCommand("Outtake", + new ParallelDeadlineGroup(new WaitCommand(0.25), new NoteOuttakeCommand(m_intakeSubsystem))); NamedCommands.registerCommand("Intake in", new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); + NamedCommands.registerCommand("Intake out", + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended)); + AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry, m_robotDrive::getChassisSpeeds, m_robotDrive::autonDrive, @@ -172,8 +182,9 @@ private void configureBindings() { // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); new JoystickButton(m_driverController, Button.kX.value) - .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), - new NoteOuttakeCommand(m_intakeSubsystem))) + .onTrue(new SequentialCommandGroup( + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), + new ParallelDeadlineGroup(new WaitCommand(1), new NoteOuttakeCommand(m_intakeSubsystem)))) .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime)); new JoystickButton(m_driverController, Button.kRightBumper.value) @@ -193,7 +204,7 @@ private void configureBindings() { // Outtake, Operator Controller Right Trigger new Trigger(() -> { return m_operatorController.getRightTriggerAxis() > 0.5; - }).whileTrue(new NoteOuttakeCommand(m_intakeSubsystem)); + }).whileTrue(new ParallelDeadlineGroup(new WaitCommand(1), new NoteOuttakeCommand(m_intakeSubsystem))); new Trigger(() -> { return m_operatorController.getLeftTriggerAxis() > 0.5; @@ -201,16 +212,16 @@ private void configureBindings() { .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime)); // Climber Up, Operator Controller Right Bumper + A Button - // new Trigger(() -> { - // return m_operatorController.getAButton() && m_operatorController.getRightBumper(); - // }).whileTrue(new InstantCommand(() -> m_climberSubsystem.forward())); + new Trigger(() -> { + return m_operatorController.getAButton() && m_operatorController.getRightBumper(); + }).whileTrue(new InstantCommand(() -> m_climberSubsystem.forward())); // // Climber Down, Operator Controller Right Bumper + B Button - // new Trigger(() -> { - // return m_operatorController.getBButton() && m_operatorController.getRightBumper(); - // }).whileTrue(new InstantCommand(() -> m_climberSubsystem.reverse())); + new Trigger(() -> { + return m_operatorController.getBButton() && m_operatorController.getRightBumper(); + }).whileTrue(new InstantCommand(() -> m_climberSubsystem.reverse())); - // Toggle Distance Sensor, Operatoe Controller Left Bumper + Start Button + // Toggle Distance Sensor, Operator Controller Left Bumper + Start Button new Trigger(() -> { return m_operatorController.getLeftBumper() && m_operatorController.getStartButton(); }).onTrue(new InstantCommand(() -> m_intakeSubsystem.toggleDistanceSensor())); @@ -247,13 +258,13 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); // return new SequentialCommandGroup( - // new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, 3), - // new ParallelDeadlineGroup(new WaitCommand(1.5), new NoteOuttakeCommand(m_intakeSubsystem)), - // new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.01) - // , new ParallelDeadlineGroup(new WaitCommand(20), new RepeatCommand(new InstantCommand(() -> m_robotDrive.autonDrive(new ChassisSpeeds(3, 0, 0))))) - // ); - - + // new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, 3), + // new ParallelDeadlineGroup(new WaitCommand(1.5), new + // NoteOuttakeCommand(m_intakeSubsystem)), + // new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.01) + // , new ParallelDeadlineGroup(new WaitCommand(20), new RepeatCommand(new + // InstantCommand(() -> m_robotDrive.autonDrive(new ChassisSpeeds(3, 0, 0))))) + // ); } } diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 3fd93f1..2e235d0 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; import frc.robot.Constants.LEDConstants; public class LEDSubsystem extends SubsystemBase { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 81e62ae..b8bf286 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -40,12 +40,12 @@ public void reset() { public void setShootingSpeed(ShootSpeed speed) { switch (speed) { case Shooting: - m_topSpeed = ShooterConstants.kTopShooterSpeed; - m_bottomSpeed = ShooterConstants.kBottomShooterSpeed; + m_topSpeed = ShooterConstants.kShooterSpeedTop; + m_bottomSpeed = ShooterConstants.kShooterSpeedBottom; break; - case Halfway: - m_topSpeed = ShooterConstants.kPreShooterSpeed; - m_bottomSpeed = ShooterConstants.kPreShooterSpeed; + case Prep: + m_topSpeed = ShooterConstants.kPrepShooterSpeed; + m_bottomSpeed = ShooterConstants.kPrepShooterSpeed; break; case Off: m_topSpeed = 0.0; @@ -70,7 +70,7 @@ public void periodic() { public static enum ShootSpeed { Shooting, - Halfway, + Prep, Off } } From bf5a4e4c7809a1aece331eb8fd247452f4db95a9 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Tue, 26 Mar 2024 16:49:28 -0700 Subject: [PATCH 16/24] Simulation Support --- simgui-ds.json | 4 ++++ simgui.json | 13 +++++++++++++ src/main/java/frc/robot/RobotContainer.java | 2 ++ .../java/frc/robot/subsystems/ShooterSubsystem.java | 5 +++-- 4 files changed, 22 insertions(+), 2 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3c..15cec71 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -92,6 +92,10 @@ "robotJoysticks": [ { "guid": "Keyboard0" + }, + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/simgui.json b/simgui.json index 8c63454..c639b3c 100644 --- a/simgui.json +++ b/simgui.json @@ -1,4 +1,14 @@ { + "HALProvider": { + "Addressable LEDs": { + "0": { + "columns": 44 + }, + "window": { + "visible": true + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", @@ -36,5 +46,8 @@ }, "NetworkTables Info": { "visible": true + }, + "NetworkTables View": { + "visible": false } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 54bdf3b..4ff4043 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -218,6 +218,8 @@ private void configureBindings() { return m_operatorController.getRightTriggerAxis() > 0.5; }).whileTrue(new NoteOuttakeCommand(m_intakeSubsystem)); + + // Spin-up Shooter, Operator Controller left trigger new Trigger(() -> { return m_operatorController.getLeftTriggerAxis() > 0.5; }).onTrue(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime)) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index a0d8a99..049090c 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; import frc.robot.Constants.ShooterConstants; public class ShooterSubsystem extends SubsystemBase { @@ -58,14 +59,14 @@ public void setShootingSpeed(ShootSpeed speed) { } public double returnCurrentSpeed() { - return m_bottom.getEncoder().getVelocity(); + return Robot.isReal() ? m_bottom.getEncoder().getVelocity() : (m_topSpeed > 0.5) ? 4000 : 0; } @Override public void periodic() { // This method will be called once per scheduler run // SmartDashboard.putNumber("bottom Speed", m_bottomSpeed); - // SmartDashboard.putNumber("top Speed", m_topSpeed); + SmartDashboard.putNumber("top Speed", m_topSpeed); m_bottom.set(m_bottomSpeed); m_top.set(m_topSpeed); From b5f87895a0a056f85925a4c249cc8ecdfe2f9ca5 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Tue, 26 Mar 2024 17:02:54 -0700 Subject: [PATCH 17/24] Modify Sim Support --- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 049090c..b765535 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -22,6 +22,8 @@ public class ShooterSubsystem extends SubsystemBase { private double m_topSpeed = 0; private double m_bottomSpeed = 0; + private double m_simRPM = 0; + public ShooterSubsystem() { m_bottom.setIdleMode(IdleMode.kCoast); m_top.setIdleMode(IdleMode.kCoast); @@ -59,7 +61,9 @@ public void setShootingSpeed(ShootSpeed speed) { } public double returnCurrentSpeed() { - return Robot.isReal() ? m_bottom.getEncoder().getVelocity() : (m_topSpeed > 0.5) ? 4000 : 0; + if (m_topSpeed > 0.5 && m_simRPM < 15) m_simRPM++; + else if (m_simRPM > 0) m_simRPM--; + return Robot.isReal() ? m_bottom.getEncoder().getVelocity() : m_simRPM * 400; } @Override From dd5d610897e3c862a644968d0ecf5fd2d908391b Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Fri, 29 Mar 2024 19:37:17 -0700 Subject: [PATCH 18/24] Fixed LED constants --- src/main/java/frc/robot/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a4485a5..27cbd23 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -158,8 +158,8 @@ public static final class VisionConstants { public static final class LEDConstants { // TODO: find plz - public static final int kLEDPort = 9; - public static final int kLEDLength = 44; + public static final int kLEDPort = 0; + public static final int kLEDLength = 97; } } From c8ceb770bd312f124c219f956d27658c6c261a83 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Sat, 30 Mar 2024 09:36:35 -0700 Subject: [PATCH 19/24] LED tuning --- src/main/java/frc/robot/commands/DefaultLEDCommand.java | 4 ++-- src/main/java/frc/robot/subsystems/LEDSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/DefaultLEDCommand.java index 6d837de..dfdb6e8 100644 --- a/src/main/java/frc/robot/commands/DefaultLEDCommand.java +++ b/src/main/java/frc/robot/commands/DefaultLEDCommand.java @@ -47,11 +47,11 @@ public void initialize() { } shootSpeed = m_shooterSubsystem.returnCurrentSpeed(); - if (shootSpeed > 500 && shootSpeed < 1899 * 2) {// if charging up + if (shootSpeed > 500 && shootSpeed < 1899 * Math.PI) { // if charging up rgb[0] = 150; rgb[1] = 150; rgb[2] = 0; - } else if (shootSpeed > 1899 * 2) {// if the shooter is ready to shoot + } else if (shootSpeed > 1899 * Math.PI) { // if the shooter is ready to shoot rgb[0] = 0; rgb[1] = 255; rgb[2] = 0; diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 2e235d0..7ab725e 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -59,7 +59,7 @@ private void rainbow() { m_LEDBuffer.setHSV(i, hue, 255, 128); } // Increase by to make the rainbow "move" - m_rainbowFirstPixelHue += 1; + m_rainbowFirstPixelHue += 2; // Check bounds m_rainbowFirstPixelHue %= 180; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index b765535..54ffc08 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -70,7 +70,7 @@ public double returnCurrentSpeed() { public void periodic() { // This method will be called once per scheduler run // SmartDashboard.putNumber("bottom Speed", m_bottomSpeed); - SmartDashboard.putNumber("top Speed", m_topSpeed); + SmartDashboard.putNumber("top Speed", m_bottom.getEncoder().getVelocity()); m_bottom.set(m_bottomSpeed); m_top.set(m_topSpeed); From 4c6e29bbcba2601e1df28ea2671bcf91053cc737 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Sat, 30 Mar 2024 10:35:41 -0700 Subject: [PATCH 20/24] added pneumatics LED --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/commands/DefaultLEDCommand.java | 15 ++++++++++- .../robot/subsystems/ClimberSubsystem.java | 4 +++ .../frc/robot/subsystems/LEDSubsystem.java | 26 ++++++++++++++++++- 4 files changed, 44 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4ff4043..adb44ba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -134,7 +134,7 @@ public RobotContainer() { configureBindings(); m_ledSubsystem.setDefaultCommand( - new DefaultLEDCommand(m_ledSubsystem, m_intakeSubsystem, m_shooterSubsystem) + new DefaultLEDCommand(m_ledSubsystem, m_intakeSubsystem, m_shooterSubsystem, m_climberSubsystem) ); m_robotDrive.setDefaultCommand( diff --git a/src/main/java/frc/robot/commands/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/DefaultLEDCommand.java index dfdb6e8..2b63054 100644 --- a/src/main/java/frc/robot/commands/DefaultLEDCommand.java +++ b/src/main/java/frc/robot/commands/DefaultLEDCommand.java @@ -6,24 +6,28 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.IntakeConstants; +import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShooterSubsystem; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; public class DefaultLEDCommand extends Command { private LEDSubsystem m_ledSubsystem; private IntakeSubsystem m_intakeSubsystem; private ShooterSubsystem m_shooterSubsystem; + private ClimberSubsystem m_climberSubsystem; private double armSetpoint; private double shootSpeed; private int[] rgb = new int[3]; /** Creates a new SetLED. */ - public DefaultLEDCommand(LEDSubsystem LED, IntakeSubsystem intake, ShooterSubsystem shooter) { + public DefaultLEDCommand(LEDSubsystem LED, IntakeSubsystem intake, ShooterSubsystem shooter, ClimberSubsystem climb) { m_ledSubsystem = LED; m_intakeSubsystem = intake; m_shooterSubsystem = shooter; + m_climberSubsystem = climb; addRequirements(m_ledSubsystem); } @@ -35,6 +39,7 @@ public void initialize() { rgb[1] = 256; rgb[2] = 256; + //check for arm angle & haveNote armSetpoint = m_intakeSubsystem.getArmPosition(); if (armSetpoint == IntakeConstants.kIntakeLoweredAngle) { rgb[0] = 255; @@ -46,6 +51,7 @@ public void initialize() { rgb[2] = 255; } + //check for shooter spinup and override b4 value shootSpeed = m_shooterSubsystem.returnCurrentSpeed(); if (shootSpeed > 500 && shootSpeed < 1899 * Math.PI) { // if charging up rgb[0] = 150; @@ -57,6 +63,13 @@ public void initialize() { rgb[2] = 0; } + //check for pneumatics state and override b4 value + if (m_climberSubsystem.getState() == Value.kForward){ + rgb[0] = 257; + rgb[1] = 257; + rgb[2] = 257; + } + m_ledSubsystem.setLED(rgb[0], rgb[1], rgb[2]); } diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 3a42743..9bd6f9c 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -62,6 +62,10 @@ public void reverse() { m_state = kReverse; } + public Value getState(){ + return m_state; + } + /** * Toggles the state of the compressor (on/off) */ diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 7ab725e..eb1b672 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -37,11 +37,16 @@ public LEDSubsystem() { public void setLED(int r, int g, int b) { // If we get an invalid value just set it to a rainbow - if (r > 255 || b > 255 || g > 255) { + if (r == 256 && g == 256 && b == 256) { rainbow(); return; } + else if (r == 257 && g == 257 && b == 257){ + golden(); + return; + } + for (var i = 0; i < LEDConstants.kLEDLength; i++) { m_LEDBuffer.setRGB(i, r, g, b); } @@ -63,6 +68,25 @@ private void rainbow() { // Check bounds m_rainbowFirstPixelHue %= 180; + m_LED.setData(m_LEDBuffer); + SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); + } + + private void golden() { + // For every pixel + for (var i = 0; i < m_LEDBuffer.getLength(); i++) { + // Calculate the hue - hue is easier for rainbows because the color + // shape is a circle so only one value needs to precess + final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_LEDBuffer.getLength())) % 120 + 90; + // Set the value + m_LEDBuffer.setHSV(i, hue, 240, 180); + } + // Increase by to make the rainbow "move" + m_rainbowFirstPixelHue += 1; + // Check bounds + m_rainbowFirstPixelHue %= 120; + + m_LED.setData(m_LEDBuffer); SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); } From 1474c5074548484530ad2a39f70eedbecf23f763 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Sat, 30 Mar 2024 11:47:42 -0700 Subject: [PATCH 21/24] purple --- src/main/java/frc/robot/commands/DefaultLEDCommand.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/DefaultLEDCommand.java index 2b63054..b598a8f 100644 --- a/src/main/java/frc/robot/commands/DefaultLEDCommand.java +++ b/src/main/java/frc/robot/commands/DefaultLEDCommand.java @@ -54,9 +54,9 @@ public void initialize() { //check for shooter spinup and override b4 value shootSpeed = m_shooterSubsystem.returnCurrentSpeed(); if (shootSpeed > 500 && shootSpeed < 1899 * Math.PI) { // if charging up - rgb[0] = 150; - rgb[1] = 150; - rgb[2] = 0; + rgb[0] = 128; + rgb[1] = 0; + rgb[2] = 255; } else if (shootSpeed > 1899 * Math.PI) { // if the shooter is ready to shoot rgb[0] = 0; rgb[1] = 255; From 36359cd123678fbc2334a4a8db62005b2aa1d597 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Sat, 30 Mar 2024 11:55:01 -0700 Subject: [PATCH 22/24] keep it rainbow while climbing --- .../java/frc/robot/commands/DefaultLEDCommand.java | 2 +- .../frc/robot/subsystems/ClimberSubsystem.java | 14 ++++++++++++++ 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/DefaultLEDCommand.java index b598a8f..58b166c 100644 --- a/src/main/java/frc/robot/commands/DefaultLEDCommand.java +++ b/src/main/java/frc/robot/commands/DefaultLEDCommand.java @@ -64,7 +64,7 @@ public void initialize() { } //check for pneumatics state and override b4 value - if (m_climberSubsystem.getState() == Value.kForward){ + if (m_climberSubsystem.getState() == Value.kForward || m_climberSubsystem.deployed()){ rgb[0] = 257; rgb[1] = 257; rgb[2] = 257; diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 9bd6f9c..f3df0c2 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.PneumaticHub; import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ClimberConstants; @@ -23,6 +24,9 @@ public class ClimberSubsystem extends SubsystemBase { private Value m_state; + private double togglecount = 0; + private Timer m_timer = new Timer(); + public ClimberSubsystem() { m_pHub = new PneumaticHub(2); @@ -39,6 +43,10 @@ public void periodic() { SmartDashboard.putNumber("pressure", m_pHub.getPressure(0)); SmartDashboard.putBoolean("Compressor Enabled", m_compressorEnabled); SmartDashboard.putBoolean("Compressor Running", m_pHub.getCompressor()); + + if (togglecount >= 3){ + togglecount = 0; + } } /** @@ -53,6 +61,7 @@ public void solenoidOff() { */ public void forward() { m_state = kForward; + togglecount++; } /** @@ -60,12 +69,17 @@ public void forward() { */ public void reverse() { m_state = kReverse; + togglecount++; } public Value getState(){ return m_state; } + public boolean deployed(){ + return togglecount > 0; + } + /** * Toggles the state of the compressor (on/off) */ From 970175af3356d9f8a96067a659e49376d81294f8 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Sat, 30 Mar 2024 11:59:54 -0700 Subject: [PATCH 23/24] Revert "keep it rainbow while climbing" This reverts commit 36359cd123678fbc2334a4a8db62005b2aa1d597. --- .../java/frc/robot/commands/DefaultLEDCommand.java | 2 +- .../frc/robot/subsystems/ClimberSubsystem.java | 14 -------------- 2 files changed, 1 insertion(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/commands/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/DefaultLEDCommand.java index 58b166c..b598a8f 100644 --- a/src/main/java/frc/robot/commands/DefaultLEDCommand.java +++ b/src/main/java/frc/robot/commands/DefaultLEDCommand.java @@ -64,7 +64,7 @@ public void initialize() { } //check for pneumatics state and override b4 value - if (m_climberSubsystem.getState() == Value.kForward || m_climberSubsystem.deployed()){ + if (m_climberSubsystem.getState() == Value.kForward){ rgb[0] = 257; rgb[1] = 257; rgb[2] = 257; diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index f3df0c2..9bd6f9c 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.PneumaticHub; import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ClimberConstants; @@ -24,9 +23,6 @@ public class ClimberSubsystem extends SubsystemBase { private Value m_state; - private double togglecount = 0; - private Timer m_timer = new Timer(); - public ClimberSubsystem() { m_pHub = new PneumaticHub(2); @@ -43,10 +39,6 @@ public void periodic() { SmartDashboard.putNumber("pressure", m_pHub.getPressure(0)); SmartDashboard.putBoolean("Compressor Enabled", m_compressorEnabled); SmartDashboard.putBoolean("Compressor Running", m_pHub.getCompressor()); - - if (togglecount >= 3){ - togglecount = 0; - } } /** @@ -61,7 +53,6 @@ public void solenoidOff() { */ public void forward() { m_state = kForward; - togglecount++; } /** @@ -69,17 +60,12 @@ public void forward() { */ public void reverse() { m_state = kReverse; - togglecount++; } public Value getState(){ return m_state; } - public boolean deployed(){ - return togglecount > 0; - } - /** * Toggles the state of the compressor (on/off) */ From a760b5d2e17e67d50cc1454e23db0d87cd969271 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Sat, 30 Mar 2024 12:23:02 -0700 Subject: [PATCH 24/24] wtf is get odometry --- src/main/java/frc/robot/subsystems/DriveSubsystem.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 320c4e7..0934ad3 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -199,9 +199,6 @@ public void resetOdometry(Pose2d pose) { }, pose); } - public Pose2d getOdometry(){ - return m_poseEstimator.getEstimatedPosition(); - } /** Zeroes the heading of the robot. */ public void zeroHeading() {