diff --git a/.gradle/6.0.1/executionHistory/executionHistory.bin b/.gradle/6.0.1/executionHistory/executionHistory.bin index 5b39978..9697be0 100644 Binary files a/.gradle/6.0.1/executionHistory/executionHistory.bin and b/.gradle/6.0.1/executionHistory/executionHistory.bin differ diff --git a/.gradle/6.0.1/executionHistory/executionHistory.lock b/.gradle/6.0.1/executionHistory/executionHistory.lock index 6740a78..53b9990 100644 Binary files a/.gradle/6.0.1/executionHistory/executionHistory.lock and b/.gradle/6.0.1/executionHistory/executionHistory.lock differ diff --git a/.gradle/6.0.1/fileHashes/fileHashes.bin b/.gradle/6.0.1/fileHashes/fileHashes.bin index 595f833..e927b81 100644 Binary files a/.gradle/6.0.1/fileHashes/fileHashes.bin and b/.gradle/6.0.1/fileHashes/fileHashes.bin differ diff --git a/.gradle/6.0.1/fileHashes/fileHashes.lock b/.gradle/6.0.1/fileHashes/fileHashes.lock index 97dc64f..a51e60e 100644 Binary files a/.gradle/6.0.1/fileHashes/fileHashes.lock and b/.gradle/6.0.1/fileHashes/fileHashes.lock differ diff --git a/.gradle/6.0.1/fileHashes/resourceHashesCache.bin b/.gradle/6.0.1/fileHashes/resourceHashesCache.bin index 08185f9..5007228 100644 Binary files a/.gradle/6.0.1/fileHashes/resourceHashesCache.bin and b/.gradle/6.0.1/fileHashes/resourceHashesCache.bin differ diff --git a/.gradle/6.0.1/javaCompile/classAnalysis.bin b/.gradle/6.0.1/javaCompile/classAnalysis.bin index 965cf90..3a5baa0 100644 Binary files a/.gradle/6.0.1/javaCompile/classAnalysis.bin and b/.gradle/6.0.1/javaCompile/classAnalysis.bin differ diff --git a/.gradle/6.0.1/javaCompile/jarAnalysis.bin b/.gradle/6.0.1/javaCompile/jarAnalysis.bin index 2031c3c..6b56214 100644 Binary files a/.gradle/6.0.1/javaCompile/jarAnalysis.bin and b/.gradle/6.0.1/javaCompile/jarAnalysis.bin differ diff --git a/.gradle/6.0.1/javaCompile/javaCompile.lock b/.gradle/6.0.1/javaCompile/javaCompile.lock index e2eb0bd..9029d93 100644 Binary files a/.gradle/6.0.1/javaCompile/javaCompile.lock and b/.gradle/6.0.1/javaCompile/javaCompile.lock differ diff --git a/.gradle/6.0.1/javaCompile/taskHistory.bin b/.gradle/6.0.1/javaCompile/taskHistory.bin index 4b7acaf..2d490cd 100644 Binary files a/.gradle/6.0.1/javaCompile/taskHistory.bin and b/.gradle/6.0.1/javaCompile/taskHistory.bin differ diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index b205252..31e4105 100644 Binary files a/.gradle/buildOutputCleanup/buildOutputCleanup.lock and b/.gradle/buildOutputCleanup/buildOutputCleanup.lock differ diff --git a/.gradle/buildOutputCleanup/outputFiles.bin b/.gradle/buildOutputCleanup/outputFiles.bin index 414e3c3..4bc15b4 100644 Binary files a/.gradle/buildOutputCleanup/outputFiles.bin and b/.gradle/buildOutputCleanup/outputFiles.bin differ diff --git a/build/debug/debuginfo.json b/build/debug/debuginfo.json index 4c84b93..af4759e 100644 --- a/build/debug/debuginfo.json +++ b/build/debug/debuginfo.json @@ -5,5 +5,12 @@ "debugfile": "frcJava_roborio.debugconfig", "project": "2020Robot", "language": "java" + }, + { + "artifact": "frcJava (in project 2020RobotOld)", + "target": "roborio", + "debugfile": "frcJava_roborio.debugconfig", + "project": "2020RobotOld", + "language": "java" } ] \ No newline at end of file diff --git a/build/debug/riopartial/2020RobotOld.json b/build/debug/riopartial/2020RobotOld.json new file mode 100644 index 0000000..785a527 --- /dev/null +++ b/build/debug/riopartial/2020RobotOld.json @@ -0,0 +1,9 @@ +[ + { + "artifact": "frcJava (in project 2020RobotOld)", + "target": "roborio", + "debugfile": "frcJava_roborio.debugconfig", + "project": "2020RobotOld", + "language": "java" + } +] \ No newline at end of file diff --git a/build/libs/2020RobotOld.jar b/build/libs/2020RobotOld.jar new file mode 100644 index 0000000..999640e Binary files /dev/null and b/build/libs/2020RobotOld.jar differ diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b039d5f..58f110c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,7 +15,8 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.core751.subsystems.DifferentialDriveTrain; import frc.robot.core751.subsystems.DifferentialDriveTrain.driveMotor; -import frc.robot.core751.subsystems.LightStrip.Orientation; +import frc.robot.core751.wrappers.OverrideableJoystick; + /** * The Constants class provides a convenient place for teams to hold robot-wide @@ -48,7 +49,7 @@ public int getButtonMapping() { } - public static Joystick driverStick = new Joystick(Constants.driveStickPort); + public static OverrideableJoystick driverStick = new OverrideableJoystick(Constants.driveStickPort); /*================================/ /===========Drive Train===========/ @@ -104,17 +105,6 @@ public int getButtonMapping() { public static Button panelToggleButton = new JoystickButton(driverStick, Controller.Y.buttonNum); - /*======================================/ - /===========Lightstrip=================/ - /=====================================*/ - public static int FTLEDPort = 0; - public static int FTLEDLength = 15; - public static Orientation FTLEDOrientation = Orientation.HORIZONTAL; - - public static int FBLEDPort = 1; - public static int FBLEDLength = 13; - public static Orientation FBLEDOrientation = Orientation.HORIZONTAL; - // Robot-specific PIDTrajectory constants public static double trackWidthMeters = 0.0762; // horizontal distance between the wheels diff --git a/src/main/java/frc/robot/Replay.java b/src/main/java/frc/robot/Replay.java new file mode 100644 index 0000000..05422c8 --- /dev/null +++ b/src/main/java/frc/robot/Replay.java @@ -0,0 +1,16 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot; + +/** + * Add your docs here. + */ +public class Replay { + public static Double[][] array = {{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.15748031437397003D},{0.4330708682537079D,0.15748031437397003D},{0.4330708682537079D,0.15748031437397003D},{0.4330708682537079D,0.15748031437397003D},{0.4330708682537079D,0.17322835326194763D},{0.4330708682537079D,0.18110236525535583D},{0.4330708682537079D,0.18110236525535583D},{0.4330708682537079D,0.18110236525535583D},{0.4330708682537079D,0.18110236525535583D},{0.4330708682537079D,0.18897637724876404D},{0.4330708682537079D,0.18897637724876404D},{0.4330708682537079D,0.18897637724876404D},{0.4330708682537079D,0.18110236525535583D},{0.4330708682537079D,0.18110236525535583D},{0.4330708682537079D,0.18110236525535583D},{0.4330708682537079D,0.16535432636737823D},{0.4330708682537079D,0.14960630238056183D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.14173229038715363D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.12598425149917603D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11811023950576782D},{0.4330708682537079D,0.11023622006177902D},{0.4330708682537079D,0.11023622006177902D},{0.4330708682537079D,0.11023622006177902D},{0.4330708682537079D,0.09448818862438202D},{0.4251968562602997D,0.07086614519357681D},{0.4251968562602997D,0.06299212574958801D},{0.4251968562602997D,0.05511811003088951D},{0.4251968562602997D,0.05511811003088951D},{0.4251968562602997D,0.05511811003088951D},{0.4173228442668915D,0.05511811003088951D},{0.4173228442668915D,0.05511811003088951D},{0.4094488322734833D,0.05511811003088951D},{0.4094488322734833D,0.05511811003088951D},{0.4094488322734833D,0.05511811003088951D},{0.4094488322734833D,0.05511811003088951D},{0.4094488322734833D,0.05511811003088951D},{0.4094488322734833D,0.05511811003088951D},{0.4015747904777527D,0.07874015718698502D},{0.3937007784843445D,0.10236220806837082D},{0.3858267664909363D,0.11023622006177902D},{0.3779527544975281D,0.12598425149917603D},{0.3779527544975281D,0.12598425149917603D},{0.3779527544975281D,0.12598425149917603D},{0.3779527544975281D,0.12598425149917603D},{0.3779527544975281D,0.12598425149917603D},{0.3700787425041199D,0.14173229038715363D,1D},{0.3700787425041199D,0.14173229038715363D,1D},{0.3700787425041199D,0.14173229038715363D,1D},{0.3700787425041199D,0.14173229038715363D,1D},{0.3700787425041199D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3779527544975281D,0.14173229038715363D,1D},{0.3858267664909363D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3937007784843445D,0.14173229038715363D,1D},{0.3779527544975281D,0.11023622006177902D,1D},{0.3385826647281647D,0.08661417663097382D,1D},{0.3385826647281647D,0.08661417663097382D,1D},{0.10236220806837082D,0.0D,1D},{0.007874015718698502D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D},{-0.015625D,0.0D,1D}}; + +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c842fec..016744c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,8 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.commands.Ball.*; import frc.robot.commands.Panel.*; +import frc.robot.core751.commands.JoystickPlayer; +import frc.robot.core751.commands.JoystickRecorder; import frc.robot.core751.commands.Drivetrain.ArcadeDrive; import frc.robot.core751.commands.Drivetrain.ReversableArcadeDrive; import frc.robot.core751.commands.Drivetrain.SwitchDriveDirection; @@ -37,31 +39,26 @@ public class RobotContainer { // private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); + private final Camera camera0 = new Camera(0); + private final Camera camera1 = new Camera(1); private final DifferentialDriveTrain differentialDriveTrain = new DifferentialDriveTrain(Constants.leftDrivetrainIDs, Constants.rightDrivetrainIDs, Constants.driveTrainMotorType, Constants.driveMotorProfile, Constants.driveInvertLeft, Constants.driveInvertRight); private final ReversableArcadeDrive reversableArcadeDrive = new ReversableArcadeDrive(Constants.driverStick, differentialDriveTrain); - private final SwitchDriveDirection switchDriveDirection = new SwitchDriveDirection(differentialDriveTrain); + private final SwitchDriveDirection switchDriveDirection = new SwitchDriveDirection(differentialDriveTrain,0,1); - private final LightStrip[] lightStrips = new LightStrip[] { - new LightStrip(0, Constants.FTLEDLength, Constants.FTLEDOrientation), - //new LightStrip(1, Constants.FBLEDLength, Constants.FBLEDOrientation) - }; - private final TeamColorLights teamColorLights = new TeamColorLights(lightStrips); - public final Panel panel = new Panel(Constants.leftColorsensorPort, Constants.rightColorsensorPort, Constants.panelSpinID, Constants.panelRotateID, Constants.panelTopLimitPort, Constants.panelBottomLimitPort); - private final GoToColor goToColor = new GoToColor(lightStrips, panel); - private final RotateWheel rotateWheel = new RotateWheel(lightStrips, panel); private final ManualPanel manualPanel = new ManualPanel(panel, Constants.driverStick, Constants.rightTrigger, Constants.leftTrigger); - private final RotateThenSelect rotateThenSelect = new RotateThenSelect(panel, lightStrips); - private final TogglePanelPosition togglePanelPosition = new TogglePanelPosition(panel); - - private final Camera camera = new Camera(0); private final Ball ball = new Ball(Constants.ballIntakeMotorID, Constants.ballPolycordMotorID, Constants.ballOutakeMotorID); private final DefaultBall defaultBall = new DefaultBall(ball, Constants.driverStick, Constants.ballLBumper, Constants.ballRBumper, Constants.ballOutButton, Constants.ballReverseOutButton); private final PowerDistributionPanel pdp = new PowerDistributionPanel(); + private final JoystickRecorder joystickRecorder = new JoystickRecorder(Constants.driverStick); + private final JoystickPlayer joystickPlayer = new JoystickPlayer(Replay.array, Constants.driverStick); + + + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -77,23 +74,18 @@ public RobotContainer() { * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - for (LightStrip l : lightStrips) { - l.setDefaultCommand(teamColorLights); - } + panel.setDefaultCommand(manualPanel); differentialDriveTrain.setDefaultCommand(reversableArcadeDrive); ball.setDefaultCommand(defaultBall); - Constants.panelToggleButton.whenPressed(togglePanelPosition); Constants.driveSwitchDirectionButton.whenPressed(switchDriveDirection); SmartDashboard.putData(pdp); + SmartDashboard.putData(joystickRecorder); + SmartDashboard.putData(joystickPlayer); - SmartDashboard.putData(togglePanelPosition); - SmartDashboard.putData(goToColor); - SmartDashboard.putData(rotateWheel); - SmartDashboard.putData(rotateThenSelect); } diff --git a/src/main/java/frc/robot/commands/Panel/GoToColor.java b/src/main/java/frc/robot/commands/Panel/GoToColor.java deleted file mode 100644 index ba91b8e..0000000 --- a/src/main/java/frc/robot/commands/Panel/GoToColor.java +++ /dev/null @@ -1,92 +0,0 @@ -package frc.robot.commands.Panel; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.core751.subsystems.LightStrip; -import frc.robot.core751.subsystems.LightStrip.PostProccessingEffects; -import frc.robot.subsystems.Panel; -import frc.robot.subsystems.Panel.PositionState; -import frc.robot.subsystems.Panel.WheelColor; - -public class GoToColor extends CommandBase { - - public LightStrip[] lightStrips; - public Panel panel; - public WheelColor wheelColor; - public int[] currentHSV; - public WheelColor lastColor; - public WheelColor targetColor; - public boolean finished; - - - public GoToColor(LightStrip[] lightStrips, Panel panel) { - this.lightStrips = lightStrips; - this.panel = panel; - for (LightStrip l : lightStrips) { - addRequirements(l); - } - addRequirements(panel); - this.lastColor = WheelColor.UNKNOWN; - this.currentHSV = new int[]{300, 255, 255}; - this.wheelColor = WheelColor.UNKNOWN; - } - - @Override - public void initialize() { - if (this.panel.positionState !=PositionState.UP) { - this.finished = true; - return; - }else { - this.panel.setPositionMotor(this.panel.getPassiveUpSpeed()); - } - this.finished = false; - for (LightStrip l : lightStrips) { - l.clearEffects(); - l.setEffect(PostProccessingEffects.WAVE); - l.setEffect(PostProccessingEffects.HUE_SHIFT); - l.update(); - } - this.panel.clearRotations(); - this.targetColor = WheelColor.getTargetColor(); - } - - @Override - - public void execute(){ - this.wheelColor = this.panel.getColor(); - - SmartDashboard.putNumber("Rotations", this.panel.getRotations()); - SmartDashboard.putString("Color", this.wheelColor.name()); - for (LightStrip l : lightStrips) { - l.fillHSV(wheelColor.HSV[0], wheelColor.HSV[0], wheelColor.HSV[0]); - l.update(); - } - this.lastColor = this.wheelColor; - if (this.wheelColor == WheelColor.BETWEEN) { - this.panel.setSpinMotor(0.5); - } - if (this.wheelColor == WheelColor.UNKNOWN) { - this.panel.stopSpinMotor(); - this.finished = true; - } - - int dist = this.wheelColor.colorDist(this.targetColor); - if (dist == 0) { - this.panel.stopSpinMotor(); - this.finished = true; - } - if (dist == 2) { - this.panel.setSpinMotor(1); - }else { - this.panel.setSpinMotor(dist*0.5); - } - - } - - @Override - public boolean isFinished(){ - return this.finished; - } - - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Panel/RotateThenSelect.java b/src/main/java/frc/robot/commands/Panel/RotateThenSelect.java deleted file mode 100644 index ae6dcdb..0000000 --- a/src/main/java/frc/robot/commands/Panel/RotateThenSelect.java +++ /dev/null @@ -1,14 +0,0 @@ -package frc.robot.commands.Panel; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.core751.subsystems.LightStrip; -import frc.robot.subsystems.Panel; - -public class RotateThenSelect extends SequentialCommandGroup { - - public RotateThenSelect (Panel panel, LightStrip[] lightStrips) { - addCommands(new RotateWheel(lightStrips, panel)); - addCommands(new GoToColor(lightStrips, panel)); - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Panel/RotateWheel.java b/src/main/java/frc/robot/commands/Panel/RotateWheel.java deleted file mode 100644 index d657a8b..0000000 --- a/src/main/java/frc/robot/commands/Panel/RotateWheel.java +++ /dev/null @@ -1,100 +0,0 @@ -package frc.robot.commands.Panel; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.core751.subsystems.LightStrip; -import frc.robot.core751.subsystems.LightStrip.PostProccessingEffects; -import frc.robot.subsystems.Panel; -import frc.robot.subsystems.Panel.PositionState; -import frc.robot.subsystems.Panel.WheelColor; - -public class RotateWheel extends CommandBase { - - public LightStrip[] lightStrips; - public Panel panel; - public WheelColor wheelColor; - public int[] currentHSV; - public WheelColor lastColor; - public boolean finished; - public float target; - - - public RotateWheel(LightStrip[] lightStrips, Panel panel) { - this.lightStrips = lightStrips; - this.panel = panel; - addRequirements(panel); - for (LightStrip l : lightStrips) { - addRequirements(l); - } - this.lastColor = WheelColor.UNKNOWN; - this.currentHSV = new int[]{300, 255, 255}; - this.wheelColor = WheelColor.UNKNOWN; - } - - @Override - public void initialize() { - if (this.panel.positionState !=PositionState.UP) { - this.finished = true; - return; - }else { - this.panel.setPositionMotor(this.panel.getPassiveUpSpeed()); - } - this.finished = false; - for (LightStrip l : lightStrips) { - l.clearEffects(); - l.setEffect(PostProccessingEffects.WAVE); - l.setEffect(PostProccessingEffects.HUE_SHIFT); - l.update(); - } - this.panel.clearRotations(); - - WheelColor targetColor = WheelColor.getTargetColor(); - // if (targetColor == WheelColor.UNKNOWN) { - // this.target = 3.5f; - // } else { - // int dist = this.wheelColor.colorDist(targetColor); - // if (dist < 0) { - // this.target = -3.125f + (dist*0.125f); - // } else { - // this.target = -3.125f + (dist*0.125f); - // } - // } - this.target = 3.5f; - } - - @Override - - public void execute(){ - this.wheelColor = this.panel.getColor(); - SmartDashboard.putNumber("Target Rot", this.target); - SmartDashboard.putNumber("Rotations", this.panel.getRotations()); - for (LightStrip l : lightStrips) { - l.fillHSV(wheelColor.HSV[0], wheelColor.HSV[0], wheelColor.HSV[0]); - l.update(); - } - if (this.wheelColor == WheelColor.BETWEEN) { - this.panel.setSpinMotor(1); - } - double speed = 1; - if (Math.abs(Math.abs(this.target) - Math.abs(this.panel.getRotations()))<=0.125f) { - speed = 1; - } - this.lastColor = this.wheelColor; - SmartDashboard.putNumber("speed", speed); - //double speed = 1; - if (this.panel.getRotations() < this.target) { - this.panel.setSpinMotor(-speed); - }else if (this.panel.getRotations() > this.target) { - this.panel.setSpinMotor(speed); - }else { - this.panel.stopSpinMotor(); - this.finished = true; - } - - } - - @Override - public boolean isFinished(){ - return this.finished; - } -} diff --git a/src/main/java/frc/robot/commands/Panel/SensorLights.java b/src/main/java/frc/robot/commands/Panel/SensorLights.java deleted file mode 100644 index d447fb5..0000000 --- a/src/main/java/frc/robot/commands/Panel/SensorLights.java +++ /dev/null @@ -1,60 +0,0 @@ -package frc.robot.commands.Panel; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.core751.subsystems.LightStrip; -import frc.robot.core751.subsystems.LightStrip.PostProccessingEffects; -import frc.robot.subsystems.Panel; -import frc.robot.subsystems.Panel.WheelColor; - -public class SensorLights extends CommandBase { - - public LightStrip[] lightStrips; - public Panel panel; - public WheelColor wheelColor; - public int[] currentHSV; - public WheelColor lastColor; - - public SensorLights(LightStrip[] lightStrips, Panel panel) { - this.lightStrips = lightStrips; - this.panel = panel; - addRequirements(panel); - for (LightStrip l : lightStrips) { - addRequirements(l); - } - this.lastColor = WheelColor.UNKNOWN; - this.currentHSV = new int[]{300, 255, 255}; - this.wheelColor = WheelColor.UNKNOWN; - } - - @Override - public void initialize() { - int[] c = panel.getColor().HSV; - for (LightStrip l : lightStrips) { - l.clearEffects(); - l.setEffect(PostProccessingEffects.WAVE); - l.setEffect(PostProccessingEffects.HUE_SHIFT); - l.update(); - } - this.panel.clearRotations(); - } - - @Override - - public void execute(){ - this.panel.setSpinMotor(0); - this.panel.stopSpinMotor(); - this.wheelColor = this.panel.getColor(); - - SmartDashboard.putNumber("Rotations", this.panel.getRotations()); - SmartDashboard.putString("Color", this.wheelColor.name()); - for (LightStrip l : lightStrips) { - l.fillHSV(wheelColor.HSV[0], wheelColor.HSV[0], wheelColor.HSV[0]); - l.update(); - } - this.lastColor = this.wheelColor; - - } - - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/core751 b/src/main/java/frc/robot/core751 index 6ff7292..8e72898 160000 --- a/src/main/java/frc/robot/core751 +++ b/src/main/java/frc/robot/core751 @@ -1 +1 @@ -Subproject commit 6ff7292ec6ff4a857b1b8d67b4a0d09988e17cce +Subproject commit 8e72898c80c94bbf467432320672f9c3eb4270bd