David vision integration

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

@@ -18,6 +18,7 @@
 import frc.robot.sensors.AnalogSelector;
 import frc.robot.subsystems.*;
 import frc.robot.commands.ArcadeDriveWithJoystick;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 
 import java.util.logging.Logger;
 
@@ -117,6 +118,8 @@ private void configureButtonBindings() {
         sensitiveDriving = new SensitiveDriverControl(driverStick);
         new JoystickButton(driverStick, XboxController.Button.kBumperLeft.value).whenHeld(sensitiveDriving);
 
+        targetRPM = new VisionAssistedTargetRPM();
+        new RunCommand(targetRPM);
     }
 
     /**

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

@@ -1,11 +1,10 @@
 package frc.robot.commands;
 
 import frc.robot.config.Config;
-
-import frc.robot.subsystems.DriveBase;
 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,20 +14,15 @@ 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 HALF_OF_GRAVITY = 4.91;
-  private final double CONVERSION_NUMBER = 3000;
 
   // values from Vision Network Table
-  private double distanceToOuterPort;
+  private double distanceToOuterPortInMeters;
+
+  //adjusted value
+  private double adjustedDistanceToOutPortInMeters;
 
   //network table for vision control
-  private VisionCtrlNetTable visionControlNetTable;
+  private VisionCtrlNetTable visionControlNetTable = new VisionCtrlNetTable ();
 
   //calculated for the shooter
   double targetDistance = 0;
@@ -38,35 +32,41 @@ 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
   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 target.
+    distanceToOuterPortInMeters = visionControlNetTable.distanceToOuterPort.get();
 
-    //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();
+    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 * Config.METER_PER_FOOT ;
 
-    //todo: to adjuste the distance for the shooter 
-    //targetDistance
+      // adjuste the distance for the shooter 
+      adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters + Config.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 +76,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
@@ -90,24 +92,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(Config.SHOOTER_ANGLE_IN_DEGREES)*distanceToTargetInMeters - Config.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(Config.HALF_OF_GRAVITY/dCheck);
+        dInitVelocity = distanceToTargetInMeters/Math.cos(Config.SHOOTER_ANGLE_IN_DEGREES) * dTemp;
     }
     else
     {
-         dInitVelocity = 0.0;
-         System.out.println("WARNING! Not suitable for shooting!");      
-     }
-
+        dInitVelocity = 0.0;
+    }
+
     return dInitVelocity;
 
  }
@@ -117,7 +117,7 @@ public void end(boolean interrupted) {
  // return: unit revolutions per minute
 double velocityToRPM( double velocity)
  {     
-     double rpm = velocity*CONVERSION_NUMBER/(Math.PI*SHOOTER_WHEEL_RADIUS_IN_CM);
+     double rpm = velocity*Config.CONVERSION_NUMBER/(Math.PI*Config.SHOOTER_WHEEL_RADIUS_IN_CM);
      return rpm;
  }
 

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

@@ -0,0 +1,100 @@
+package frc.robot.commands;
+
+import frc.robot.config.Config;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+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 {
+
+
+  // values from Vision Network Table
+  private double distanceToOuterPortInMeters;
+
+  //adjusted value
+  private double adjustedDistanceToOutPortInMeters;
+
+  //network table for vision control
+  private VisionCtrlNetTable visionControlNetTable = new VisionCtrlNetTable ();
+
+  //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
+    //don't need this any more
+    //VisionCtrlNetTable.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 * Config.METER_PER_FOOT ;
+
+      // adjuste the distance for the shooter 
+      adjustedDistanceToOutPortInMeters = distanceToOuterPortInMeters + Config.D_CAMERA_SHOOTER_IN_METERS;
+
+      //Calculate the RPM of the shooter wheel.
+      double targetV  = initVelocity( adjustedDistanceToOutPortInMeters);
+      targetRPM       = velocityToRPM (targetV);
+    }
+
+    System.out.println("Vision RPM");
+    System.out.println( targetRPM);
+    // provide feedback to the shuffleboard for Driver Team
+    // vision assisted calculated RPM
+    SmartDashboard.putNumber("Vision RPM", targetRPM);
+  }
+
+ private double initVelocity(double distanceToTargetInMeters) {
+    double dTemp;
+    //unit: m/s
+    double dInitVelocity;
+
+    double dCheck = Math.tan(Config.SHOOTER_ANGLE_IN_DEGREES)*distanceToTargetInMeters - Config.TARGET_HEIGHT_IN_METERS;
+    if (dCheck > 0)
+    {
+        dTemp = Math.sqrt(Config.HALF_OF_GRAVITY/dCheck);
+        dInitVelocity = distanceToTargetInMeters/Math.cos(Config.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*Config.CONVERSION_NUMBER/(Math.PI*Config.SHOOTER_WHEEL_RADIUS_IN_CM);
+     return rpm;
+ }
+
+}

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

@@ -144,10 +144,19 @@ private Config() {
     public static String VISION_TABLE_NAME_POWERCELL = "MergeVisionPi20";
     public static String VISION_TABLE_NAME_OUTERPORT = "MergeVisionPipelinePi21";
     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";
 
+    // Vision Assisted RPM
+    public static double SHOOTER_ANGLE_IN_DEGREES   = 46.88;
+    public static double TARGET_HEIGHT_IN_METERS    = 2.02;
+    public static double SHOOTER_WHEEL_RADIUS_IN_CM = 5.5;
+    public static double D_CAMERA_SHOOTER_IN_METERS = 0.26;
+    public static double HALF_OF_GRAVITY            = 4.91;
+    public static double CONVERSION_NUMBER          = 3000;
+    public static double METER_PER_FOOT             = 0.3048;
+
     // Drivetrain PID values
     public static double DRIVETRAIN_P_SPECIFIC = robotSpecific(0.037, 0.0, 0.0, 0.018d, 0.0, 0.25);
     public static double DRIVETRAIN_D_SPECIFIC = robotSpecific(0.0023, 0.0, 0.0, 0.0016d, 0.0, 0.03);