-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
basic math for shooter position finding
- Loading branch information
1 parent
a6999e3
commit 73fc54b
Showing
7 changed files
with
311 additions
and
75 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,25 @@ | ||
| package frc.robot.subsystems; | ||
|
|
||
| import edu.wpi.first.math.geometry.Pose2d; | ||
| import edu.wpi.first.math.geometry.Pose3d; | ||
| import edu.wpi.first.math.geometry.Rotation3d; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import lib.utils.FieldConstants; | ||
| import lib.utils.AllianceFlipUtil; | ||
|
|
||
| public class Supersystem extends SubsystemBase { | ||
| public Supersystem() { | ||
|
|
||
| } | ||
|
|
||
| public double calcShooterAngle(Pose2d robotPose) { | ||
| Pose3d speakerPose = new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d()); | ||
| double groundDistance = Math.sqrt(Math.pow(speakerPose.getX(), 2) + Math.pow(speakerPose.getY(), 2)); | ||
| return Math.atan2(groundDistance, FieldConstants.CENTER_SPEAKER.getZ()); | ||
| } | ||
|
|
||
| public double calcMaxShooterAngle(Pose2d robotPose, double shoulderAngle) { | ||
| return 0; | ||
| } | ||
| } | ||
|
|
103 changes: 50 additions & 53 deletions
103
src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,61 +1,58 @@ | ||
| package frc.robot.subsystems.arm; | ||
|
|
||
|
|
||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.geometry.Rotation3d; | ||
| import edu.wpi.first.math.geometry.Translation3d; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import frc.robot.Constants; | ||
|
|
||
| public class ArmSubsystem extends SubsystemBase { | ||
| private final ArmIO m_io; | ||
| public ArmSubsystem(ArmIO io) { | ||
| m_io = io; | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| m_io.updateInputs(new ArmIOInputsAutoLogged()); | ||
|
|
||
| // m_io.setWristVoltage(0.0); | ||
| // m_io.setShoulderVoltage(0.0); | ||
| SmartDashboard.putNumber("Recorded Wrist Position", m_io.getWristPosition().getDegrees()); | ||
| SmartDashboard.putNumber("Recorded Shoulder Position", m_io.getShoulderPosition().getDegrees()); | ||
| } | ||
|
|
||
| public void setShoulderPower(double power) { | ||
| m_io.setShoulderVoltage(power * 12.0); | ||
| } | ||
|
|
||
| public void setShoulderPosition(double degrees) { | ||
| m_io.setShoulderAngle(degrees); | ||
| } | ||
|
|
||
| public void setWristPower(double power) {m_io.setWristVoltage(power * 12.0);} | ||
|
|
||
| public void setWristPosition(double degrees) { | ||
| m_io.setWristAngle(degrees); | ||
| } | ||
|
|
||
| public Command setShoulderPowerFactory(double power) { | ||
| return runOnce(() -> setShoulderPower(power)); | ||
| } | ||
|
|
||
| public Command setShoulderPositionFactory(double degrees) { | ||
| return runOnce(() -> setShoulderPosition(degrees)); | ||
| } | ||
|
|
||
| public Command setWristPowerFactory(double power) { | ||
| return runOnce(() -> setWristPower(power)); | ||
| } | ||
|
|
||
| public Command setWristPositionFactory(double degrees) { | ||
| return runOnce(() -> setWristPosition(degrees)); | ||
| } | ||
|
|
||
| public Command stopArmFactory() { | ||
| return runOnce(() -> { | ||
| setWristPower(0.0); | ||
| setShoulderPower(0.0); | ||
| }); | ||
| } | ||
| public enum ArmStates { | ||
| STOW, | ||
| AIM, | ||
| AIM_BLOCKED, | ||
| AMP, | ||
| TRAP, | ||
| MOVING | ||
| } | ||
|
|
||
| private final ArmIO m_io; | ||
| public ArmSubsystem(ArmIO io) { | ||
| m_io = io; | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| m_io.updateInputs(new ArmIOInputsAutoLogged()); | ||
| } | ||
|
|
||
| public void setShoulderPower(double power) { | ||
| m_io.setShoulderVoltage(power * 12.0); | ||
| } | ||
|
|
||
| public void setShoulderPosition(double degrees) { | ||
| m_io.setShoulderAngle(degrees); | ||
| } | ||
|
|
||
| public void setWristPower(double power) { | ||
| m_io.setWristVoltage(power * 12.0); | ||
| } | ||
|
|
||
| public void setWristPosition(double degrees) { | ||
| m_io.setWristAngle(degrees); | ||
| } | ||
|
|
||
| public Translation3d getShooterTranslation(Rotation2d shoulderRotation, Rotation2d shooterRotation) { | ||
| return Constants.ArmConstants.PIVOT_TRANSLATION_METERS.plus( | ||
| new Translation3d( | ||
| Constants.ArmConstants.SHOULDER_BAR_LENGTH_METERS, | ||
| new Rotation3d(0.0, shoulderRotation.getDegrees(), 0.0))) | ||
| .plus( | ||
| new Translation3d( | ||
| Constants.ArmConstants.SHOOTER_BAR_LENGTH_METERS, | ||
| new Rotation3d(0.0, shooterRotation.getDegrees(), 0.0)) | ||
| ); | ||
| } | ||
| } | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,98 @@ | ||
| // Copyright (c) 2023 FRC 6328 | ||
| // http://github.com/Mechanical-Advantage | ||
| // | ||
| // Use of this source code is governed by an MIT-style | ||
| // license that can be found in the LICENSE file at | ||
| // the root directory of this project. | ||
|
|
||
| package lib.utils; | ||
|
|
||
| import edu.wpi.first.math.geometry.Pose2d; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.geometry.Translation2d; | ||
| import edu.wpi.first.math.geometry.Translation3d; | ||
| import edu.wpi.first.math.trajectory.Trajectory; | ||
| import edu.wpi.first.wpilibj.DriverStation; | ||
| import edu.wpi.first.wpilibj.DriverStation.Alliance; | ||
|
|
||
| /** | ||
| * Utility functions for flipping from the blue to red alliance. By default, all translations and | ||
| * poses in {@link FieldConstants} are stored with the origin at the rightmost point on the blue | ||
| * alliance wall. | ||
| */ | ||
| public class AllianceFlipUtil { | ||
| private AllianceFlipUtil() {} | ||
| /** Flips a translation to the correct side of the field based on the current alliance color. */ | ||
| public static Translation2d apply(Translation2d translation) { | ||
| if (shouldFlip()) { | ||
| return new Translation2d(FieldConstants.FIELD_LENGTH - translation.getX(), translation.getY()); | ||
| } else { | ||
| return translation; | ||
| } | ||
| } | ||
|
|
||
| public static Translation3d apply(Translation3d translation) { | ||
| if (shouldFlip()) { | ||
| return new Translation3d(FieldConstants.FIELD_LENGTH - translation.getX(), | ||
| translation.getY(), | ||
| translation.getZ()); | ||
| } else { | ||
| return translation; | ||
| } | ||
| } | ||
|
|
||
| /** Flips an x coordinate to the correct side of the field based on the current alliance color. */ | ||
| public static double apply(double xCoordinate) { | ||
| if (shouldFlip()) { | ||
| return FieldConstants.FIELD_LENGTH - xCoordinate; | ||
| } else { | ||
| return xCoordinate; | ||
| } | ||
| } | ||
|
|
||
| /** Flips a rotation based on the current alliance color. */ | ||
| public static Rotation2d apply(Rotation2d rotation) { | ||
| if (shouldFlip()) { | ||
| return new Rotation2d(-rotation.getCos(), rotation.getSin()); | ||
| } else { | ||
| return rotation; | ||
| } | ||
| } | ||
|
|
||
| /** Flips a pose to the correct side of the field based on the current alliance color. */ | ||
| public static Pose2d apply(Pose2d pose) { | ||
| if (shouldFlip()) { | ||
| return new Pose2d( | ||
| FieldConstants.FIELD_LENGTH - pose.getX(), | ||
| pose.getY(), | ||
| new Rotation2d(-pose.getRotation().getCos(), pose.getRotation().getSin())); | ||
| } else { | ||
| return pose; | ||
| } | ||
| } | ||
|
|
||
| /** | ||
| * Flips a trajectory state to the correct side of the field based on the current alliance color. | ||
| */ | ||
| public static Trajectory.State apply(Trajectory.State state) { | ||
| if (shouldFlip()) { | ||
| return new Trajectory.State( | ||
| state.timeSeconds, | ||
| state.velocityMetersPerSecond, | ||
| state.accelerationMetersPerSecondSq, | ||
| new Pose2d( | ||
| FieldConstants.FIELD_LENGTH - state.poseMeters.getX(), | ||
| state.poseMeters.getY(), | ||
| new Rotation2d( | ||
| -state.poseMeters.getRotation().getCos(), | ||
| state.poseMeters.getRotation().getSin())), | ||
| -state.curvatureRadPerMeter); | ||
| } else { | ||
| return state; | ||
| } | ||
| } | ||
|
|
||
| private static boolean shouldFlip() { | ||
| return DriverStation.getAlliance().orElseGet(() -> Alliance.Blue) == Alliance.Red; | ||
| } | ||
| } |
Oops, something went wrong.