Control Panel Stage 2 and 3.

.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"
+            }
+          }
+        }
       }
     }
   ],

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 {

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));
   }

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));
   }
 }

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());
   }
 }

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);
   }
 }

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() {

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