Led testing ethan

Shuffle-board-layouts/shuffle-board-week-5.json

@@ -1014,9 +1014,9 @@
     }
   ],
   "windowGeometry": {
-    "x": 55.0,
-    "y": 2.0,
-    "width": 1870.0,
-    "height": 1085.0
+    "x": 0.800000011920929,
+    "y": 0.800000011920929,
+    "width": 1534.4000244140625,
+    "height": 814.4000244140625
   }
 }

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

@@ -23,7 +23,7 @@
  */
 public final class Constants {
   public static final Mode CURRENT_MODE = Mode.REAL;
-  public static final boolean TUNING_MODE = true;
+  public static final boolean TUNING_MODE = false;
 
   public static enum Mode {
     /** Running on a real robot. */

src/main/java/frc/robot/HardwareConstants.java

@@ -5,7 +5,7 @@
 import edu.wpi.first.math.geometry.Translation3d;
 
 public class HardwareConstants {
-    public static final int NUMBER_OF_CAMERAS = 2;
+    public static final int NUMBER_OF_CAMERAS = 3;
     public class CanIds {
         // |====================== SWERVE MODULE CAN IDs ======================|
         public static int FL_DRIVE = 1;
@@ -59,37 +59,51 @@ public class ComponentTransformations {
         * positive X, and where the left side of the robot is the negative Y.
         */
         // How far foward/backward the camera is from robot center.
-        private static final double CAMERA_ONE_X_FROM_ROBOT_CENTER = -0.279; // Mechanical offset + 1/8" offset
+        private static final double CAMERA_ONE_X_FROM_ROBOT_CENTER = -0.032 + 0.279; // Mechanical offset + 1/8" offset
         // How far left/right the camera is from robot center.
-        private static final double CAMERA_ONE_Y_FROM_ROBOT_CENTER = 0.03 - 0.127;
+        private static final double CAMERA_ONE_Y_FROM_ROBOT_CENTER = 0.273;
         // How far up/down the camera is from center if we look at robot from side in 3D
         // space.
         private static final double CAMERA_ONE_Z_FROM_ROBOT_CENTER = 0.2583;
 
         private static final double CAMERA_ONE_ROLL = 0;
         private static final double CAMERA_ONE_PITCH = Math.toRadians(20.0);
-        private static final double CAMERA_ONE_YAW = Math.toRadians(180.0);
+        private static final double CAMERA_ONE_YAW = Math.toRadians(0.0);
 
         // Intake Camera
         private static Transform3d _cameraOnePosition = new Transform3d(
         new Translation3d(CAMERA_ONE_X_FROM_ROBOT_CENTER, CAMERA_ONE_Y_FROM_ROBOT_CENTER, CAMERA_ONE_Z_FROM_ROBOT_CENTER),
         new Rotation3d(CAMERA_ONE_ROLL, CAMERA_ONE_PITCH, CAMERA_ONE_YAW));
 
-        private static final double CAMERA_TWO_X_FROM_ROBOT_CENTER = 0.0279 + 0.121; // Mechanical offset - 1/8" offset
-        private static final double CAMERA_TWO_Y_FROM_ROBOT_CENTER = 0.1586 + 0.1524;
+        private static final double CAMERA_TWO_X_FROM_ROBOT_CENTER = -0.16 - 0.254; // Mechanical offset - 1/8" offset
+        private static final double CAMERA_TWO_Y_FROM_ROBOT_CENTER = -0.273;
         private static final double CAMERA_TWO_Z_FROM_ROBOT_CENTER = 0.2583;
 
         private static final double CAMERA_TWO_ROLL = 0;
         private static final double CAMERA_TWO_PITCH = Math.toRadians(20.0);
-        private static final double CAMERA_TWO_YAW = Math.toRadians(0.0);
+        private static final double CAMERA_TWO_YAW = Math.toRadians(180.0);
+
+        // METERS
+        private static final double CAMERA_THREE_X_FROM_ROBOT = -0.2508;
+        private static final double CAMERA_THREE_Y_FROM_ROBOT = 0.2508;
+        private static final double CAMERA_THREE_Z_FROM_ROBOT = 0.27;
+
+        private static final double CAMERA_THREE_ROLL = 0.0;
+        private static final double CAMERA_THREE_PITCH = Math.toRadians(28.125);
+        private static final double CAMERA_THREE_YAW = Math.toRadians(135.0);
+
+        private static final Transform3d _cameraThreePosition = new Transform3d(
+            new Translation3d(CAMERA_THREE_X_FROM_ROBOT, CAMERA_THREE_Y_FROM_ROBOT, CAMERA_THREE_Z_FROM_ROBOT),
+            new Rotation3d(CAMERA_THREE_ROLL, CAMERA_THREE_PITCH, CAMERA_THREE_YAW)
+        );
 
         // Shooter Camera
         private static Transform3d _cameraTwoPosition = new Transform3d(
         new Translation3d(CAMERA_TWO_X_FROM_ROBOT_CENTER, CAMERA_TWO_Y_FROM_ROBOT_CENTER, CAMERA_TWO_Z_FROM_ROBOT_CENTER),
         new Rotation3d(CAMERA_TWO_ROLL, CAMERA_TWO_PITCH, CAMERA_TWO_YAW));
 
         // TODO: Swapped camera position. Rename after competition.
-        public static Transform3d[] _cameraPosition = new Transform3d[] {_cameraTwoPosition, _cameraOnePosition};
+        public static Transform3d[] _cameraPosition = new Transform3d[] {_cameraOnePosition, _cameraTwoPosition, _cameraThreePosition};
 
     // |====================== END VISION SUBSYSTEM TRANSFORMATIONS ======================|
 

src/main/java/frc/robot/Robot.java

@@ -162,6 +162,11 @@ public void robotPeriodic() {
   public void disabledInit() {
     _robotContainer.getRunAnglePIDCommand().cancel();
     SmartDashboard.putData("Autonomous Selection Preview", this._autonomousTrajectory);
+
+        if(!CommandScheduler.getInstance().isScheduled(_robotContainer.getRunLEDs())){ 
+      _robotContainer.getRunLEDs().schedule();
+    }
+
   }
 
   /** This function is called periodically when disabled. */
@@ -196,6 +201,11 @@ public void autonomousInit() {
       _robotContainer.getFeederInitialStateCommand().schedule();
     }
 
+    if(!CommandScheduler.getInstance().isScheduled(
+      _robotContainer.getRunLEDs())){ 
+      _robotContainer.getRunLEDs().schedule();
+    }
+
     // _robotContainer.getRunAnglePIDCommand().schedule();
     _robotContainer.getSetInitalShooterPosition().schedule();
     // schedule the autonomous command (example)
@@ -223,6 +233,11 @@ public void teleopInit() {
       _robotContainer.getAdjustShooterAutomaticallyCommand().schedule();
       _robotContainer.getFeederInitialStateCommand().schedule();
     }
+
+    if(!CommandScheduler.getInstance().isScheduled(
+      _robotContainer.getRunLEDs())){ 
+      _robotContainer.getRunLEDs().schedule();
+    }
 
     _robotContainer.getSetInitalShooterPosition().schedule();
     //_robotContainer.getRunAnglePIDCommand().schedule();
@@ -251,4 +266,4 @@ public void simulationInit() {}
   /** This function is called periodically whilst in simulation. */
   @Override
   public void simulationPeriodic() {}
-}
+}

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

@@ -24,6 +24,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 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;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -37,6 +38,8 @@
 import frc.robot.subsystems.intake.IntakeConstants;
 import frc.robot.subsystems.intake.IntakeIO;
 import frc.robot.subsystems.intake.RealIntakeIO;
+import frc.robot.subsystems.leds.LEDs;
+import frc.robot.subsystems.leds.commands.CmdLEDsRunLEDs;
 import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIONavX2;
 import frc.robot.subsystems.drive.ModuleIO;
@@ -72,12 +75,15 @@ public class RobotContainer {
         private final Drive _drive;
         private final Intake _intake;
         private final Shooter _shooter;
+        private final LEDs _leds;
         // private final Climber _climber;
         private final VisionDrive _visionDrive;
 
         private double _driveMultiplier = 1.0;
 
         private Command _adjustShooterAutomaticallyCommand;
+        private Command _runLEDsCommand;
+
 
         // logged dashboard inputs
         private final LoggedDashboardChooser<Command> _autoChooser;
@@ -142,6 +148,7 @@ public RobotContainer() {
                                                 new ModuleIOSparkFlex(3));
                                 _intake = new Intake(new RealIntakeIO());
                                 _shooter = new Shooter(new RealShooterIO());
+                                _leds = new LEDs();
                                 // _climber = new Climber(new RealClimberIO());
                                 _visionDrive = new VisionDrive(new RealVisionDriveIO());
                                 break;
@@ -165,6 +172,7 @@ public RobotContainer() {
                                 // });
                                 _visionDrive = new VisionDrive(new RealVisionDriveIO() {
                                 });
+                                _leds = new LEDs();
                                 break;
                         default:
                                 // Replayed robot, disable IO implementations
@@ -189,11 +197,13 @@ public RobotContainer() {
                                 // });
                                 _visionDrive = new VisionDrive(new RealVisionDriveIO() {
                                 });
+                                _leds = new LEDs();
                                 break;
                 }
 
                 // setup hand-scheduled commands
                 _adjustShooterAutomaticallyCommand = new CmdAdjustShooterAutomatically(_drive, _shooter, _intake);
+                _runLEDsCommand = new CmdLEDsRunLEDs(_leds, _shooter, _intake).ignoringDisable(true);
 
                 NamedCommands.registerCommand("Pickup_Note_Without_Limelight", _intake.buildCommand().pickUpFromGround(4000));
                 NamedCommands.registerCommand("Pickup_Note_With_Limelight", new PrintCommand("[ERROR] Not implemented"));
@@ -386,4 +396,8 @@ public Command getSetInitalShooterPosition() {
         public Command getFeederInitialStateCommand() {
                 return Commands.runOnce(() -> _intake.setFeederMotorPickupSpeed());
         }
-}
+
+        public Command getRunLEDs() {
+                return this._runLEDsCommand;
+        }
+}

src/main/java/frc/robot/subsystems/drive/ModuleIOSparkFlex.java

@@ -102,8 +102,8 @@ public ModuleIOSparkFlex(int index) {
     _turnSparkFlex.setSmartCurrentLimit(30);
     _driveSparkFlex.enableVoltageCompensation(12.0);
     _turnSparkFlex.enableVoltageCompensation(12.0);
-    _driveSparkFlex.setClosedLoopRampRate(0.25);
-    _turnSparkFlex.setClosedLoopRampRate(0.25);
+    _driveSparkFlex.setClosedLoopRampRate(0.1);
+    _turnSparkFlex.setClosedLoopRampRate(0.1);
 
     _driveEncoder.setPosition(0.0);
     _driveEncoder.setMeasurementPeriod(10);

src/main/java/frc/robot/subsystems/drive/commands/CmdDriveAutoAim.java

@@ -2,7 +2,6 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.drive.Drive;
@@ -88,6 +87,7 @@ public void execute() {
             rotationVal, true));
 
         Logger.recordOutput("Drive/autoaim/isReady", _angleController.atSetpoint());
+        Logger.recordOutput("Drive/autoaim/isEnabled", true);
     }
 
 
@@ -100,6 +100,7 @@ public void end(boolean interrupted) {
             _translationYSupplier.getAsDouble(),
             0, false));
         Logger.recordOutput("Drive/autoaim/isReady", false);
+        Logger.recordOutput("Drive/autoaim/isEnabled", false);
     }
 
     // Returns true when the command should end.

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

@@ -1,25 +1,46 @@
 package frc.robot.subsystems.leds;
 
+import org.easymock.bytebuddy.dynamic.scaffold.TypeInitializer.None;
+import org.littletonrobotics.junction.Logger;
+
 import com.ctre.phoenix.led.Animation;
 import com.ctre.phoenix.led.CANdle;
+import com.ctre.phoenix.led.FireAnimation;
 import com.ctre.phoenix.led.LarsonAnimation;
+import com.ctre.phoenix.led.RainbowAnimation;
 import com.ctre.phoenix.led.StrobeAnimation;
 import com.ctre.phoenix.led.LarsonAnimation.BounceMode;
 
+import edu.wpi.first.networktables.BooleanSubscriber;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import frc.robot.HardwareConstants;
 
 public class LEDs {
     CANdle _candle;
-    static int _numLEDs = 8;
+    static int _numLEDs = 27;
     LEDAnimation _currentAnimation = LEDAnimation.None;
     boolean _prevHasNote;
+    boolean _ampReady = false;
+    NetworkTable _adkitNetworkTable;
+    BooleanSubscriber _isAutoAimEnabled;
+    BooleanSubscriber _isAutoAimReady;
+
 
     public enum LEDAnimation {
         None(null, null, 0),
+        // RobotIdle(null, new FireAnimation(1.0, 0.02, _numLEDs, 0.01, 0.0), 0),
+        RobotIdle(new LEDColor(255, 128, 0), null, 0),
         ReadyToShoot(new LEDColor(0, 255, 0), null, 3),
+        SolidRed(new LEDColor(255, 0, 0), null, 0),
+        SolidGreen(new LEDColor(0, 255, 0), null, 0),
+        BlinkingRed(null, new StrobeAnimation(255, 0, 0, 0, 0.5, _numLEDs), 3),
+        SolidYellow(new LEDColor(255, 128, 0), null, 0),
         PickedUpNote(null, new StrobeAnimation(255, 128, 0, 0, 0.5, _numLEDs), 3),
-        ClimberAscending(new LEDColor(255, 215, 0), new LarsonAnimation(0, 234, 255, 0, 0.5, _numLEDs, BounceMode.Back, 5, 0), 3);
-
+        ClimberAscending(new LEDColor(255, 215, 0), new LarsonAnimation(0, 234, 255, 0, 0.5, _numLEDs, BounceMode.Back, 5, 0), 3),
+        AmpReady(null, new StrobeAnimation(204, 0, 255, 0, 0.5, _numLEDs), 3),
+        //Change time on RobotDisabled
+        RobotDisabled(null, new RainbowAnimation(100, 0.5, _numLEDs), 0);
         LEDColor _color;
         Animation _animation;
         int _secondsToRun;
@@ -45,48 +66,98 @@ public int getSecondsToRun() {
 
     public LEDs() {
         _candle = new CANdle(HardwareConstants.CanIds.CANDLE_ID);
+        _adkitNetworkTable = NetworkTableInstance.getDefault().getTable("AdvantageKit");
+        _isAutoAimEnabled = _adkitNetworkTable.getBooleanTopic("RealOutputs/Drive/autoaim/isEnabled")
+                            .subscribe(false);
+        _isAutoAimReady = _adkitNetworkTable.getBooleanTopic("RealOutputs/Drive/autoaim/isReady")
+                            .subscribe(false);
     }
 
     public LEDAnimation getCurrentAnimation() {
         return _currentAnimation;
     }
 
+    public boolean _isAutoAimEnabled(){
+        boolean enabled = _isAutoAimEnabled.get();
+        Logger.recordOutput("LEDS/isAutoAimEnabled", enabled);
+        return enabled;
+    }
+
+    public boolean _isAutoAimReady(){
+        boolean enabled = _isAutoAimReady.get();
+        Logger.recordOutput("LEDS/isAutoAimReady", enabled);
+        return enabled;    }
+
     public void runAnimation(LEDAnimation animation) {
+        Logger.recordOutput("LEDS/currentAnimation", _currentAnimation);
+        Logger.recordOutput("LEDS/requestedAnimation", animation);
         if (animation != _currentAnimation) {
             _currentAnimation = animation;
 
             if (animation.getColor() == null) {
                 _candle.clearAnimation(0);
+                _candle.setLEDs(0,0,0);
                 _candle.animate(animation.getAnimation());
             } else if (animation.getAnimation() == null){
                 LEDColor color = animation.getColor();
-                _candle.setLEDs(color.getR(), color.getB(), color.getG());
+                _candle.clearAnimation(0);
+                _candle.setLEDs(0,0,0);     
+                _candle.setLEDs(color.getR(), color.getG(), color.getB());
             } else {
                 _candle.clearAnimation(0);
                 _candle.animate(animation.getAnimation());
                 LEDColor color = animation.getColor();
-                _candle.setLEDs(color.getR(), color.getB(), color.getG());
+                _candle.setLEDs(color.getR(), color.getG(), color.getB());
             }
 
         }
     }
 
-    public void stopAnimationIfTimedOut(int currentRunTimeIn20Ms) {
+    /**
+     * returns true if the animation was stopped so we can reset the timer
+     * @param currentRunTimeIn20Ms
+     * @return
+     */
+    public boolean stopAnimationIfTimedOut(int currentRunTimeIn20Ms) {
         // Stop the current animation if it has run for its time
         int animationTime = _currentAnimation.getSecondsToRun();
+        boolean wasAnimationStopped = false;
         if (animationTime > 0) {
             // We actually have a timeout
             if (currentRunTimeIn20Ms >= animationTime * 50) {
-                // We have finished running, so stop the animation
+                Logger.recordOutput("LEDS/animationTimeout",true);
+                wasAnimationStopped = true;
+                if (_currentAnimation.getColor() == null) {
                 _candle.clearAnimation(0);
-                _currentAnimation = LEDAnimation.None;
+                } else if (_currentAnimation.getAnimation() == null){
+                    _candle.setLEDs(0, 0, 0);
+                } else {
+                    _candle.clearAnimation(0);
+                    _candle.setLEDs(0, 0, 0);
+                }
+            _currentAnimation = LEDAnimation.None;
+            }
+            else{
+                Logger.recordOutput("LEDS/animationTimeout",false);
             }
         }
+        else{
+            Logger.recordOutput("LEDS/animationTimeout",false);
+        }
+        return wasAnimationStopped;
     }
 
     public boolean shouldRunHasNoteAnimation(boolean hasNote) {
         boolean ret = hasNote && !_prevHasNote;
         _prevHasNote = hasNote;
         return ret;
     }
+
+    public boolean getAmpReady() {
+        return _ampReady;
+    }
+
+    public void setAmpReady(boolean isReady) {
+        _ampReady = isReady;
+    }
 }