testing changes

src/main/java/frc/robot/Constants.java

@@ -26,11 +26,11 @@ public static class ElevatorConstants {
 
     public static final float EXTENSION_LIMIT_METERS = 0;
 
-    public static final int ZERO_LIMIT_ID = 2;
+    public static final int ZERO_LIMIT_ID = 3;
 
-    public static final double GROUND_POSITION = 0;
+    public static final double GROUND_POSITION = Units.inchesToMeters(0.5);
     public static final double SPEAKER_POSITION = 0;
-    public static final double AMP_POSITION = Units.inchesToMeters(28);
+    public static final double AMP_POSITION = Units.inchesToMeters(29);
     public static final double CHUTE_POSITION = Units.inchesToMeters(1);
     public static final double TRAP_POSITION = Units.inchesToMeters(30);
     public static final double START_POSITION = 0; 
@@ -91,7 +91,7 @@ public static class IntakeConstants {
     public static final double sensorreached = .3;
     public static final double pivotclockwise = 1;
     public static final double pivotcounterclockwise = -1;
-    public static final double pastsensortime = 0.5;
+    public static final double pastsensortime = 3;
   }
 
   public static class ShooterConstants {
@@ -125,5 +125,6 @@ public static class ClimbConstants {
   public static class LEDConstants {
     public static final int LED_LENGTH = 140;
     public static final int LED_PWM_PORT = 0;
+    public static final double BRIGHTNESS_SCALE_FACTOR = .5;
   }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -43,6 +43,7 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -81,6 +82,9 @@ public class RobotContainer {
     private final JoystickButton rightBumper = new JoystickButton(mechController, XboxController.Button.kRightBumper.value);
     private final JoystickButton xButton = new JoystickButton(mechController, XboxController.Button.kX.value);
     private final JoystickButton yButton = new JoystickButton(mechController, XboxController.Button.kY.value);
+
+    private final GenericHID switchboard = new GenericHID(3);
+    private final JoystickButton redButton = new JoystickButton(switchboard, 5);
 
     private UsbCamera camera1;
     private MjpegServer mjpgserver1;
@@ -142,6 +146,8 @@ private void configureBindings() {
                            new IntakeRollerOutakeCommand(intakeRollerSubsystem)
         )));
 
+        xButton.onTrue(new IntakeRollerOutakeCommand(intakeRollerSubsystem));
+
 
 
 
@@ -194,6 +200,10 @@ private void configureBindings() {
         }));
 
       }
+
+      redButton.onTrue(new RunCommand(() -> {ledSubsystem.setRainbow(true);}, ledSubsystem));
+      redButton.onFalse(new RunCommand(() -> {ledSubsystem.setRainbow(false);}, ledSubsystem));
+
 
     }
 

src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java

