Unknown shot added

Author: JoshuaAN

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

@@ -94,6 +94,7 @@ public void robotPeriodic() {
     // robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
+    mRobotContainer.enableLights();
   }
 
   /** This function is called once each time the robot enters Disabled mode. */
@@ -117,7 +118,7 @@ public void disabledPeriodic() {
       }
     }
 
-    mRobotContainer.disableLights();
+    mRobotContainer.enableLights();
     mRobotContainer.displaySwitch();
   }
 

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

@@ -73,6 +73,7 @@
 import frc.robot.shooter.commands.SetShooterState;
 import frc.robot.shooter.commands.StopShooter;
 import frc.robot.shooter.commands.StopTrigger;
+import frc.robot.shooter.commands.VisionShooter;
 import frc.robot.status.Status;
 import frc.robot.status.commands.IdleCommand;
 import frc.robot.status.commands.SetColor;
@@ -333,13 +334,17 @@ public void configureButtonBindings() {
                             .alongWith(new IdleCommand()));
 
       JoystickButton xBoxX = new JoystickButton(operator, X_BOX_X);
-      xBoxX.whenHeld(new PresetFlywheelController(mShooter, "TLR")
-                          // .alongWith(new TurnOnLEDs(mLimelight))
-                          .alongWith(new XBoxButtonCommand(X_BOX_X))); // tarmac, lower goal, rear shot    
+      // xBoxX.whenHeld(new PresetFlywheelController(mShooter, "TLR")
+      //                     // .alongWith(new TurnOnLEDs(mLimelight))
+      //                     .alongWith(new XBoxButtonCommand(X_BOX_X))); // tarmac, lower goal, rear shot    
 
-      xBoxX.whenReleased(new StopShooter(mShooter)
-                            .alongWith(new TurnOffLEDs(mLimelight))
-                            .alongWith(new IdleCommand()));
+      // xBoxX.whenReleased(new StopShooter(mShooter)
+      //                       .alongWith(new TurnOffLEDs(mLimelight))
+      //                       .alongWith(new IdleCommand()));
+
+      xBoxX.whenHeld(new VisionShooter(mShooter, mLimelight));
+
+      xBoxX.whenReleased(new StopShooter(mShooter));
 
       JoystickButton xBoxY = new JoystickButton(operator, X_BOX_Y);
       xBoxY.whenHeld(new PresetFlywheelController(mShooter, "TUR")

src/main/java/frc/robot/shooter/commands/VisionShooter.java

@@ -0,0 +1,84 @@
+package frc.robot.shooter.commands;
+
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.lib.control.DCMotor;
+import frc.lib.control.PIDController;
+import frc.robot.shooter.Shooter;
+import frc.robot.vision.Limelight;
+
+public class VisionShooter extends CommandBase {
+
+    Shooter shooter;
+    Limelight limelight;
+    double lastPosition, lastTime, hoodAngle;
+    double rpm;
+    private Timer timer = new Timer();
+    Notifier controller = new Notifier(this::controller);
+    private DCMotor flywheel = new DCMotor(0.002081897, 0, 0.317466);
+    private PIDController flywheelController = new PIDController(0.02, 0, 0);
+    boolean closeToTarget = false;
+    double velocity = 0;
+
+    public VisionShooter(Shooter shooter, Limelight limelight) {
+        this.shooter = shooter;
+        this.limelight = limelight;
+        lastPosition = shooter.getEncoderPosition();
+        lastTime = 0;
+        timer.reset();
+        timer.start();
+        addRequirements(shooter);
+    }
+
+    @Override
+    public void initialize() {
+        
+        closeToTarget = false;
+        velocity = 0;       
+        lastPosition = shooter.getEncoderPosition();
+        timer.reset();
+        lastTime = 0;
+        // timer.start();
+        controller.startPeriodic(0.02); 
+    }
+
+    void controller() {
+        double time = timer.get();
+        double position = shooter.getEncoderPosition();
+        double dt = time - lastTime;
+        if (dt < 0.0001) {
+            dt = 0.001;
+        }
+        velocity = (position - lastPosition) / (dt) * 60;
+
+        double distancePercent = (limelight.getState().distance - 1.7) / (2.9 - 1.7);
+        double rpm = distancePercent * (180) + 1800;
+        double hoodAngle = distancePercent * -5 + 78.25;
+
+        shooter.setHoodPosition(hoodAngle);
+        flywheelController.setReference(rpm);
+        SmartDashboard.putNumber("Estimated velocity", velocity);
+
+        double voltage = flywheel.solveFeedforward(rpm, 0) + flywheelController.calculate(velocity, dt);
+        // double voltage = flywheel.solveFeedforward(rpm, 0);
+
+        shooter.setShooterVoltage(voltage);
+        
+        lastPosition = position;
+        lastTime = time;        
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        controller.stop();
+        closeToTarget= false;
+        velocity = 0;
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

src/main/java/frc/robot/vision/Limelight.java

@@ -6,6 +6,7 @@
 
 import edu.wpi.first.networktables.EntryListenerFlags;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 import static edu.wpi.first.networktables.NetworkTableInstance.getDefault;
 
@@ -14,13 +15,18 @@
 public class Limelight extends SubsystemBase {
   private volatile VisionState state = new VisionState(0,0,0);
 
+  double h = 2.1;
+  double theta = 45;
+
   public Limelight() {
     getDefault().getTable("limelight").getEntry("tx").addListener(event -> {
       if (getDefault().getTable("limelight").getEntry("tv").getDouble(0) == 1) {
         double xOffset = getDefault().getTable("limelight").getEntry("tx").getDouble(0);
         double yOffset = getDefault().getTable("limelight").getEntry("ty").getDouble(0);
+        double distance = h/(Math.tan(Math.toRadians(yOffset+theta))*Math.cos(Math.toRadians(xOffset)));
+        SmartDashboard.putNumber("Distance", distance);
         double latency = getDefault().getTable("limelight").getEntry("tl").getDouble(0) / 1000.0 + 0.011;
-        state = new VisionState(xOffset, yOffset, Timer.getFPGATimestamp() - latency);
+        state = new VisionState(xOffset, distance, Timer.getFPGATimestamp() - latency);
       }
     }, EntryListenerFlags.kUpdate);
   }
@@ -43,12 +49,12 @@ public void turnOnLEDs() {
 
   public static class VisionState {
     public final double xOffset;
-    public final double yOffset;
+    public final double distance;
     public final double timestamp;
 
-    public VisionState(double xOffset, double yOffset, double timestamp) {
+    public VisionState(double xOffset, double distance, double timestamp) {
       this.xOffset = xOffset;
-      this.yOffset = yOffset;
+      this.distance = distance;
       this.timestamp = timestamp;
     }
   }