diff --git a/.shuffleboard/shuffleboard.json b/.shuffleboard/shuffleboard.json index ab7d7c1..58597e4 100644 --- a/.shuffleboard/shuffleboard.json +++ b/.shuffleboard/shuffleboard.json @@ -664,7 +664,19 @@ "showGrid": true, "hgap": 16.0, "vgap": 16.0, - "tiles": {} + "tiles": { + "3,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Command", + "_source0": "network_table:///SmartDashboard/Run Brushed", + "_title": "SmartDashboard/Run Brushed" + } + } + } } } ], diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dd520b4..061c295 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,6 +7,8 @@ package frc.robot; +import edu.wpi.first.wpilibj.AnalogInput; + /** * The Constants class provides a convenient place for teams to hold robot-wide * numerical or boolean constants. This class should not be used for any other @@ -31,7 +33,7 @@ public final static class DriveConstants { public static final double NORMAL_SPEED = 0.5; public static final int driverController = 0; public static final int mechanismController = 1; - public static final double TURBO_SPEED = 1; + public static final double TURBO_SPEED = .85; public static final double SLOW_SPEED = 0.25; } @@ -41,15 +43,16 @@ public final static class ControllerConstants { public static final int LEFT_TRIGGER = 2; public static final double TRIGGER_ACTIVATION_THRESHOLD = .3; public static final int POV_ANGLE_UP = 0; - public static final int POV_ANGLE_LEFT = 270; - public static final int POV_ANGLE_RIGHT = 90; + public static final int POV_ANGLE_LEFT = 90; + public static final int POV_ANGLE_RIGHT = 270; + public static final int POV_ANGLE_DOWN = 180; } public final static class HangConstants { public static final int LIFT_MOTOR = 8; public static final int WINCH_MOTOR = 7; - public static final int LIFT_ENCODER_VALUE = 262; + public static final int LIFT_ENCODER_VALUE = 240; public static final int BOTTOM_LIFT_ENCODER = 225; public static final double LIFT_SPEED = 1; public static final double WINCH_MOTOR_SPEED = 1; @@ -69,7 +72,7 @@ public final static class ControlPanelConstants { public static final int REVERSE_CHANNEL = 0; public static final int CONTROL_PANEL_MOTOR = 5; public static final int CONTROL_PANEL_LIMIT = 30; - public static final double MOTOR_SPEED = 0.75; + public static final double MOTOR_SPEED = 0.25; } public final static class LowPressureConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 68398fd..17108fb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -123,6 +123,7 @@ private void configureButtonBindings() { JoystickButton mechanismRightBumper = new JoystickButton(mechanismController, Button.kBumperRight.value); POVButton mechanismDPadLeft = new POVButton(mechanismController, ControllerConstants.POV_ANGLE_LEFT); POVButton mechanismDPadRight = new POVButton(mechanismController, ControllerConstants.POV_ANGLE_RIGHT); + POVButton mechanismDPadDown = new POVButton(mechanismController, ControllerConstants.POV_ANGLE_DOWN); JoystickButton mechanismY = new JoystickButton(mechanismController, Button.kY.value); JoystickButton mechanismA = new JoystickButton(mechanismController, Button.kA.value); JoystickButton mechanismB = new JoystickButton(mechanismController, Button.kB.value); @@ -140,11 +141,12 @@ private void configureButtonBindings() { mechanismLeftTrigger.whenActive(new AdvanceConveyor(lowScoringSubsystem)); mechanismLeftBumper.whenPressed(new WithdrawConveyor(lowScoringSubsystem)); mechanismDPadRight.whenPressed(new ScoreStageTwoRotations(controlPanelSubsystem)); - // mechanismDPadLeft.whenPressed(new ScoreStageThree(colorSensor, - // controlPanelSubsystem)); + mechanismDPadLeft.whenPressed(new ScoreStageThree(colorSensor, controlPanelSubsystem)); + mechanismDPadDown.whenPressed(new StopControlPanel(controlPanelSubsystem)); mechanismX.whenHeld(new RollersIn(lowScoringSubsystem)).whenReleased(new RollersOff(lowScoringSubsystem)); mechanismB.whenHeld(new RollersOut(lowScoringSubsystem)).whenReleased(new RollersOff(lowScoringSubsystem)); mechanismY.whenPressed(new EngageControlPanelWheel(controlPanelSubsystem)); + // mechanismX.whenPressed(new ToggleStreamMode(limelight)); // mechanismA.whenPressed(new ToggleCameraMode(limelight)); } diff --git a/src/main/java/frc/robot/commands/ScoreLowAndTrench.java b/src/main/java/frc/robot/commands/ScoreLowAndTrench.java index 99eeb2a..e654037 100644 --- a/src/main/java/frc/robot/commands/ScoreLowAndTrench.java +++ b/src/main/java/frc/robot/commands/ScoreLowAndTrench.java @@ -19,8 +19,16 @@ public class ScoreLowAndTrench extends SequentialCommandGroup { * Creates a new ScoreAndTurn. */ public ScoreLowAndTrench(DriveSubsystem driveSubsystem, LowScoringSubsystem lowScoringSubsystem) { + // addCommands(new DriveAndScoreLow(driveSubsystem, lowScoringSubsystem), new + // TurnDegrees(driveSubsystem, -.3, 45), + // new DriveDistance(driveSubsystem, .5, 27), new TurnDegrees(driveSubsystem, + // .3, 38), + // new DriveDistance(driveSubsystem, .5, 13), new + // GetPowercellsInTrench(driveSubsystem, lowScoringSubsystem)); + addCommands(new DriveAndScoreLow(driveSubsystem, lowScoringSubsystem), new TurnDegrees(driveSubsystem, -.3, 45), new DriveDistance(driveSubsystem, .5, 27), new TurnDegrees(driveSubsystem, .3, 38), - new DriveDistance(driveSubsystem, .5, 13), new GetPowercellsInTrench(driveSubsystem, lowScoringSubsystem)); + new DriveDistance(driveSubsystem, .5, 13), new GetPowercellsInTrench(driveSubsystem, lowScoringSubsystem), + new DriveDistance(driveSubsystem, .45, 49)); } } diff --git a/src/main/java/frc/robot/commands/ScoreStageThree.java b/src/main/java/frc/robot/commands/ScoreStageThree.java index 823d405..c36181f 100644 --- a/src/main/java/frc/robot/commands/ScoreStageThree.java +++ b/src/main/java/frc/robot/commands/ScoreStageThree.java @@ -22,7 +22,7 @@ public class ScoreStageThree extends CommandBase { private ControlPanelSubsystem controlPanel; private Color detected; - private Color actual; + private Color target; private Color targetColor; private double velocity; @@ -44,10 +44,10 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - controlPanel.setMotorSpeed(10); - velocity = controlPanel.getVelocity(); - detected = colorSensor.getSensorColor(); - Dashboard.putDebugNumber("Panel RPM", velocity / 8.f); + controlPanel.setMotorSpeed(.1); + // velocity = controlPanel.getVelocity(); + // detected = colorSensor.getSensorColor(); + // Dashboard.putDebugNumber("Panel RPM", velocity / 8.f); } // Called once the command ends or is interrupted. @@ -60,7 +60,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { // color we want is 2 ahead, so we need to go before - actual = ColorSensor.colors.get(colorSensor.getColorIndex(targetColor) - 2); - return colorSensor.isSameColor(actual, detected); + // target = colorSensor.getTargetColor(); + return colorSensor.isSameColor(colorSensor.getTargetColor(), colorSensor.getSensorColor()); } } diff --git a/src/main/java/frc/robot/commands/ScoreStageTwoRotations.java b/src/main/java/frc/robot/commands/ScoreStageTwoRotations.java index db52dfd..5417b0d 100644 --- a/src/main/java/frc/robot/commands/ScoreStageTwoRotations.java +++ b/src/main/java/frc/robot/commands/ScoreStageTwoRotations.java @@ -47,11 +47,12 @@ public void execute() { @Override public void end(boolean interrupted) { controlPanel.stopMotor(); + controlPanel.resetEncoders(); } // Returns true when the command should end. @Override public boolean isFinished() { - return Math.abs(rotations) >= Math.abs(32.0f); + return Math.abs(rotations) >= Math.abs(32.0f * 4); } } diff --git a/src/main/java/frc/robot/subsystems/ColorSensor.java b/src/main/java/frc/robot/subsystems/ColorSensor.java index 5308727..a5fa930 100644 --- a/src/main/java/frc/robot/subsystems/ColorSensor.java +++ b/src/main/java/frc/robot/subsystems/ColorSensor.java @@ -65,6 +65,20 @@ private Color getColorMatch() { return NullTarget; } + public Color getTargetColor() { + if (givenColor == RedTarget) { + return BlueTarget; + } else if (givenColor == YellowTarget) { + return GreenTarget; + } else if (givenColor == BlueTarget) { + return RedTarget; + } else if (givenColor == GreenTarget) { + return YellowTarget; + } else { + return RedTarget; + } + } + private void createColorChooser() { SendableRegistry.add(colorChooser, "Color Chooser"); SendableRegistry.setName(colorChooser, "Color Chooser"); @@ -101,23 +115,28 @@ public void periodic() { } ColorMatchResult match = m_colorMatcher.matchClosestColor(detectedColor); - if (match.color == BlueTarget) { + SmartDashboard.putString("Detected Color", getColorString(match.color)); + SmartDashboard.putString("Given Color", getColorString(getTargetColor())); + Dashboard.putDebugNumber("Confidence", match.confidence); + Dashboard.putDebugNumber("Detected Red", match.color.red); + Dashboard.putDebugNumber("Detected Green", match.color.green); + Dashboard.putDebugNumber("Detected Blue", match.color.blue); + } + + public String getColorString(Color color) { + String colorString; + if (color == BlueTarget) { colorString = "BLUE"; - } else if (match.color == RedTarget) { + } else if (color == RedTarget) { colorString = "RED"; - } else if (match.color == GreenTarget) { + } else if (color == GreenTarget) { colorString = "GREEN"; - } else if (match.color == YellowTarget) { + } else if (color == YellowTarget) { colorString = "YELLOW"; } else { colorString = "UNKNOWN"; } - - SmartDashboard.putString("Detected Color", colorString); - Dashboard.putDebugNumber("Confidence", match.confidence); - Dashboard.putDebugNumber("Detected Red", match.color.red); - Dashboard.putDebugNumber("Detected Green", match.color.green); - Dashboard.putDebugNumber("Detected Blue", match.color.blue); + return colorString; } public Color getSensorColor() { diff --git a/src/main/java/frc/robot/subsystems/ControlPanelSubsystem.java b/src/main/java/frc/robot/subsystems/ControlPanelSubsystem.java index 981e2b9..2ab9d82 100644 --- a/src/main/java/frc/robot/subsystems/ControlPanelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControlPanelSubsystem.java @@ -9,9 +9,13 @@ import com.revrobotics.CANEncoder; import com.revrobotics.CANSparkMax; +import com.revrobotics.EncoderType; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.wpilibj.AnalogEncoder; +import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ControlPanelConstants; @@ -21,17 +25,15 @@ public class ControlPanelSubsystem extends SubsystemBase { private DoubleSolenoid controlPanelSolenoid = new DoubleSolenoid(ControlPanelConstants.FORWARD_CHANNEL, ControlPanelConstants.REVERSE_CHANNEL); private CANSparkMax controlPanelMotor = new CANSparkMax(ControlPanelConstants.CONTROL_PANEL_MOTOR, - MotorType.kBrushless); - - private CANEncoder controlPanelEncoder; + MotorType.kBrushed); private boolean pistonsForward = false; + private CANEncoder controlPanelEncoder; public ControlPanelSubsystem() { controlPanelSolenoid.set(Value.kOff); controlPanelMotor.restoreFactoryDefaults(); controlPanelMotor.setSmartCurrentLimit(ControlPanelConstants.CONTROL_PANEL_LIMIT); - - controlPanelEncoder = controlPanelMotor.getEncoder(); + controlPanelEncoder = controlPanelMotor.getEncoder(EncoderType.kQuadrature, 1024); controlPanelEncoder.setPosition(0); } @@ -70,8 +72,8 @@ public double getRotations() { } public double getVelocity() { - // return 0.0; - return controlPanelEncoder.getVelocity(); + return 0.0; + // return controlPanelEncoder.getVelocity(); } @Override