-
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.
- Loading branch information
1 parent
c681e67
commit 31fbfc3
Showing
5 changed files
with
132 additions
and
107 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
119 changes: 119 additions & 0 deletions
119
src/main/java/frc/robot/subsystems/AngleShooterSubsystem.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 |
|---|---|---|
| @@ -0,0 +1,119 @@ | ||
| package frc.robot.subsystems; | ||
|
|
||
| import frc.robot.settings.Constants.Field; | ||
| import frc.robot.settings.Constants.ShooterConstants; | ||
|
|
||
| import java.util.Optional; | ||
| import java.util.function.BooleanSupplier; | ||
| import java.util.function.DoubleSupplier; | ||
|
|
||
| import com.ctre.phoenix6.hardware.CANcoder; | ||
| import com.revrobotics.CANSparkMax; | ||
| import com.revrobotics.SparkPIDController; | ||
| import com.revrobotics.CANSparkLowLevel.MotorType; | ||
|
|
||
| import edu.wpi.first.math.geometry.Pose2d; | ||
| import edu.wpi.first.math.geometry.Translation2d; | ||
| import edu.wpi.first.math.kinematics.ChassisSpeeds; | ||
| import edu.wpi.first.wpilibj.DriverStation; | ||
| import edu.wpi.first.wpilibj.DriverStation.Alliance; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
|
|
||
| public class AngleShooterSubsystem extends SubsystemBase { | ||
| CANSparkMax pitchMotor; | ||
| SparkPIDController pitchPID; | ||
| CANcoder angleEncoder; | ||
| BooleanSupplier aimAtAmp; | ||
| double shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS; | ||
| public static Pose2d dtvalues; | ||
| public static ChassisSpeeds DTChassisSpeeds; | ||
|
|
||
| public AngleShooterSubsystem(BooleanSupplier aimAtAmp) { | ||
| this.aimAtAmp = aimAtAmp; | ||
| pitchMotor = new CANSparkMax(ShooterConstants.PITCH_MOTOR_ID, MotorType.kBrushless); | ||
|
|
||
| pitchPID = pitchMotor.getPIDController(); | ||
|
|
||
| pitchPID.setFF(ShooterConstants.pitchFeedForward); | ||
| } | ||
|
|
||
| public void pitchShooter(double pitchSpeed) { | ||
| pitchMotor.set(pitchSpeed); | ||
| } | ||
|
|
||
| public double getShooterAngle() { | ||
| return angleEncoder.getPosition().getValueAsDouble() * ShooterConstants.DEGREES_PER_ROTATION; | ||
| } | ||
|
|
||
| public static void setDTPose(Pose2d pose) { | ||
| dtvalues = pose; | ||
| } | ||
|
|
||
| public static void setDTChassisSpeeds(ChassisSpeeds speeds) { | ||
| DTChassisSpeeds = speeds; | ||
| } | ||
|
|
||
| public double calculateSpeakerAngle() { | ||
| double deltaY; | ||
| shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS; | ||
| // triangle for robot angle | ||
| Optional<Alliance> alliance = DriverStation.getAlliance(); | ||
| if (alliance.isPresent() && alliance.get() == Alliance.Red) { | ||
| deltaY = Math.abs(dtvalues.getY() - Field.RED_SPEAKER_Y); | ||
| } else { | ||
| deltaY = Math.abs(dtvalues.getY() - Field.BLUE_SPEAKER_Y); | ||
| } | ||
| double deltaX = Math.abs(dtvalues.getX() - Field.SPEAKER_X); | ||
| double speakerDist = Math.sqrt(Math.pow(deltaY, 2) + Math.pow(deltaX, 2)); | ||
| SmartDashboard.putNumber("dist to speakre", speakerDist); | ||
|
|
||
| double shootingTime = speakerDist / shootingSpeed; // calculates how long the note will take to reach the target | ||
| double currentXSpeed = DTChassisSpeeds.vxMetersPerSecond; | ||
| double currentYSpeed = DTChassisSpeeds.vyMetersPerSecond; | ||
| Translation2d targetOffset = new Translation2d(currentXSpeed * shootingTime, currentYSpeed * shootingTime); | ||
| // line above calculates how much our current speed will affect the ending | ||
| // location of the note if it's in the air for ShootingTime | ||
|
|
||
| // next 3 lines set where we actually want to aim, given the offset our shooting | ||
| // will have based on our speed | ||
| double offsetSpeakerX = Field.SPEAKER_X + targetOffset.getX(); | ||
| double offsetSpeakerY; | ||
|
|
||
| if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) { | ||
| offsetSpeakerY = Field.RED_SPEAKER_Y + targetOffset.getY(); | ||
| } else { | ||
| offsetSpeakerY = Field.BLUE_SPEAKER_Y + targetOffset.getY(); | ||
| } | ||
| double offsetDeltaX = Math.abs(dtvalues.getX() - offsetSpeakerX); | ||
| double offsetDeltaY = Math.abs(dtvalues.getY() - offsetSpeakerY); | ||
| double offsetSpeakerdist = Math.sqrt(Math.pow(offsetDeltaX, 2) + Math.pow(offsetDeltaY, 2)); | ||
| SmartDashboard.putString("offset amount", targetOffset.toString()); | ||
| SmartDashboard.putString("offset speaker location", | ||
| new Translation2d(offsetSpeakerX, offsetSpeakerY).toString()); | ||
| // getting desired robot angle | ||
| double totalDistToSpeaker = Math | ||
| .sqrt(Math.pow(offsetSpeakerdist, 2) + Math.pow(Field.SPEAKER_Z - ShooterConstants.SHOOTER_HEIGHT, 2)); | ||
| double desiredShooterAngle = Math | ||
| .toDegrees(Math.asin(Field.SPEAKER_Z - ShooterConstants.SHOOTER_HEIGHT / totalDistToSpeaker)); | ||
|
|
||
| SmartDashboard.putNumber("desired shooter angle", desiredShooterAngle); | ||
|
|
||
| double differenceAngle = (desiredShooterAngle - this.getShooterAngle()); | ||
| SmartDashboard.putNumber("differenceAngleShooter", differenceAngle); | ||
|
|
||
| return differenceAngle; | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| if (!aimAtAmp.getAsBoolean()) { | ||
| double desiredShooterAngleSpeed = calculateSpeakerAngle() * ShooterConstants.AUTO_AIM_SHOOTER_kP; | ||
| pitchShooter(desiredShooterAngleSpeed); | ||
| } else { | ||
| double differenceAmp = Field.AMPLIFIER_ANGLE - this.getShooterAngle(); | ||
| double ampSpeed = differenceAmp * ShooterConstants.AUTO_AIM_SHOOTER_kP; | ||
| pitchShooter(ampSpeed); | ||
| } | ||
| } | ||
| } |
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