Work from 3/19 Meeting

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

@@ -5,6 +5,7 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 
 import java.util.Optional;
 import java.util.OptionalDouble;
@@ -27,6 +28,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -89,6 +91,7 @@ public class RobotContainer {
                 vp.visualPose()
             );
         });
+
         vision.addPoseEstimateConsumer((vp) -> {
             if(resetOdometryToVisualPose) {
                 drivetrain.resetOdometry(vp.visualPose());
@@ -96,7 +99,6 @@ public class RobotContainer {
             }
         });
         
-
         driver = new CommandXboxController(OIConstants.kDriverControllerPort);
         secondary = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
@@ -104,8 +106,9 @@ public class RobotContainer {
         shiftTimer.reset();
 
         configureBindings();
-        //testConfigureBindings();
         configureShiftDisplay();
+        //testConfigureBindings();
+        
 
         if(AutoConstants.kAutoConfigOk) {
             autoChooser = AutoBuilder.buildAutoChooser();
@@ -285,7 +288,17 @@ public class RobotContainer {
         driver.leftBumper().whileTrue(intakeRoller.runOut());
         driver.rightBumper().whileTrue(intakeRoller.runIn());
 
-        driver.rightTrigger().whileTrue(spindexer.spinToShooter());
+        driver.rightTrigger().whileTrue(
+            spindexer.spinToShooter().alongWith(
+                intakeRoller.runIn(),
+                intakePivot.jimmy(.5)
+            )
+        );
+
+        driver.rightTrigger().onFalse(
+            intakePivot.manualSpeed(() -> 0.75).withTimeout(0.5)
+        );
+
         driver.b().whileTrue(spindexer.spinToIntake());
        /*  driver.b().whileTrue(
             drivetrain.lockToYaw(
@@ -309,13 +322,6 @@ public class RobotContainer {
         //30 degrees good for shooter far near outpost
         secondary.rightBumper().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(10)));
         //10 degrees good for shooting ~33in away from hub
-
-        secondary.y().whileTrue(
-            intakePivot.jimmy(.2)
-            .andThen(
-                intakePivot.manualSpeed(() -> -0.5)
-            )
-        );
         
         hood.setDefaultCommand(hood.trackToAngle(() -> {
             Pose2d drivetrainPose = drivetrain.getPose();
@@ -340,6 +346,24 @@ public class RobotContainer {
                 return 0;
             }
         }));
+
+        new Trigger(() -> MathUtil.isNear(
+            shooter.getTargetSpeeds().isEmpty() ? 0 : shooter.getTargetSpeeds().get().getSpeedRPM(), 
+            shooter.getAverageActualSpeeds(), 
+            150)).onTrue(
+            new FunctionalCommand(
+                () -> {}, 
+                () -> {
+                    driver.setRumble(RumbleType.kBothRumble, .75);
+                    secondary.setRumble(RumbleType.kBothRumble, .75);
+                }, 
+                (b) -> {
+                    driver.setRumble(RumbleType.kBothRumble, 0);
+                    secondary.setRumble(RumbleType.kBothRumble, 0);
+                }, 
+                () -> false
+            ).withTimeout(5)
+        );
     }
 
     private void configureNamedCommands() {

src/main/java/frc/robot/constants/ShooterConstants.java

@@ -1,5 +1,6 @@
 package frc.robot.constants;
 
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;

src/main/java/frc/robot/subsystems/Hood.java

@@ -12,7 +12,9 @@ import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -68,6 +70,21 @@ public class Hood extends SubsystemBase {
 
     @Override
     public void periodic() {
+        SmartDashboard.putNumber(
+            "HoodTargetDegrees", 
+            Math.toDegrees(currentTargetDegrees)
+        );
+
+        SmartDashboard.putNumber(
+            "HoodCurrentAngle", 
+            Math.toDegrees(encoder.getPosition())
+        );
+
+        SmartDashboard.putBoolean(
+            "HoodAtSetpoint", 
+            controller.isAtSetpoint()
+        );
+        
         Logger.recordOutput("Hood/OutputCurrent", motor.getOutputCurrent());
         Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees));
         Logger.recordOutput("Hood/CurrentAngle", Math.toDegrees(encoder.getPosition()));

src/main/java/frc/robot/subsystems/IntakePivot.java

@@ -89,8 +89,8 @@ public class IntakePivot extends SubsystemBase {
      */
     public Command jimmy(double time){
         return Commands.repeatingSequence(
-            manualSpeed(() -> -0.75).withTimeout(time)
+            manualSpeed(() -> -0.75).withTimeout(time),
-            .andThen(manualSpeed(() -> 0.75).withTimeout(time))
+            manualSpeed(() -> 0.75).withTimeout(time)
         );
     }
 
@@ -102,8 +102,8 @@ public class IntakePivot extends SubsystemBase {
      */
     public Command jimmy(DoubleSupplier time) {
         return Commands.repeatingSequence(
-            manualSpeed(() -> -0.75).withTimeout(time.getAsDouble())
+            manualSpeed(() -> -0.75).withTimeout(time.getAsDouble()),
-            .andThen(manualSpeed(() -> 0.75).withTimeout(time.getAsDouble()))
+            manualSpeed(() -> 0.75).withTimeout(time.getAsDouble())
         );
     }
 

src/main/java/frc/robot/subsystems/Shooter.java

@@ -14,6 +14,7 @@ import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ShooterConstants;
@@ -63,6 +64,30 @@ public class Shooter extends SubsystemBase {
 
     @Override
     public void periodic() {
+        SmartDashboard.putNumber(
+            "ShooterTargetRPM", 
+            targetSpeeds == null ? 0 : targetSpeeds.getSpeedRPM());
+
+        SmartDashboard.putNumber(
+            "ShooterLeftSideRPM", 
+            leftEncoder.getVelocity()
+        );
+
+        SmartDashboard.putNumber(
+            "ShooterRightSideRPM", 
+            rightEncoder.getVelocity()
+        );
+
+        SmartDashboard.putBoolean(
+            "ShooterLeftSideAtSetpoint", 
+            leftClosedLoopController.isAtSetpoint()
+        );
+
+        SmartDashboard.putBoolean(
+            "ShooterRightSideAtSetpoint", 
+            rightClosedLoopController.isAtSetpoint()
+        );
+
         Logger.recordOutput(
             "Shooter/TargetRPM", 
             targetSpeeds == null ? 0 : targetSpeeds.getSpeedRPM()
@@ -73,6 +98,8 @@ public class Shooter extends SubsystemBase {
 
         Logger.recordOutput("Shooter/RightRollers/rightmotor", rightRelative.getVelocity());
 
+        Logger.recordOutput("Shooter/LeftRollers/OutputVoltage", leftMotor.getAppliedOutput() * leftMotor.getBusVoltage());
+        Logger.recordOutput("Shooter/RightRollers/OutputVoltage", rightMotor.getAppliedOutput() * rightMotor.getBusVoltage());
 
         // TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance?
         Logger.recordOutput("Shooter/LeftRollers/AtSetpoint", leftClosedLoopController.isAtSetpoint());