diff --git a/.gitignore b/.gitignore index 1a35c5c..862cfb0 100644 --- a/.gitignore +++ b/.gitignore @@ -105,3 +105,7 @@ gradle-app.setting # End of https://www.gitignore.io/api/gradle,intellij+all imgui.ini +.settings/org.eclipse.buildship.core.prefs +.settings/org.eclipse.jdt.core.prefs +.project +.classpath diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1648c82..d1ea4ff 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,7 +1,11 @@ package frc.robot; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.commands.Test; +import frc.robot.led.LED; +import frc.robot.led.LEDComponentsImpl; import frc.robot.robotControl.DeputyOi; import frc.robot.robotControl.DriverOi; import frc.robot.subsystems.driveTrain.DriveTrain; @@ -10,6 +14,7 @@ import frc.robot.subsystems.driveTrain.features.KeepAngleController; import frc.robot.subsystems.driveTrain.features.PoseEstimator; import frc.robot.subsystems.driveTrain.features.SwerveModule; +import sensors.Switch.Switch; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -20,6 +25,9 @@ public class Robot extends TimedRobot { private DriverOi driverOi; private DeputyOi deputyOi; + private DigitalInput toplimitSwitch; + + /** * This function is run when the robot is first started up and should be used for any @@ -28,10 +36,12 @@ public class Robot extends TimedRobot { @Override public void robotInit() { DriveTrain.init(new DriveTrainComponentsImpl()); + LED.init(new LEDComponentsImpl()); PoseEstimator.init(); KeepAngleController.init(); driverOi = new DriverOi(); deputyOi = new DeputyOi(); + toplimitSwitch = new DigitalInput(9); } /** @@ -59,6 +69,11 @@ public void disabledInit() { @Override public void disabledPeriodic() { + if(! toplimitSwitch.get()){ + LED.getInstance().setStrip(0,255,0); + }else { + LED.getInstance().setStrip(255,0,0); + } } /** diff --git a/src/main/java/frc/robot/commands/Test.java b/src/main/java/frc/robot/commands/Test.java new file mode 100644 index 0000000..34f2c9c --- /dev/null +++ b/src/main/java/frc/robot/commands/Test.java @@ -0,0 +1,22 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.led.LED; + +public class Test extends CommandBase { + private DigitalInput toplimitSwitch; + + public Test() { + toplimitSwitch = new DigitalInput(9); + } + + @Override + public void execute() { + if(! toplimitSwitch.get()){ + LED.getInstance().setStrip(0,255,0); + }else { + LED.getInstance().setStrip(255,0,0); + } + } +} diff --git a/src/main/java/frc/robot/commands/led/Blink.java b/src/main/java/frc/robot/commands/led/Blink.java new file mode 100644 index 0000000..82cce80 --- /dev/null +++ b/src/main/java/frc/robot/commands/led/Blink.java @@ -0,0 +1,38 @@ +package frc.robot.commands.led; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.led.LED; + +import static frc.robot.led.LEDConstants.BLACK; + +public class Blink extends SequentialCommandGroup { + private int ranTimes = 0; + + public Blink(int red, int green, int blue, double blinkDelays, int timesToBlink) { + addCommands + ( + new SequentialCommandGroup( + new BlinkOnce(red, green, blue, blinkDelays), + new InstantCommand(() -> ranTimes++) + ).repeatedly().until(() -> ranTimes == timesToBlink) + ); + } + + private class BlinkOnce extends SequentialCommandGroup { + + private LED led; + + public BlinkOnce(int red, int green, int blue, double blinkDelays) { + led = LED.getInstance(); + addRequirements(led); + addCommands( + new InstantCommand(() -> led.setStrip(red, green, blue)), + new WaitCommand(blinkDelays), + new InstantCommand(() -> led.setStrip(BLACK.getRed(), BLACK.getGreen(), BLACK.getBlue())), + new WaitCommand(blinkDelays) + ); + } + } +} diff --git a/src/main/java/frc/robot/commands/led/Gaming.java b/src/main/java/frc/robot/commands/led/Gaming.java new file mode 100644 index 0000000..e71f34c --- /dev/null +++ b/src/main/java/frc/robot/commands/led/Gaming.java @@ -0,0 +1,26 @@ +package frc.robot.commands.led; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.led.LED; + +public class Gaming extends CommandBase { + + private LED led; + private int jumps; + + public Gaming(int jumps) { + this.led = LED.getInstance(); + this.jumps = jumps; + addRequirements(led); + } + + @Override + public void execute() { + led.setGaming(jumps); + } + + @Override + public void end(boolean interrupted) { + led.setStripOff(); + } +} diff --git a/src/main/java/frc/robot/led/LED.java b/src/main/java/frc/robot/led/LED.java new file mode 100644 index 0000000..e67da4c --- /dev/null +++ b/src/main/java/frc/robot/led/LED.java @@ -0,0 +1,81 @@ +package frc.robot.led; + +import static frc.robot.led.LEDConstants.BLACK; +import static frc.robot.led.LEDConstants.MAX_HUE; +import static frc.robot.led.LEDConstants.RAINBOW_INDICATOR; +import static frc.robot.led.LEDConstants.RAINBOW_SATURATION; +import static frc.robot.led.LEDConstants.RAINBOW_VALUE; +import static frc.robot.led.LEDConstants.STRIP_LENGTH; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LED extends SubsystemBase { + + private int red; + private int green; + private int blue; + + private final LEDComponents components; + + private LED(LEDComponents components) { + this.components = components; + } + + public String getCurrentColor() { + return "Red: " + red + + " Green: " + green + + " Blue: " + blue; + } + + private void setOneLed(int ledIndex, int red, int green, int blue) { + components.getBuffer().setRGB(ledIndex, red, green, blue); + } + + public void setStrip(int red, int green, int blue) { + for (int ledIndex = 0; ledIndex < STRIP_LENGTH; ++ledIndex) + setOneLed(ledIndex, red, green, blue); + + this.red = red; + this.blue = blue; + this.green = green; + + Update(); + } + + public void setStripOff() { + setStrip(BLACK.getRed(), BLACK.getGreen(), BLACK.getBlue()); + } + + public void setGaming(int ledJumps) { + int hue; + int ledOffset = 0; + for (int ledIndex = 0; ledIndex < STRIP_LENGTH; ++ledIndex) { + hue = (ledOffset + (ledIndex * MAX_HUE / STRIP_LENGTH)) % MAX_HUE; + + components.getBuffer().setHSV(ledIndex, hue, RAINBOW_SATURATION, RAINBOW_VALUE); + } + + ledOffset += ledJumps; + ledOffset %= MAX_HUE; + + this.red = RAINBOW_INDICATOR.getRed(); + this.green = RAINBOW_INDICATOR.getGreen(); + this.blue = RAINBOW_INDICATOR.getBlue(); + + Update(); + } + + public void Update() { + components.getStrip().setData(components.getBuffer()); + } + + private static LED instance; + + public static void init(LEDComponents components) { + instance = new LED(components); + } + + public static LED getInstance() { + return instance; + } +} diff --git a/src/main/java/frc/robot/led/LEDComponents.java b/src/main/java/frc/robot/led/LEDComponents.java new file mode 100644 index 0000000..09b746a --- /dev/null +++ b/src/main/java/frc/robot/led/LEDComponents.java @@ -0,0 +1,11 @@ +package frc.robot.led; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; + +public interface LEDComponents { + + AddressableLED getStrip(); + + AddressableLEDBuffer getBuffer(); +} diff --git a/src/main/java/frc/robot/led/LEDComponentsImpl.java b/src/main/java/frc/robot/led/LEDComponentsImpl.java new file mode 100644 index 0000000..3efa211 --- /dev/null +++ b/src/main/java/frc/robot/led/LEDComponentsImpl.java @@ -0,0 +1,34 @@ +package frc.robot.led; + +import static frc.robot.led.LEDConstants.PORT; +import static frc.robot.led.LEDConstants.STRIP_LENGTH; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; + +public class LEDComponentsImpl implements LEDComponents { + + private final AddressableLED Strip; + private final AddressableLEDBuffer Buffer; + + public LEDComponentsImpl() { + Strip = new AddressableLED(PORT); + Buffer = new AddressableLEDBuffer(STRIP_LENGTH); + + + Strip.setLength(STRIP_LENGTH); + + Strip.setData(Buffer); + Strip.start(); + } + + @Override + public AddressableLED getStrip() { + return Strip; + } + + @Override + public AddressableLEDBuffer getBuffer() { + return Buffer; + } +} diff --git a/src/main/java/frc/robot/led/LEDConstants.java b/src/main/java/frc/robot/led/LEDConstants.java new file mode 100644 index 0000000..8992e2e --- /dev/null +++ b/src/main/java/frc/robot/led/LEDConstants.java @@ -0,0 +1,15 @@ +package frc.robot.led; + +import java.awt.Color; + +public class LEDConstants { + + public static Color BLACK = new Color(0, 0, 0); + public static Color RAINBOW_INDICATOR = new Color(210, 52, 235); + + public static final int STRIP_LENGTH = 144; + public static final int PORT = 9; + public static final int RAINBOW_SATURATION = 255; + public static final int RAINBOW_VALUE = 128; + public static final int MAX_HUE = 180; +} diff --git a/src/main/java/frc/robot/logging/DriveTrainShuffleBoard.java b/src/main/java/frc/robot/logging/DriveTrainShuffleBoard.java index d9659d2..3918c96 100644 --- a/src/main/java/frc/robot/logging/DriveTrainShuffleBoard.java +++ b/src/main/java/frc/robot/logging/DriveTrainShuffleBoard.java @@ -1,20 +1,65 @@ package frc.robot.logging; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import frc.robot.commands.driveTrain.MoveByDistance; +import frc.robot.commands.led.Blink; +import frc.robot.commands.led.Gaming; import frc.robot.subsystems.driveTrain.DriveTrain; import frc.robot.subsystems.driveTrain.DriveTrainComponents; import frc.robot.subsystems.driveTrain.features.PoseEstimator; import frc.robot.subsystems.driveTrain.features.SwerveModule; +import frc.robot.led.LED; + +import java.util.function.DoubleSupplier; +import java.util.function.IntSupplier; +import java.util.function.LongSupplier; public class DriveTrainShuffleBoard { - public DriveTrainShuffleBoard(DriveTrain driveTrain, DriveTrainComponents components) { + private DriveTrain driveTrain; + private ShuffleboardTab tab; + + private int red; - ShuffleboardTab tab = Shuffleboard.getTab("Swerve"); + public DriveTrainShuffleBoard(DriveTrainComponents components) { + driveTrain = DriveTrain.getInstance(); SwerveModule[] swerveModules = components.getSwerveModules(); - tab.addString("Pose",()-> PoseEstimator.getInstance().getPose2d().toString()); - tab.addNumber("CurrentModuleAngle", () -> swerveModules[0].getCurrentAbsoluteDeg()); - tab.addNumber("cancoderAng", () -> swerveModules[0].getAbsEncDeg() - swerveModules[0].getAngleOffset());} -} + tab = Shuffleboard.getTab("DriveTrain"); + tab.addNumber("FL encoderUnits ", () -> swerveModules[0].getAbsEncDeg()); +// tab.addNumber("FR encoderUnits ", () -> swerveModules[1].getAbsEncDeg()); +// tab.addNumber("BL encoderUnits ", () -> swerveModules[2].getAbsEncDeg()); +// tab.addNumber("BR encoderUnits ", () -> swerveModules[3].getAbsEncDeg()); + + tab.addNumber("FL CurrentModuleAngle", () -> swerveModules[0].getCurrentAbsoluteDeg()); +// tab.addNumber("FR CurrentModuleAngle", () -> swerveModules[1].getCurrentAbsoluteDeg()); +// tab.addNumber("BL CurrentModuleAngle", () -> swerveModules[2].getCurrentAbsoluteDeg()); +// tab.addNumber("BR CurrentModuleAngle", () -> swerveModules[3].getCurrentAbsoluteDeg()); + + tab.addNumber("Robot's angle", () -> PoseEstimator.getInstance().getHeading().getDegrees()); + + //tab.add("Turning 90 ",new MoveByDistance(new Pose2d(0, 0, Rotation2d.fromDegrees(90)))); +// rotate 90d, (not working) + + + tab.add("change color Red", new InstantCommand(()-> LED.getInstance().setStrip(255, 0, 0))); +//change color to red + tab.add("change color Rainbow", new InstantCommand(()-> LED.getInstance().setGaming(3))); + //change to rainbow (not working) + + tab.addString("get current color", ()-> LED.getInstance().getCurrentColor()); + // Get current color + + tab.add("change color null", new InstantCommand(()-> LED.getInstance().setStrip(0, 0, 0))); + + + tab.add("change color Blink", new InstantCommand(()-> new Blink(183,30,190,1.5,5))); + tab.add("change color Gaming", new InstantCommand(()-> new Gaming(3))); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/robotControl/DriverOi.java b/src/main/java/frc/robot/robotControl/DriverOi.java index b34c6a1..042bda0 100644 --- a/src/main/java/frc/robot/robotControl/DriverOi.java +++ b/src/main/java/frc/robot/robotControl/DriverOi.java @@ -6,10 +6,13 @@ import commandControl.CommandPlaystation5Controller; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.driveTrain.MoveByDistance; import frc.robot.commands.driveTrain.ResetPose; import frc.robot.commands.driveTrain.SwerveDrive; +import frc.robot.led.LED; import frc.robot.subsystems.driveTrain.DriveTrain; @@ -20,11 +23,13 @@ public class DriverOi { private final CommandConsoleController controller; private DriveTrain driveTrain = DriveTrain.getInstance(); + //private LED led = new LED.g public DriverOi() { controller = new CommandPlaystation5Controller(DRIVE_JOYSTICK_PORT); Trigger resetPose = controller.centerLeft(); - Trigger moveByDistance = controller.rightTrigger(); + //Trigger moveByDistance = controller.rightTrigger(); + Trigger changeColor = controller.rightTrigger(); CommandJoystickAxis xAxis = controller.leftYAxis(); CommandJoystickAxis yAxis = controller.leftXAxis(); @@ -37,6 +42,6 @@ public DriverOi() { () -> true )); resetPose.onTrue(new ResetPose()); - moveByDistance.whileTrue(new MoveByDistance(new Pose2d(3, 0, Rotation2d.fromDegrees(-90)))); + changeColor.onTrue(new InstantCommand(()-> LED.getInstance().setStrip(87, 10, 123))); } } diff --git a/src/main/java/frc/robot/subsystems/driveTrain/DriveTrain.java b/src/main/java/frc/robot/subsystems/driveTrain/DriveTrain.java index 0e5eb02..465bafa 100644 --- a/src/main/java/frc/robot/subsystems/driveTrain/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/driveTrain/DriveTrain.java @@ -4,11 +4,10 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.logging.DriveTrainShuffleBoard; import frc.robot.subsystems.driveTrain.features.KeepAngleController; import frc.robot.subsystems.driveTrain.features.PoseEstimator; import frc.robot.subsystems.driveTrain.features.SwerveModule; - +import frc.robot.logging.DriveTrainShuffleBoard; import static frc.robot.subsystems.driveTrain.DriveTrainConstants.*; public class DriveTrain extends SubsystemBase { @@ -114,7 +113,7 @@ public static void init(DriveTrainComponents components) { if (instance == null) { instance = new DriveTrain(components); } - new DriveTrainShuffleBoard(instance, instance.components); + new DriveTrainShuffleBoard(components); } public static DriveTrain getInstance() { diff --git a/src/main/java/frc/robot/subsystems/driveTrain/features/AutoPilotController.java b/src/main/java/frc/robot/subsystems/driveTrain/features/AutoPilotController.java index fd55bf5..83062f3 100644 --- a/src/main/java/frc/robot/subsystems/driveTrain/features/AutoPilotController.java +++ b/src/main/java/frc/robot/subsystems/driveTrain/features/AutoPilotController.java @@ -21,9 +21,9 @@ public class AutoPilotController { public AutoPilotController(Pose2d deltaPose) { ShuffleboardTab tab = Shuffleboard.getTab("PoseEstimator"); - tab.addNumber("errorX",()->controllerX.getPositionError()); - tab.addNumber("errorY",()->controllerY.getPositionError()); - tab.addNumber("errorRot",()->controllerRot.getPositionError()); +// tab.addNumber("errorX",()->controllerX.getPositionError()); +// tab.addNumber("errorY",()->controllerY.getPositionError()); +// tab.addNumber("errorRot",()->controllerRot.getPositionError()); controllerX = new PIDController( AUTO_PILOT_PID_X.getKp(), diff --git a/src/main/java/frc/robot/subsystems/driveTrain/features/SwerveModule.java b/src/main/java/frc/robot/subsystems/driveTrain/features/SwerveModule.java index ebc117e..dc3cf02 100644 --- a/src/main/java/frc/robot/subsystems/driveTrain/features/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/driveTrain/features/SwerveModule.java @@ -85,7 +85,7 @@ public void updateSwerveModule(SwerveModuleState targetModuleState) { turningController.update(degreesToEnc(targetModuleState.angle.getDegrees())); double angleError = Math.toRadians(encToDegrees(turningController.getCurrentError())); - System.out.println(targetModuleState.speedMetersPerSecond * Math.cos(angleError)); + //System.out.println(targetModuleState.speedMetersPerSecond * Math.cos(angleError)); driveController.update(mpsToEpd(targetModuleState.speedMetersPerSecond * Math.cos(angleError))); }