Eastbots Wiki

User Tools

Site Tools

Trace: • control_system
computer_vision_and_shooter_targeting

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
computer_vision_and_shooter_targeting [2022/08/14 23:56] – rfhuangcomputer_vision_and_shooter_targeting [2022/08/15 00:00] (current) – rfhuang
Line 23: Line 23:
 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%**. 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>
computer_vision_and_shooter_targeting.1660535775.txt.gz · Last modified: 2022/08/14 23:56 by rfhuang