David vision integration

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

@@ -20,6 +20,7 @@
 import frc.robot.subsystems.*;
 import frc.robot.commands.ArcadeDriveWithJoystick;
 import frc.robot.commands.DriveWithTime;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 
 import java.util.logging.Logger;
 
@@ -53,6 +54,7 @@ public class RobotContainer {
   private Command rampShooterCommand;
   private Command incrementFeeder;
   private Command perfectPosition;
+  private VisionAssistedTargetRPM targetRPM;
   private Logger logger = Logger.getLogger("RobotContainer");
   private final double AUTO_DRIVE_TIME = 1.0;
   private final double AUTO_LEFT_MOTOR_SPEED = 0.2;
@@ -111,6 +113,9 @@ private void configureButtonBindings() {
 
         reverseArmManually = new DrivetrainPIDTurnDelta(45, 0, 5, 3.0);
         new JoystickButton(driverStick, XboxController.Button.kB.value).whenHeld(reverseArmManually);
+
+        targetRPM = new VisionAssistedTargetRPM();
+        new RunCommand(targetRPM);
     }
 
     /**

src/main/java/frc/robot/commands/VisionAssistedShooter.java

@@ -6,6 +6,7 @@
 import frc.robot.subsystems.ShooterSubsystem;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.nettables.VisionCtrlNetTable;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * VisionAssistedShooter command.
@@ -15,17 +16,22 @@ public class VisionAssistedShooter extends CommandBase {
 
   //subsystem
   private final ShooterSubsystem shooter;
-    
+
   //todo: can be configured in config file as well
-  //todo: measure the radius for the shooting wheel
-  private final double SHOOTER_ANGLE_IN_DEGREES  = 60.0;
-  private final double TARGET_HEIGHT_IN_METERS = 2.49;
-  private final double SHOOTER_WHEEL_RADIUS_IN_CM = 10;
+  private final double SHOOTER_ANGLE_IN_DEGREES  = 46.88;
+  private final double TARGET_HEIGHT_IN_METERS = 2.02;
+  private final double SHOOTER_WHEEL_RADIUS_IN_CM = 5.5;
+  private final double D_CAMERA_SHOOTER_IN_METERS = 0.26;
+
   private final double HALF_OF_GRAVITY = 4.91;
   private final double CONVERSION_NUMBER = 3000;
+  private final double METER_PER_FOOT = 0.3048;
 
   // values from Vision Network Table
-  private double distanceToOuterPort;
+  private double distanceToOuterPortInMeters;
+
+  //adjusted value
+  private double adjustedDistanceToOutPortInMeters;
 
   //network table for vision control
   private VisionCtrlNetTable visionControlNetTable;
@@ -38,15 +44,16 @@ public class VisionAssistedShooter extends CommandBase {
   /**
    * Creates a new VisionAssistedShooter Command.
    *
-   * @param subsystem The subsystem used by this command.
    */
-  public VisionAssistedShooter(ShooterSubsystem subsystem) {
+  public VisionAssistedShooter( ) {
 
     //subsystem
-    shooter = subsystem;
+    shooter = ShooterSubsystem.getInstance();
 
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
+    if (shooter.isActive()){
+      // Use addRequirements() here to declare subsystem dependencies.
+      addRequirements(shooter);
+    }
   }
 
   @Override
@@ -55,18 +62,26 @@ public void initialize() {
     // Ensure the vision is running in tape mode
     visionControlNetTable.setTapeMode();
 
-    //Read the network table from vision to get the distance from the power port.
-    //distance from vision
-    //NOTE: unit should be meter. If not, need conversion here.
-    distanceToOuterPort = visionControlNetTable.distanceToOuterPort.get();
+    //Read the network table from vision to get the distance from the target.
+    distanceToOuterPortInMeters = visionControlNetTable.distanceToOuterPort.get();
+
+    if ( distanceToOuterPortInMeters < 0.0 )
+    {
+      //Vision can not provide valid detection
+      targetRPM = 0;
+    }
+    else
+    {
+      //NOTE: unit in the vision network table is feet. Convert it to meters.
+      distanceToOuterPortInMeters = distanceToOuterPortInMeters * METER_PER_FOOT ;
 
-    //todo: to adjuste the distance for the shooter 
-    //targetDistance
+      // adjuste the distance for the shooter 
+      adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters + D_CAMERA_SHOOTER_IN_METERS;
 
-    //Calculate the RPM of the shooter wheel.
-    double targetV  = initVelocity( distanceToOuterPort);
-    targetRPM     = velocityToRPM (targetV);
-
+      //Calculate the RPM of the shooter wheel.
+      double targetV  = initVelocity( adjustedDistanceToOutPortInMeters);
+      targetRPM       = velocityToRPM (targetV);
+    }
   }
 
   @Override
@@ -76,7 +91,9 @@ public void execute() {
     shooter.setTargetRPM((int) targetRPM);
 
     //todo: provide feedback to the shuffleboard for Driver Team
-
+    //vision assisted RPM
+
+    SmartDashboard.putNumber("Vision Assisted Shooter RPM", targetRPM);
   }
 
   @Override
@@ -92,22 +109,22 @@ public void end(boolean interrupted) {
 
 
   double initVelocity(double distanceToTargetInMeters) {
-    double dCheck = Math.tan(SHOOTER_ANGLE_IN_DEGREES)*distanceToTargetInMeters - TARGET_HEIGHT_IN_METERS;
     double dTemp;
-
     //unit: m/s
     double dInitVelocity;
+
+    double dCheck = Math.tan(SHOOTER_ANGLE_IN_DEGREES)*distanceToTargetInMeters - TARGET_HEIGHT_IN_METERS;
     if (dCheck > 0)
     {
-         dTemp = Math.sqrt(HALF_OF_GRAVITY/dCheck);
-         dInitVelocity = distanceToTargetInMeters/Math.cos(SHOOTER_ANGLE_IN_DEGREES) * dTemp;
+        dTemp = Math.sqrt(HALF_OF_GRAVITY/dCheck);
+        dInitVelocity = distanceToTargetInMeters/Math.cos(SHOOTER_ANGLE_IN_DEGREES) * dTemp;
     }
     else
     {
-         dInitVelocity = 0.0;
-         System.out.println("WARNING! Not suitable for shooting!");      
-     }
-
+        dInitVelocity = 0.0;
+        System.out.println("WARNING! Not suitable for shooting!");      
+    }
+  
     return dInitVelocity;
 
  }

src/main/java/frc/robot/commands/VisionAssistedTargetRPM.java

@@ -0,0 +1,111 @@
+package frc.robot.commands;
+
+import frc.robot.config.Config;
+
+//import frc.robot.subsystems.DriveBase;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.nettables.VisionCtrlNetTable;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * VisionAssistedTargetRPM command.
+ * NOTE: this command should be run after command TurnToOuterPortCommand
+ */
+public class VisionAssistedTargetRPM implements Runnable {
+
+  //subsystem
+
+  //todo: can be configured in config file as well
+  private final double SHOOTER_ANGLE_IN_DEGREES  = 46.88;
+  private final double TARGET_HEIGHT_IN_METERS = 2.02;
+  private final double SHOOTER_WHEEL_RADIUS_IN_CM = 5.5;
+  private final double D_CAMERA_SHOOTER_IN_METERS = 0.26;
+
+  private final double HALF_OF_GRAVITY = 4.91;
+  private final double CONVERSION_NUMBER = 3000;
+  private final double METER_PER_FOOT = 0.3048;
+
+  // values from Vision Network Table
+  private double distanceToOuterPortInMeters;
+
+  //adjusted value
+  private double adjustedDistanceToOutPortInMeters;
+
+  //network table for vision control
+  private VisionCtrlNetTable visionControlNetTable;
+
+  //calculated RPM
+  double targetDistance = 0;
+  double targetRPM = 0;
+
+  /**
+   * Creates a new VisionAssistedTargetRPM Command.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public VisionAssistedTargetRPM() {
+
+    // Ensure the vision is running in tape mode
+    visionControlNetTable.setTapeMode();
+
+  }
+
+  public void run() {
+
+    //Read the network table from vision 
+    distanceToOuterPortInMeters = visionControlNetTable.distanceToOuterPort.get();
+
+    if ( distanceToOuterPortInMeters < 0.0 )
+    {
+      // Vision can not provide valid detection
+      targetRPM = 0;
+    }
+    else
+    {
+      // NOTE: unit in the vision network table is feet. Convert it to meters.
+      distanceToOuterPortInMeters = distanceToOuterPortInMeters * METER_PER_FOOT ;
+
+      // adjuste the distance for the shooter 
+      adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters + D_CAMERA_SHOOTER_IN_METERS;
+
+      //Calculate the RPM of the shooter wheel.
+      double targetV  = initVelocity( adjustedDistanceToOutPortInMeters);
+      targetRPM       = velocityToRPM (targetV);
+    }
+
+    // provide feedback to the shuffleboard for Driver Team
+    // vision assisted calculated RPM
+    SmartDashboard.putNumber("Info: Vision Assisted Calculated Target RPM", targetRPM);
+  }
+
+ private double initVelocity(double distanceToTargetInMeters) {
+    double dTemp;
+    //unit: m/s
+    double dInitVelocity;
+
+    double dCheck = Math.tan(SHOOTER_ANGLE_IN_DEGREES)*distanceToTargetInMeters - TARGET_HEIGHT_IN_METERS;
+    if (dCheck > 0)
+    {
+        dTemp = Math.sqrt(HALF_OF_GRAVITY/dCheck);
+        dInitVelocity = distanceToTargetInMeters/Math.cos(SHOOTER_ANGLE_IN_DEGREES) * dTemp;
+    }
+    else
+    {
+        dInitVelocity = 0.0;
+        System.out.println("WARNING! Not suitable for shooting!");      
+    }
+
+    return dInitVelocity;
+
+ }
+
+ // convert velocity to RPM
+ // velocity: unit m/s
+ // return: unit revolutions per minute
+private double velocityToRPM( double velocity)
+ {     
+     double rpm = velocity*CONVERSION_NUMBER/(Math.PI*SHOOTER_WHEEL_RADIUS_IN_CM);
+     return rpm;
+ }
+
+}

src/main/java/frc/robot/config/Config.java

@@ -142,7 +142,7 @@ private Config() {
     // Vision Table Constants
     public static String VISION_TABLE_NAME      = "MergeVision";
     public static String DISTANCE_POWERCELL     = "DistanceToPowerCell";
-    public static String DISTANCE_OUTER_PORT    = "DistanceToOuterPort";
+    public static String DISTANCE_OUTER_PORT    = "DistanceToTarget";
     public static String YAW_POWERCELL          = "YawToPowerCell";
     public static String YAW_OUTER_PORT         = "YawToTarget";