Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

STEMnasium LED #41

Merged
merged 28 commits into from
Mar 30, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
28 commits
Select commit Hold shift + click to select a range
2ef7ae2
feat: LED subsystem
ProgrammingSR Feb 27, 2024
2eb69bd
Changed LED port constant
ProgrammingSR Feb 27, 2024
f8bb56e
WIP: added a new command and method for odometry
ProgrammingSR Mar 2, 2024
ba56350
feat: basic code for setting the LED color based on robot positions f…
ProgrammingSR Mar 4, 2024
20d6f40
Feat: Blue LED Lights when Robot is in an Intake Position
ProgrammingSR Mar 5, 2024
9473322
Merge branch 'main' into LED
ProgrammingSR Mar 12, 2024
01658bc
LED william
ProgrammingSR Mar 14, 2024
39b31e1
Merge branch 'main' into LED
ProgrammingSR Mar 14, 2024
6afeeae
feat: added shooter functionality and fixed naming
ArnoUUU Mar 15, 2024
d03cb86
Revert "LED william"
ProgrammingSR Mar 15, 2024
cc052ee
LED stuff
ProgrammingSR Mar 16, 2024
db7d31a
removed consumer LED
ProgrammingSR Mar 19, 2024
f3e1067
fix: checks distance sensor toggle before reading value
ProgrammingSR Mar 19, 2024
4992c07
chore: deleted unused constants
ProgrammingSR Mar 19, 2024
a8dc6f6
chore: updated kLEDLength
ProgrammingSR Mar 19, 2024
a0d6586
Check for note when using LED, slowed down rainbow LED
ProgrammingSR Mar 19, 2024
746c1d3
Merge branch 'main' into LED
ArnoUUU Mar 20, 2024
301c8ed
Merge branch 'main' into LED
ArnoUUU Mar 20, 2024
3aa7eb8
Merge branch 'main' into LED
ProgrammingSR Mar 26, 2024
bf5a4e4
Simulation Support
ProgrammingSR Mar 26, 2024
b5f8789
Modify Sim Support
ProgrammingSR Mar 27, 2024
dd5d610
Fixed LED constants
ProgrammingSR Mar 30, 2024
c8ceb77
LED tuning
ProgrammingSR Mar 30, 2024
4c6e29b
added pneumatics LED
ProgrammingSR Mar 30, 2024
1474c50
purple
ProgrammingSR Mar 30, 2024
36359cd
keep it rainbow while climbing
ProgrammingSR Mar 30, 2024
970175a
Revert "keep it rainbow while climbing"
ProgrammingSR Mar 30, 2024
a760b5d
wtf is get odometry
ProgrammingSR Mar 30, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions simgui-ds.json
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,10 @@
"robotJoysticks": [
{
"guid": "Keyboard0"
},
{
"guid": "78696e70757401000000000000000000",
"useGamepad": true
}
]
}
13 changes: 13 additions & 0 deletions simgui.json
Original file line number Diff line number Diff line change
@@ -1,4 +1,14 @@
{
"HALProvider": {
"Addressable LEDs": {
"0": {
"columns": 44
},
"window": {
"visible": true
}
}
},
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo",
Expand Down Expand Up @@ -36,5 +46,8 @@
},
"NetworkTables Info": {
"visible": true
},
"NetworkTables View": {
"visible": false
}
}
6 changes: 6 additions & 0 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -156,4 +156,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 = 97;
}

}
9 changes: 9 additions & 0 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -27,13 +27,15 @@
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;
import frc.robot.commands.ShooterSetSpeedCommand;
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;
Expand All @@ -52,6 +54,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);
Expand Down Expand Up @@ -130,6 +133,10 @@ public RobotContainer() {
// Configure the trigger bindings
configureBindings();

m_ledSubsystem.setDefaultCommand(
new DefaultLEDCommand(m_ledSubsystem, m_intakeSubsystem, m_shooterSubsystem, m_climberSubsystem)
);

m_robotDrive.setDefaultCommand(
new RunCommand(
() -> m_robotDrive.drive(
Expand Down Expand Up @@ -211,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))
Expand Down
92 changes: 92 additions & 0 deletions src/main/java/frc/robot/commands/DefaultLEDCommand.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,92 @@
// 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.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, ClimberSubsystem climb) {
m_ledSubsystem = LED;
m_intakeSubsystem = intake;
m_shooterSubsystem = shooter;
m_climberSubsystem = climb;
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;

//check for arm angle & haveNote
armSetpoint = m_intakeSubsystem.getArmPosition();
if (armSetpoint == IntakeConstants.kIntakeLoweredAngle) {
rgb[0] = 255;
rgb[1] = 0;
rgb[2] = 0;
} else if (armSetpoint == IntakeConstants.kIntakeRaisedAngle && m_intakeSubsystem.haveNote()) {
rgb[0] = 0;
rgb[1] = 0;
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] = 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;
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]);
}

// 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;
}
}
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/subsystems/ClimberSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,10 @@ public void reverse() {
m_state = kReverse;
}

public Value getState(){
return m_state;
}

/**
* Toggles the state of the compressor (on/off)
*/
Expand Down
93 changes: 93 additions & 0 deletions src/main/java/frc/robot/subsystems/LEDSubsystem.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,93 @@
// 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);
private int m_rainbowFirstPixelHue = 0;

/** 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) {

// If we get an invalid value just set it to a rainbow
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);
}
m_LED.setData(m_LEDBuffer);
SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString());
}

private void rainbow() {
// 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 += 2;
// 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());
}
}
10 changes: 8 additions & 2 deletions src/main/java/frc/robot/subsystems/ShooterSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,13 @@
package frc.robot.subsystems;

import com.revrobotics.CANSparkBase.IdleMode;

import com.revrobotics.CANSparkFlex;
import com.revrobotics.CANSparkLowLevel.MotorType;

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 {
Expand All @@ -20,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);
Expand Down Expand Up @@ -57,14 +61,16 @@ public void setShootingSpeed(ShootSpeed speed) {
}

public double returnCurrentSpeed() {
return m_bottom.getEncoder().getVelocity();
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
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);
Expand Down
Loading