Eastbots Wiki

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
===== Computer Vision and Shooter Targeting =====

__Color Based Shooter Speed Modulation Computer Vision Pipeline__

**Github** \\ 
[[https://github.com/Team-4795/2022-Rapid-React/blob/8e39e70995130413f89f3a0e958ad59cd0c398be/src/main/java/frc/robot/commands/PrepareShot.java|PrepareShot.java]] [[https://github.com/Team-4795/2022-Rapid-React/blob/8e39e70995130413f89f3a0e958ad59cd0c398be/src/main/java/frc/robot/commands/Shoot.java|Shoot.java]]

----

==== Detection Pipeline ====

For this game in particular, it is advantageous to be able to reliably shoot from variable distances. To accomplish this, we needed to figure out a reliable method of targeting the goal and choosing the right shooter speeds. When we saw that there were retro reflective tape strips that lined the goals, we immediately looked into using CV. \\ 

In 2020, we attempted to develop in-house CV solutions for ball detection. However, this exposed many issues in our design, development, and testing of this complex system. Learning from that experience, we decided to focus more on the application of CV rather than the development of it. We did some research on the different options that would cover our needs in this space and we eventually settled for PhotonVision with a SnakeEyes HAT. PhotonVision allowed for a rigorous set of options for us to tune our CV while providing easy code integration and a clean web interface. This allowed us to streamline the process of obtaining and installing the CV and shift the onus onto integrating the data that it provided and turning it into useful shooter information.\\ 

{{ ::screen_shot_2022-08-14_at_11.46.54_pm.png?600 |}}

----

==== Shooter RPM Interpolation ====

{{ ::screen_shot_2022-08-14_at_11.52.48_pm.png?600|}}
Our main use of CV was to detect the distance from the robot from the base of the goal and to calculate the angle offset between the robot and the center of the goal. We use the angle offset to align ourselves with the goal, through the use of PID, and we interpolate the distance and RPM presets in order to shoot the ball. We achieved this through the use of a shooter preset, which is a collection of flywheel RPMs that are known to work from specific distances. From this, we select the two closest presets based on our current distance, feed those values into interpolation algorithms, set our shooter RPMs to that of the output from the algorithms, and then fire. Shooting in this manner allows us to shoot the game-peice without fear of bounce outs, thereby increasing reliability and consistency. From our scouting data on our team's robot our **score when shot rate is 95%**.

----

==== Other Notable Structures ====


=== Turn to Goal ===
<code java>
if (useCV && vision.hasTarget() && System.currentTimeMillis() - start < 3000) {
      double distance = vision.getTargetDistance();
      double angle = vision.getTargetAngle();
      double turnSpeed = angle / 50.0;

      preset = interpolate(distance);

      turnSpeed = MathUtil.clamp(Math.copySign(Math.max(Math.abs(turnSpeed), 0.125), turnSpeed), -0.35, 0.35);

      if (useAlignment && Math.abs(angle) > 2) {
        isAligned = false;
      } else {
        var goalPose = drivebase.getGoalPose();
        double centerDistance = Units.feetToMeters(vision.getTargetDistance() + 1.5 + 2);
        double poseAngle = drivebase.getPose().getRotation().getDegrees();
        double transformRotation = 0;
        if (angle < 0 && poseAngle > 0) {
          transformRotation = angle + poseAngle;
        } else if (angle > 0 && poseAngle > 0) {
          transformRotation = angle + poseAngle + 90;
        } else if (angle < 0 && poseAngle < 0) {
          transformRotation = angle + poseAngle + 90;
        } else if (angle >  0 && poseAngle < 0) {
          transformRotation = angle + poseAngle + 90;
        }
        double newX = goalPose.getX() - centerDistance * Math.sin(Math.toRadians(transformRotation));
        double newY = goalPose.getY() + centerDistance * Math.cos(Math.toRadians(transformRotation));
        
        drivebase.resetOdometry(new Pose2d(newX, newY, drivebase.getPose().getRotation()));
      }

      drivebase.curvatureDrive(0, !isAligned && useAlignment ? turnSpeed : 0, true);
    } else {
      drivebase.curvatureDrive(0, 0, false);
    }
</code>

----

=== Interpolate Algorithm ===

<code java>
public static ShooterPreset interpolate(double distance) {
    ArrayList<ShooterPreset> presets = new ArrayList<>();

    distance -= 0.5;

    presets.add(new ShooterPreset(900, 2200, 3));
    presets.add(new ShooterPreset(1950, 1500, 5));
    presets.add(new ShooterPreset(2150, 1500, 6.5));
    presets.add(new ShooterPreset(2500, 1450, 8));
    presets.add(new ShooterPreset(2950, 1350, 10));
    presets.add(new ShooterPreset(3350, 1200, 11));
    presets.add(new ShooterPreset(4100, 800, 12));
    presets.add(new ShooterPreset(4600, 700, 13.5));

    ShooterPreset bottomPreset = presets.get(presets.size() - 1);
    ShooterPreset upperPreset;
    
    for (ShooterPreset p : presets) {
      if (distance - p.distance < 0) {
        bottomPreset = p;
        break;
      }
    }

    try {
      bottomPreset = presets.get(presets.indexOf(bottomPreset) - 1);
      upperPreset = presets.get(presets.indexOf(bottomPreset) + 1);
    } catch (IndexOutOfBoundsException e) {
      if (distance > presets.get(presets.size() - 1).distance) {
        bottomPreset = presets.get(presets.size() - 1);
      }
      upperPreset = bottomPreset;
    }

    double topRPMDifference = upperPreset.topRPM - bottomPreset.topRPM;
    double mainRPMDifference = upperPreset.mainRPM - bottomPreset.mainRPM;
    double dist = upperPreset.distance - bottomPreset.distance;

    if (dist == 0) dist = bottomPreset.distance;

    double percentage = (distance - bottomPreset.distance) / dist;

    double topRPM = percentage * topRPMDifference + bottomPreset.topRPM;
    double mainRPM = percentage * mainRPMDifference + bottomPreset.mainRPM;
    
    return new ShooterPreset(topRPM, mainRPM, distance);
  }
</code>