@@ -22,6 +22,7 @@ public IntakeRollerFeedCommand(IntakeRollersSubsystem intakeSubsystem){
     public void initialize() {
         // TODO Auto-generated method stub
         intakeSubsystem.setAllRollSpeed(rollerscounterclockwise, rollersclockwise);
+        timer.reset();
     }
 
     @Override

src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java

@@ -21,7 +21,7 @@ public void initialize() {
 
     @Override
     public boolean isFinished() {
-        System.out.println("NOT FINISHED");
+        System.out.println("NOT FINISHED" + Math.abs(pivotSubsystem.getPosition() - angle));
         return (Math.abs(pivotSubsystem.getPosition() - angle) < pivotSubsystem.ERRORTOLERANCE);
     }
 

src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java

@@ -56,7 +56,7 @@ public ElevatorSubsystem() {
 
         timer.start();
 
-        this.zeroLimitSwitch = new DigitalInput(ElevatorConstants.ZERO_LIMIT_ID); 
+        zeroLimitSwitch = new DigitalInput(ElevatorConstants.ZERO_LIMIT_ID); 
         elevatorNetworkTableInstance = NetworkTableInstance.getDefault();
         elevatorNetworkTable = elevatorNetworkTableInstance.getTable("elevator");
         elevatorNetworkTablePositionEntry = elevatorNetworkTable.getEntry("target_position");
@@ -116,9 +116,9 @@ else if(currentTargetState.equals(ElevatorState.TRAP)){
     @Override
     public void periodic(){
         //System.out.println(elevatorNetworkTablePositionEntry.getString("default"));
-        if(timer.advanceIfElapsed(.2)){
-            // System.out.println(Units.metersToInches(getExtensionMeters()));
-        }
+        // if(timer.advanceIfElapsed(.2)){
+        //     System.out.println(zeroLimitSwitch.get());
+        // }
 
         //System.out.println(this.getTargetState());
         if(isManual){
@@ -128,6 +128,7 @@ public void periodic(){
         }
 
         if (zeroLimitSwitch != null && !zeroLimitSwitch.get()){
+            // System.out.println("RESET LIMIT");
             extensionEncoder.setPosition(0); 
         }
 

src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java

@@ -14,6 +14,8 @@ public class LEDSubsystem extends SubsystemBase {
     // private final LEDStrip rishayStrip;
     private final LEDLayer baseLayer;
     private final RotaryLEDLayer driveAngleLayer;
+    private final RotaryLEDLayer driveBackAngleLayer;
+    private final RotaryLEDLayer rainbowLayer;
     private final LEDLayer aprilDetectedLayer;
 
     private final Timer blinkTimer;
@@ -30,15 +32,16 @@ public class LEDSubsystem extends SubsystemBase {
     private boolean colorSensorOff = false;
     private double colorOffset = 0;
 
-    private static final double BRIGHTNESS_SCALE_FACTOR = .5;
+
     private static final double INPUT_DEADZONE = 0.35;
     private static final int LEDS_PER_SEC = 150;
 
     private Color pieceColor = CUBE_COLOR;
     private Color manualColor = new Color(0, 0, 0);
 
-    private boolean manual = false; // If the driver is directly controlling leds
+    private boolean rainbow = false; // If the driver is directly controlling leds
     public boolean pieceGrabbed = false;
+    private boolean enabled = false;
 
 
     private static final Color BASE_COLOR = scaleDownColorBrightness(new Color(255, 0, 0));
@@ -47,6 +50,7 @@ public class LEDSubsystem extends SubsystemBase {
     private static final Color CONE_COLOR = scaleDownColorBrightness(new Color(255, 100, 0));
     private static final Color WHITE = scaleDownColorBrightness(new Color(255,255,255));
     private static final Color COLOR_SENSOR_OFF_COLOR = scaleDownColorBrightness(new Color(255, 0, 0));
+    private static final Color BLUE = scaleDownColorBrightness(new Color(0,0,255));
 
     private final Timer ledTimer; // TODO: better naming
 
@@ -58,6 +62,8 @@ public LEDSubsystem() {
 
         baseLayer = new LEDLayer(LED_LENGTH);
         driveAngleLayer = new RotaryLEDLayer(LED_LENGTH);
+        driveBackAngleLayer = new RotaryLEDLayer(LED_LENGTH);
+        rainbowLayer = new RotaryLEDLayer(LED_LENGTH);
         aprilDetectedLayer = new LEDLayer(LED_LENGTH);
 
         blinkTimer = new Timer();
@@ -89,8 +95,12 @@ public void periodic() {
 
         if(!DriverStation.isEnabled()){
             driverHeading = 2 * Math.PI * ((double) offset) / (double) LED_LENGTH;
+            driveBackAngleLayer.fillColor(null);
+        } else {
+            driveBackAngleLayer.setAngleGroup(driverHeading + Math.PI, 5, 5, BLUE, .7);
         }
-        driveAngleLayer.setAngleGroup(driverHeading, 2, 5, WHITE, .7);
+        driveAngleLayer.setAngleGroup(driverHeading, 5, 5, WHITE, .7);
+
 
         // Update aprilDetectedLayer - white pulses to indicate an april tag detection.
         if (!aprilBlinkTimer.hasElapsed(APRIL_BLINK_DURATION_SECONDS) && aprilBlinkTimer.hasStarted()) {
@@ -99,10 +109,17 @@ public void periodic() {
             aprilDetectedLayer.incrementColors(inc, null);
         }
 
+        if(rainbow){
+            rainbowLayer.setRainbow(offset);
+        } else {
+            rainbowLayer.fillColor(null);
+        }
+
         // Add layers to buffer, set leds
         ledStrip.addLayer(baseLayer);
         ledStrip.addLayer(driveAngleLayer);
-        // ledStrip.addLayer(aprilDetectedLayer);
+        ledStrip.addLayer(driveBackAngleLayer);
+        ledStrip.addLayer(rainbowLayer);
         ledStrip.setBuffer();
 
         // rishayStrip.addLayer(baseLayer);
@@ -112,8 +129,8 @@ public void periodic() {
     /**
      * Toggles whether drivers are manually controlling the color of the LEDs.
      */
-    public void toggleManual() {
-        this.manual = !manual;
+    public void setRainbow(boolean rainbow) {
+        this.rainbow = rainbow;
     }
 
     /**

src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems.leds;
 
 import edu.wpi.first.wpilibj.util.Color;
+import static frc.robot.Constants.LEDConstants.*;
 
 public class RotaryLEDLayer extends LEDLayer {
 
@@ -58,4 +59,14 @@ public void setAngleGroup(double angle, int radius, int borderLength, Color colo
             setLED(offset + radius + i, color,  opacity * (1 - (i / (borderLength + 1.))));
         }
     }
+
+    public void setRainbow(int offset){
+        for(int i = 0; i < colorArray.length; i++){
+            setLED(i + offset, Color.fromHSV(
+                (int) (((double) i) * 180.0 / colorArray.length),
+                (int) 255,
+                (int) (255 * BRIGHTNESS_SCALE_FACTOR)
+            ));
+        }
+    }
 }

src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java

@@ -19,7 +19,7 @@ public class ShooterPivotSubsystem extends SubsystemBase {
     public final double PIVOT_SPEED = 0.1;
     final double GEARBOX_RATIO = 18.16; //ask cadders
     public final double ERRORTOLERANCE = Math.toRadians(2); //error tolerance for pid
-    final int LIMIT_SWITCH_ID = 3; //placeholder
+    final int LIMIT_SWITCH_ID = 4; //placeholder
     final double CONVERSION_FACTOR = Math.PI/(2.*4.57);
 
     //motors