Shooting while driving 😵

[Jacob1010-h]

Discussed in #73

^{Originally posted by GalexY727 January 26, 2024}
Oh boy, where do we start...

where do we start, indeed... 💀

EDIT: Task list

• Compensate for velocity tangent to the speaker by turning the robot to a new desired angle
  • Marked as complete with the creation and implementation of this equation
• Compensate for velocity normal to the speaker by changing our desired RPM of the shooter wheels

[GalexY727]

oh interesting you can only have one assignee :(

[GalexY727]

can someone link the #73 swd branch

[Jacob1010-h]

oh interesting you can only have one assignee :(

tru (only on private repos) but we can @GalexY727 them if we want them to be subscribed to the notifs for that issue

[Jacob1010-h]

Why is this not already complete because of calculateSWDAngleToSpeaker?

[GalexY727]

I was thinking only because it hasn't been tested in real life to work, but what the heck with it we can just merge if you are excited and ready for fun

[Jacob1010-h]

If you truly want to test it, I think that we may be able to use #109 to test just how good this is; all I need to do is edit the params so you can implement angle and speed into the shooting. I'll do that now...(unless we should wait?)

[GalexY727]

It works phenomenally in sim, I have tested it. Feel free to integrate #109 into it

[GalexY727]

I'm fine for the PR for this to be drafted
Do you wanna make it?

[Jacob1010-h]

From here to main?? If you mean #110 I've already made it and asked for your review

[GalexY727]

idk why but i thought #110 was just a draft pr and wasn't ready for review. I'll do that now.

[Jacob1010-h]

what else need to get done for this besides testing, can this be closed @GalexY727? (make a task list)

[Jacob1010-h]

The second task was already complete...

public Rotation2d calculateSWDAngleToSpeaker(Pose2d robotPose, ChassisSpeeds robotVelocity) {
        // Calculate the robot's pose relative to the speaker
        Pose2d poseRelativeToSpeaker = robotPose.relativeTo(FieldConstants.GET_SPEAKER_POSITION());

        // Calculate the current angle to the speaker
        currentAngleToSpeaker = new Rotation2d(poseRelativeToSpeaker.getX(), poseRelativeToSpeaker.getY());

        // Convert the robot's velocity to a Rotation2d object
        Rotation2d velocityRotation2d = new Rotation2d(robotVelocity.vxMetersPerSecond, robotVelocity.vyMetersPerSecond);

        // Calculate the total speed of the robot
        double totalSpeed = Math.hypot(robotVelocity.vxMetersPerSecond, robotVelocity.vyMetersPerSecond);

        // Calculate the component of the velocity that is tangent to the speaker
        double velocityTangentToSpeaker = totalSpeed * Math.sin(velocityRotation2d.getRadians() - currentAngleToSpeaker.getRadians());

        // Calculate the desired rotation to the speaker, taking into account the tangent velocity
        Rotation2d desiredRotation2d = Rotation2d.fromRadians(
            currentAngleToSpeaker.getRadians() - Math.atan2(
                velocityTangentToSpeaker, 
                rpmToVelocity(calculateSpeed(robotPose, true).getSpeeds())
            )
        );

        // Update the robot's pose with the desired rotation
        desiredSWDPose = new Pose2d(robotPose.getTranslation(), desiredRotation2d);

        // Return the desired rotation
        return desiredRotation2d;
    }

    /**
     * This method is averaging the speeds to make a rough estimate of the speed of the note (or the edge of the wheels).
     * The formula used is V = 2π * D/2 * RPM/60.
     * First, it converts from Rotations per Minute to Rotations per Second.
     * Then, it converts from Rotations per Second to Radians per Second.
     * Finally, it multiplies by the radius of the wheel contacting it.
     * As v = rw (w = omega | angular velocity of wheel).
     * 
     * Converts RPM (Revolutions Per Minute) to velocity in meters per second.
     * @param speeds a pair of RPM values representing the speeds of two shooter wheels
     * @return the velocity in meters per second
     */
    public double rpmToVelocity(Pair<Double, Double> speeds) {
        double rotationsPerMinute = (speeds.getFirst() + speeds.getSecond()) / 2.0;
        double rotationsPerSecond = rotationsPerMinute / 60.0;
        double radiansPerSecond = rotationsPerSecond * Math.PI;

        double diameter = ShooterConstants.WHEEL_DIAMETER_METERS;

        desiredMPSForNote = diameter * radiansPerSecond;

        // Normally this is 1 radius * 2 pi
        // but we are doing 2 radius * 1 pi
        // because we are given a diameter
        return diameter * radiansPerSecond;
    }
    ```

[GalexY727]

That only marks the first checkbox as complete. We need to use the component of our velocity that is normal (perpendicular) to the speaker to figure out how we should modify the speeds of the shooter to compensate for already having some velocity getting closer to further from the speaker.

[Jacob1010-h]

Oh, I forgot to comment abt this, but the second checkbox should now be resolved. Because of the following:
(all that is needed is direct implementation of this method in where we want to shoot, not just the traj of the note)

private SpeedAngleTriplet calculateSWDSpeedAngleTripletToSpeaker(Supplier<Pose2d> pose, Supplier<ChassisSpeeds> speeds){
        double dt = DriverUI.currentTimestamp - DriverUI.previousTimestamp;

        return new SpeedAngleTriplet(
            calculateSWDShooterSpeedsToSpeaker(pose, speeds, dt),
            calculateSWDPivotAngleToSpeaker(pose, speeds, dt).getDegrees()
        );
    }

[Jacob1010-h]

I think this can finally be closed with the addition of #142

[Jacob1010-h]

this is good