From e6986f1e191f94fa01b78cc067b23564c6589b44 Mon Sep 17 00:00:00 2001 From: GearFox Date: Sat, 13 May 2023 09:21:00 -0400 Subject: [PATCH 1/7] Feat: Slowmode and LED --- src/main/java/frc/robot/RobotContainer.java | 16 +++- .../frc/robot/subsystems/LedSubsystem.java | 73 +++++++++++++++++++ .../subsystems/arm/ArmAngleSubsystem.java | 2 +- .../robot/subsystems/arm/ArmExtSubsystem.java | 3 +- .../subsystems/wrist/WristSubsystem.java | 2 +- 5 files changed, 92 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/LedSubsystem.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fe48a4c1..228725b1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -85,7 +85,21 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - m_drive.setDefaultCommand(new SwerveTeleopDrive(m_drive, m_driveController)); + if (m_candle != null) { + /* + if (DriverStation.getAlliance() == Alliance.Red) { + m_candle.animate(new ColorFlowAnimation(255, 0, 0)); + } else { + m_candle.animate(new ColorFlowAnimation(0, 0, 255)); + } + */ + m_candle.animate(new RainbowAnimation()); + } + + if (m_driveController != null) { + m_drive.setDefaultCommand(new SwerveTeleopDrive(m_drive, m_driveController)); + m_arm.setDefaultCommand(new HoldArmAngleCommand(m_arm)); + m_driveController.button(7).onTrue(m_drive.resetGyroBase()); m_driveController.start().onTrue(m_drive.toggleFieldRelative()); 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 00000000..eb0e6fae --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LedSubsystem.java @@ -0,0 +1,73 @@ +import com.ctre.phoenix.led.Animation; +import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.ColorFlowAnimation; +import com.ctre.phoenix.led.FireAnimation; +import com.ctre.phoenix.led.LarsonAnimation; +import com.ctre.phoenix.led.RainbowAnimation; +import com.ctre.phoenix.led.ColorFlowAnimation.Direction; +import com.ctre.phoenix.led.LarsonAnimation.BounceMode; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LedSubsystem extends SubsystemBase { + private final CANdle m_candle; + private final int m_numLED; + private Animations m_currentAnimation = Animations.RAINBOW; + + private final Animation m_rainbow = new RainbowAnimation(1.0, 0.5, m_numLED); + private final Animation m_red = new ColorFlowAnimation(255, 0, 0, 0, 0.5, m_numLED, Direction.Forward); + private final Animation m_blue = new ColorFlowAnimation(0, 255, 0, 0, 0.5, m_numLED, Direction.Forward); + private final Animation m_fire = new FireAnimation(1.0, 0.5, m_numLED, 0.25, 0.25); + private final Animation m_larson = new LarsonAnimation(255, 0, 0, 0, 0.5, m_numLED, BounceMode.Front, 20); + + public enum Animations { + RAINBOW, + RED, + BLUE, + FIRE, + LARSON + } + + public LedSubsystem(int id) { + m_candle = new CANdle(id); + m_candle.animate(m_rainbow); + } + + public void nextAnimation() { + m_currentAnimation = Animations.values()[m_currentAnimation.ordinal() + 1 < 5 ? 0 : m_currentAnimation.ordinal() + 1]; + } + + public void setAnimation(Animations animation) { + m_currentAnimation = animation; + } + + public Animations getAnimation() { + return m_currentAnimation; + } + + @Override + public void periodic() { + switch(m_currentAnimation){ + case BLUE: + m_candle.animate(m_blue); + break; + case FIRE: + m_candle.animate(m_fire); + break; + case LARSON: + m_candle.animate(m_larson); + break; + case RAINBOW: + m_candle.animate(m_rainbow); + break; + case RED: + m_candle.animate(m_red); + break; + default: + m_candle.setLEDs(255, 0, 255); + break; + + } + } + +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmAngleSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmAngleSubsystem.java index 10b8bf01..0ec591d4 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmAngleSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmAngleSubsystem.java @@ -182,7 +182,7 @@ public void setArmAngle(double targetAngleRaw){ SmartDashboard.putNumber("Angle PID Output", targetAnglePID); // Set voltage based off of PID - m_armAngleMaster.setVoltage(targetAnglePID); + m_armAngleMaster.setVoltage(targetAnglePID * 0.6); } public double getArmAngle() { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmExtSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmExtSubsystem.java index 9feabd10..58874e31 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmExtSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmExtSubsystem.java @@ -60,7 +60,8 @@ public ArmExtSubsystem() { m_extPID.setP(Constants.ArmConstants.ARM_EXT_KP.getValue()); m_extPID.setI(Constants.ArmConstants.ARM_EXT_KI.getValue()); m_extPID.setD(Constants.ArmConstants.ARM_EXT_KD.getValue()); - m_extPID.setOutputRange(-2, 2); + m_extPID.setOutputRange(-0.4, 0.4); + // m_extPID.setOutputRange(-0.75, 0.75); m_armLimitSwitch = new DigitalInput(Constants.ArmConstants.LIMIT_SWITCH_PORT); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 3d71459c..a519b070 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -178,7 +178,7 @@ public void setWristPower(double speed) { SmartDashboard.putBoolean(HITTING_SOFT_LIMIT_STRING, true); m_wristMotor.set(0.0); } else { - m_wristMotor.set(speed); + m_wristMotor.set(speed * 0.2); SmartDashboard.putBoolean(HITTING_SOFT_LIMIT_STRING, false); } } From f8d9e7932b8d02e47b4baf005de69e6831075796 Mon Sep 17 00:00:00 2001 From: GearFox Date: Sat, 13 May 2023 09:22:58 -0400 Subject: [PATCH 2/7] package header --- src/main/java/frc/robot/subsystems/LedSubsystem.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/LedSubsystem.java b/src/main/java/frc/robot/subsystems/LedSubsystem.java index eb0e6fae..0d61e637 100644 --- a/src/main/java/frc/robot/subsystems/LedSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LedSubsystem.java @@ -1,3 +1,5 @@ +package frc.robot.subsystems; + import com.ctre.phoenix.led.Animation; import com.ctre.phoenix.led.CANdle; import com.ctre.phoenix.led.ColorFlowAnimation; @@ -11,7 +13,7 @@ public class LedSubsystem extends SubsystemBase { private final CANdle m_candle; - private final int m_numLED; + private final int m_numLED = 400; private Animations m_currentAnimation = Animations.RAINBOW; private final Animation m_rainbow = new RainbowAnimation(1.0, 0.5, m_numLED); From 9836ff1aef7ea5e1ffe11ecfac0281b0ad6b4762 Mon Sep 17 00:00:00 2001 From: GearFox Date: Sat, 13 May 2023 09:32:51 -0400 Subject: [PATCH 3/7] led control --- src/main/java/frc/robot/RobotContainer.java | 10 ----- .../robot/commands/LEDControllerCommand.java | 45 +++++++++++++++++++ 2 files changed, 45 insertions(+), 10 deletions(-) create mode 100644 src/main/java/frc/robot/commands/LEDControllerCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 228725b1..1fe0107a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -85,16 +85,6 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - if (m_candle != null) { - /* - if (DriverStation.getAlliance() == Alliance.Red) { - m_candle.animate(new ColorFlowAnimation(255, 0, 0)); - } else { - m_candle.animate(new ColorFlowAnimation(0, 0, 255)); - } - */ - m_candle.animate(new RainbowAnimation()); - } if (m_driveController != null) { m_drive.setDefaultCommand(new SwerveTeleopDrive(m_drive, m_driveController)); diff --git a/src/main/java/frc/robot/commands/LEDControllerCommand.java b/src/main/java/frc/robot/commands/LEDControllerCommand.java new file mode 100644 index 00000000..05ec5443 --- /dev/null +++ b/src/main/java/frc/robot/commands/LEDControllerCommand.java @@ -0,0 +1,45 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.LedSubsystem; + +public class LEDControllerCommand extends CommandBase { + /** Creates a new LEDControllerCommand. */ + LedSubsystem m_led; + Timer m_timer; + public LEDControllerCommand(LedSubsystem led) { + // Use addRequirements() here to declare subsystem dependencies. + m_led = led; + m_timer = new Timer(); + addRequirements(led); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (m_timer.hasElapsed(20)) { + m_led.nextAnimation(); + } + } + + // 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; + } +} From 1dffa5c5ab2ea90fc5b85d31de814958b2e813aa Mon Sep 17 00:00:00 2001 From: GearFox Date: Sat, 13 May 2023 09:37:40 -0400 Subject: [PATCH 4/7] enable led control --- src/main/java/frc/robot/RobotContainer.java | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8fe777a7..0a1de842 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import frc.robot.subsystems.LedSubsystem; import frc.robot.subsystems.arm.ArmAngleSubsystem; import frc.robot.subsystems.swerve.SwerveDrivetrain; import frc.robot.subsystems.wrist.WristSubsystem; @@ -43,7 +44,7 @@ public class RobotContainer { private ArmExtSubsystem m_ext; private ArmSupersystem m_super; private FootPedal m_foot; - private CANdle m_candle; + private LedSubsystem m_led; //Controllers private final CommandXboxController m_driveController = new CommandXboxController(Constants.DRIVER_PORT); @@ -59,7 +60,7 @@ public RobotContainer() { switch (Constants.CURRENT_MODE) { // Beta robot hardware implementation case HELIOS_V2: - m_candle = new CANdle(22); + m_led = new LedSubsystem(22); // m_drive = new SwerveDrivetrain(); // break; case HELIOS_V1: @@ -95,13 +96,7 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - if (m_candle != null) { - if (DriverStation.getAlliance() == Alliance.Red) { - m_candle.animate(new ColorFlowAnimation(255, 0, 0)); - } else { - m_candle.animate(new ColorFlowAnimation(0, 0, 255)); - } - } + m_led.setDefaultCommand(new LEDControllerCommand(m_led)); if (m_driveController != null) { m_drive.setDefaultCommand(new SwerveTeleopDrive(m_drive, m_driveController)); From f7fbeaf54052ff6fc164ff5f6d4702a3033f9e02 Mon Sep 17 00:00:00 2001 From: GearFox Date: Sat, 13 May 2023 09:40:55 -0400 Subject: [PATCH 5/7] uhh somthing something slowmode --- src/main/java/frc/robot/commands/SwerveTeleopDrive.java | 2 +- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/SwerveTeleopDrive.java b/src/main/java/frc/robot/commands/SwerveTeleopDrive.java index 0d9b2faf..dde7350c 100644 --- a/src/main/java/frc/robot/commands/SwerveTeleopDrive.java +++ b/src/main/java/frc/robot/commands/SwerveTeleopDrive.java @@ -38,7 +38,7 @@ public void initialize() {} public void execute() { double x = -m_driverController.getLeftY(); double y = -m_driverController.getLeftX(); - double z = -m_driverController.getRightX() / 1.8; + double z = -m_driverController.getRightX();// / 1.8; if (m_drive.getSlowmode()) { x *= 0.5; diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index dbb27e02..0f49a6e8 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -194,7 +194,7 @@ public void setIntakeSpeed(double speed) { if (speed == 0) { m_intakeMotor.set(0.1); } else { - m_intakeMotor.set(speed * 0.2); + m_intakeMotor.set(speed); } } From 7f1772491c3d778f17fe72bd4d834c7beafa7e53 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Tue, 23 May 2023 20:28:01 -0400 Subject: [PATCH 6/7] Fix: increase ext speed cap --- src/main/java/frc/robot/subsystems/arm/ArmAngleSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/arm/ArmExtSubsystem.java | 4 ++-- .../java/frc/robot/subsystems/swerve/SwerveDrivetrain.java | 2 +- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 4 ++-- src/main/java/frc/robot/supersystems/ArmSupersystem.java | 2 +- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmAngleSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmAngleSubsystem.java index 1bc0f658..fed1b3f1 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmAngleSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmAngleSubsystem.java @@ -196,7 +196,7 @@ public void setArmAngle(double targetAngleRaw) { SmartDashboard.putNumber("Angle PID Output", targetAnglePID); // Set voltage based off of PID - m_armAngleMaster.setVoltage(targetAnglePID * 0.6); + m_armAngleMaster.setVoltage(targetAnglePID); } public double getArmAngle() { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmExtSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmExtSubsystem.java index 8bf4abea..bfd372bd 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmExtSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmExtSubsystem.java @@ -65,8 +65,8 @@ public ArmExtSubsystem() { m_extPID.setP(Constants.ArmConstants.ARM_EXT_KP.getValue()); m_extPID.setI(Constants.ArmConstants.ARM_EXT_KI.getValue()); m_extPID.setD(Constants.ArmConstants.ARM_EXT_KD.getValue()); - m_extPID.setOutputRange(-0.4, 0.4); - // m_extPID.setOutputRange(-0.75, 0.75); +// m_extPID.setOutputRange(-0.4, 0.4); + m_extPID.setOutputRange(-1, 1); // m_extPID = new PIDController( // Constants.ArmConstants.ARM_EXT_KP.getValue(), diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java index 8fdd6e77..da0a8e87 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java @@ -229,7 +229,7 @@ public void drive(double xTranslation, double yTranslation, double zRotation) { public void setModuleStates(SwerveModuleState[] states) { for (SwerveModuleState state : states) { state.speedMetersPerSecond *= m_speedMult; - state.speedMetersPerSecond *= 0.2; +// state.speedMetersPerSecond *= 0.2; } m_flMod.setDesiredState(states[0]); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index dbb27e02..16a981dd 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -177,7 +177,7 @@ public void setWristPower(double speed) { SmartDashboard.putBoolean(HITTING_SOFT_LIMIT_STRING, true); m_wristMotor.set(0.0); } else { - m_wristMotor.set(speed * 0.2); + m_wristMotor.set(speed); SmartDashboard.putBoolean(HITTING_SOFT_LIMIT_STRING, false); } } @@ -194,7 +194,7 @@ public void setIntakeSpeed(double speed) { if (speed == 0) { m_intakeMotor.set(0.1); } else { - m_intakeMotor.set(speed * 0.2); + m_intakeMotor.set(speed); } } diff --git a/src/main/java/frc/robot/supersystems/ArmSupersystem.java b/src/main/java/frc/robot/supersystems/ArmSupersystem.java index b45a8e2e..dc38bed7 100644 --- a/src/main/java/frc/robot/supersystems/ArmSupersystem.java +++ b/src/main/java/frc/robot/supersystems/ArmSupersystem.java @@ -190,7 +190,7 @@ public void getDriveSpeed () { m_swerve.setSpeedMult(range.doubleValue()); if (m_ext.getArmExtension() > 5) { - m_swerve.setRotationMult(0.5); + m_swerve.setRotationMult(0.75); } else { m_swerve.setRotationMult(1); } From f33b41e538df59b44e33e48e406970b768ca6bca Mon Sep 17 00:00:00 2001 From: GearFox Date: Tue, 30 May 2023 18:32:52 -0400 Subject: [PATCH 7/7] Fix: led subsystems --- .../robot/commands/LEDControllerCommand.java | 4 +++- .../frc/robot/subsystems/LedSubsystem.java | 22 ++++++++++++++++++- 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/LEDControllerCommand.java b/src/main/java/frc/robot/commands/LEDControllerCommand.java index 05ec5443..e2de4243 100644 --- a/src/main/java/frc/robot/commands/LEDControllerCommand.java +++ b/src/main/java/frc/robot/commands/LEDControllerCommand.java @@ -22,14 +22,16 @@ public LEDControllerCommand(LedSubsystem led) { // Called when the command is initially scheduled. @Override public void initialize() { + m_timer.reset(); m_timer.start(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (m_timer.hasElapsed(20)) { + if (m_timer.hasElapsed(10)) { m_led.nextAnimation(); + m_timer.restart(); } } diff --git a/src/main/java/frc/robot/subsystems/LedSubsystem.java b/src/main/java/frc/robot/subsystems/LedSubsystem.java index 0d61e637..1af93068 100644 --- a/src/main/java/frc/robot/subsystems/LedSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LedSubsystem.java @@ -36,7 +36,27 @@ public LedSubsystem(int id) { } public void nextAnimation() { - m_currentAnimation = Animations.values()[m_currentAnimation.ordinal() + 1 < 5 ? 0 : m_currentAnimation.ordinal() + 1]; + switch (m_currentAnimation) { + case BLUE: + m_currentAnimation = Animations.FIRE; + break; + case FIRE: + m_currentAnimation = Animations.LARSON; + break; + case LARSON: + m_currentAnimation = Animations.RAINBOW; + break; + case RAINBOW: + m_currentAnimation = Animations.RED; + break; + case RED: + m_currentAnimation = Animations.BLUE; + break; + default: + m_currentAnimation = Animations.RAINBOW; + break; + + } } public void setAnimation(Animations animation) {