From 31fbfc306d9caf0c39082f8a778e2268a6232a30 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Tue, 30 Jan 2024 18:54:25 -0600 Subject: [PATCH] Moved shooter angle stuff --- src/main/java/frc/robot/RobotContainer.java | 5 +- .../java/frc/robot/commands/AngleShooter.java | 8 +- .../subsystems/AngleShooterSubsystem.java | 119 ++++++++++++++++++ .../robot/subsystems/DrivetrainSubsystem.java | 4 +- .../robot/subsystems/ShooterSubsystem.java | 103 +-------------- 5 files changed, 132 insertions(+), 107 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4077ee9e..6c215ba3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,6 +36,7 @@ import frc.robot.settings.Constants.IntakeConstants; import frc.robot.settings.Constants.PathConstants; import frc.robot.settings.Constants.ShooterConstants; +import frc.robot.subsystems.AngleShooterSubsystem; import frc.robot.subsystems.Climber; import frc.robot.commands.ManualShoot; import frc.robot.commands.RotateRobot; @@ -94,6 +95,7 @@ public class RobotContainer { private DrivetrainSubsystem driveTrain; private IntakeSubsystem intake; private ShooterSubsystem shooter; + private AngleShooterSubsystem angleShooterSubsystem; private Drive defaultDriveCommand; private Climber climber; private Lights lights; @@ -164,7 +166,8 @@ private void driveTrainInst() { driveTrain.setDefaultCommand(defaultDriveCommand); } private void shooterInst() { - shooter = new ShooterSubsystem(ShooterConstants.SHOOTER_MOTOR_POWER, ()-> (operatorController.getPOV() == 0)); + shooter = new ShooterSubsystem(ShooterConstants.SHOOTER_MOTOR_POWER); + angleShooterSubsystem = new AngleShooterSubsystem(()-> (operatorController.getPOV() == 0)); } private void intakeInst() { intake = new IntakeSubsystem(); diff --git a/src/main/java/frc/robot/commands/AngleShooter.java b/src/main/java/frc/robot/commands/AngleShooter.java index 31073561..9ef57ac1 100644 --- a/src/main/java/frc/robot/commands/AngleShooter.java +++ b/src/main/java/frc/robot/commands/AngleShooter.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.settings.Constants.ShooterConstants; -import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.AngleShooterSubsystem; public class AngleShooter extends Command { /** Creates a new AngleShooter. */ @@ -18,11 +18,11 @@ public class AngleShooter extends Command { double currentShooterAngle; double differenceAngle; double angleSpeed; - ShooterSubsystem m_shooter; + AngleShooterSubsystem m_shooter; double desiredShooterAngleSpeed; - - public AngleShooter(ShooterSubsystem shooter, DoubleSupplier desiredShooterAngleSupplier) { + + public AngleShooter(AngleShooterSubsystem shooter, DoubleSupplier desiredShooterAngleSupplier) { // Use addRequirements() here to declare subsystem dependencies.\ m_shooter = shooter; this.desiredShooterAngleSupplier = desiredShooterAngleSupplier; diff --git a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java new file mode 100644 index 00000000..574a5a9b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java @@ -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 = 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); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 73ce46ef..6f0bcce1 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -382,8 +382,8 @@ public Pose2d getAverageBotPose(LimelightValues ll2, LimelightValues ll3) { public void periodic() { SmartDashboard.putString("alliance:", DriverStation.getAlliance().get().toString()); updateOdometry(); - ShooterSubsystem.setDTPose(getPose()); - ShooterSubsystem.setDTChassisSpeeds(getChassisSpeeds()); + AngleShooterSubsystem.setDTPose(getPose()); + AngleShooterSubsystem.setDTChassisSpeeds(getChassisSpeeds()); if (SmartDashboard.getBoolean("use limelight", false)) { LimelightValues ll2 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT2_NAME); LimelightValues ll3 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT3_NAME); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index b8dacc2e..4ed8f74b 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -3,8 +3,7 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; - import com.ctre.phoenix6.hardware.CANcoder; -import com.revrobotics.CANSparkMax; + import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.SparkRelativeEncoder; @@ -15,34 +14,18 @@ import frc.robot.commands.AngleShooter; import frc.robot.commands.RotateRobot; import frc.robot.settings.Constants; -import frc.robot.settings.Constants.Field; import frc.robot.settings.Constants.ShooterConstants; - import edu.wpi.first.math.controller.PIDController; -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; - import edu.wpi.first.hal.can.CANStreamOverflowException; - import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; - -import static frc.robot.settings.Constants.ShooterConstants.*; - -import java.util.Optional; -import java.util.function.BooleanSupplier; + import static frc.robot.settings.Constants.ShooterConstants.*; public class ShooterSubsystem extends SubsystemBase { CANSparkMax shooterR; CANSparkMax shooterL; - CANSparkMax pitchMotor; double runSpeed; double differenceAngle; double currentHeading; - double speakerDist; - double deltaY; - double deltaX; double m_DesiredShooterAngle; SparkPIDController shooterPID; @@ -56,7 +39,6 @@ public class ShooterSubsystem extends SubsystemBase { double kMinOutput = Constants.ShooterConstants.kMinOutput; RelativeEncoder encoder1; - CANcoder angleEncoder; //speaker angle calculating variables: @@ -64,26 +46,12 @@ public class ShooterSubsystem extends SubsystemBase { RotateRobot rotateRobot; AngleShooter angleShooter; int accumulativeTurns; - double shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS; - double shootingTime; - double currentXSpeed; - public static Pose2d dtvalues; - public static ChassisSpeeds DTChassisSpeeds; - double currentYSpeed; - Translation2d targetOffset; - double offsetSpeakerX; - double offsetSpeakerY; - Translation2d adjustedTarget; - double offsetSpeakerdist; - BooleanSupplier aimAtAmp; /** Creates a new Shooter. */ - public ShooterSubsystem(double runSpeed, BooleanSupplier aimAtAmp) { + public ShooterSubsystem(double runSpeed) { SparkPIDController shooterPID; - this.aimAtAmp = aimAtAmp; shooterR = new CANSparkMax(ShooterConstants.SHOOTER_R_MOTORID, MotorType.kBrushless); shooterL = new CANSparkMax(ShooterConstants.SHOOTER_L_MOTORID, MotorType.kBrushless); - pitchMotor = new CANSparkMax(ShooterConstants.PITCH_MOTOR_ID, MotorType.kBrushless); shooterR.restoreFactoryDefaults(); shooterL.follow(shooterR); shooterL.setInverted(true); @@ -94,7 +62,6 @@ public ShooterSubsystem(double runSpeed, BooleanSupplier aimAtAmp) { encoder1 = shooterR.getEncoder(SparkRelativeEncoder.Type.kQuadrature, 4096); shooterPID = shooterR.getPIDController(); - pitchPID = pitchMotor.getPIDController(); pitchPID.setFF(pitchFeedForward); @@ -152,70 +119,6 @@ public void shootThing(double runSpeed) { public void turnOff(){ shooterR.set(0); shooterL.set(0); - } - public void pitchShooter(double pitchSpeed){ - pitchMotor.set(pitchSpeed); - } - public double getShooterAngle(){ - return angleEncoder.getPosition().getValueAsDouble()*DEGREES_PER_ROTATION; - } - public static void setDTPose(Pose2d pose) { - dtvalues = pose; - } - public static void setDTChassisSpeeds(ChassisSpeeds speeds) { - DTChassisSpeeds = speeds; - } - public double calculateSpeakerAngle() { - shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS; - //triangle for robot angle - Optional 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); - } - deltaX = Math.abs(dtvalues.getX() - Field.SPEAKER_X); - speakerDist = Math.sqrt(Math.pow(deltaY, 2) + Math.pow(deltaX, 2)); - SmartDashboard.putNumber("dist to speakre", speakerDist); - - shootingTime = speakerDist/shootingSpeed; //calculates how long the note will take to reach the target - currentXSpeed = DTChassisSpeeds.vxMetersPerSecond; - currentYSpeed = DTChassisSpeeds.vyMetersPerSecond; - 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 - offsetSpeakerX = Field.SPEAKER_X+targetOffset.getX(); - 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); - 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-SHOOTER_HEIGHT, 2)); - double desiredShooterAngle = Math.toDegrees(Math.asin(Field.SPEAKER_Z-SHOOTER_HEIGHT / totalDistToSpeaker)); - - SmartDashboard.putNumber("desired shooter angle", desiredShooterAngle); - - 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); - } } }