-
Notifications
You must be signed in to change notification settings - Fork 12
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #23 from aaditsangvikar/main
On-the-fly shooting - untested
- Loading branch information
Showing
11 changed files
with
370 additions
and
59 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
40 changes: 40 additions & 0 deletions
40
src/main/java/frc/robot/commands/AdvancedSpinUpCommand.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,40 @@ | ||
| package frc.robot.commands; | ||
|
|
||
| import edu.wpi.first.math.geometry.Pose2d; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import frc.robot.constants.ShooterConstants; | ||
| import frc.robot.subsystems.ShooterSubsystem; | ||
|
|
||
| public class AdvancedSpinUpCommand extends Command { | ||
| private final ShooterSubsystem shooterSubsystem; | ||
|
|
||
| private final Pose2d robotPose; | ||
|
|
||
| /** | ||
| * Command to spin up the shooter | ||
| * @param shooterSubsystem Instance of {@link ShooterSubsystem} | ||
| * @param robotPose Instance of {@link Pose2d} | ||
| */ | ||
| public AdvancedSpinUpCommand(ShooterSubsystem shooterSubsystem, Pose2d robotPose) { | ||
| this.shooterSubsystem = shooterSubsystem; | ||
| this.robotPose = robotPose; | ||
|
|
||
| addRequirements(shooterSubsystem); | ||
| } | ||
|
|
||
| @Override | ||
| public void execute() { | ||
|
|
||
| if (robotPose.getX() < 9) { | ||
| shooterSubsystem.setSpeed(ShooterConstants.shooterRPM); | ||
| } else { | ||
| shooterSubsystem.setVoltage(0,0); | ||
| } | ||
|
|
||
| } | ||
|
|
||
| @Override | ||
| public void end(boolean interrupted) { | ||
| shooterSubsystem.setVoltage(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
9 changes: 6 additions & 3 deletions
9
...ava/frc/robot/commands/SpinUpCommand.java → ...rc/robot/commands/BasicSpinUpCommand.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
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
84 changes: 84 additions & 0 deletions
84
src/main/java/frc/robot/commands/RotateToSpeakerAndDriveCommand.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,84 @@ | ||
| package frc.robot.commands; | ||
|
|
||
| import edu.wpi.first.math.controller.ProfiledPIDController; | ||
| import edu.wpi.first.math.geometry.Pose2d; | ||
| import edu.wpi.first.math.geometry.Transform2d; | ||
| import edu.wpi.first.math.geometry.Translation2d; | ||
| import edu.wpi.first.wpilibj.DriverStation; | ||
| import edu.wpi.first.wpilibj.XboxController; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import frc.robot.constants.ShooterConstants; | ||
| import frc.robot.constants.VisionConstants; | ||
| import frc.robot.subsystems.IndexerSubsystem; | ||
| import frc.robot.subsystems.SwerveSubsystem; | ||
|
|
||
| import java.util.function.DoubleSupplier; | ||
|
|
||
| public class RotateToSpeakerAndDriveCommand extends Command { | ||
| private final SwerveSubsystem swerveSubsystem; | ||
| private final DoubleSupplier forward; | ||
| private final DoubleSupplier sideways; | ||
| private final DoubleSupplier radians; | ||
| private final double odometryRotation; | ||
|
|
||
| private final ProfiledPIDController rotationPID = new ProfiledPIDController( | ||
| VisionConstants.rotateToP, | ||
| VisionConstants.rotateToI, | ||
| VisionConstants.rotateToD, | ||
| VisionConstants.rotateToConstraints | ||
| ); | ||
|
|
||
| /** | ||
| * Rotates the robot to face the speaker, while still allowing the driver to control forward and backward movement | ||
| * @param swerveSubsystem The instance of {@link SwerveSubsystem} | ||
| * @param forward The desired forward percentage of the robot | ||
| * @param sideways The desired sideways percentage of the robot | ||
| * @param radians The desired rotation speed of the robot | ||
| */ | ||
| public RotateToSpeakerAndDriveCommand(SwerveSubsystem swerveSubsystem, DoubleSupplier forward, DoubleSupplier sideways, DoubleSupplier radians) { | ||
| this.swerveSubsystem = swerveSubsystem; | ||
| this.forward = forward; | ||
| this.sideways = sideways; | ||
| this.radians = radians; | ||
|
|
||
| //Rotation tolerance in radians | ||
| rotationPID.setTolerance(0.05); | ||
|
|
||
| //Robot speed toward/away from the speaker (y-direction) | ||
| double robotYSpeed = swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond; | ||
| //Robot speed toward/away from the speaker (x-direction) | ||
| double robotXSpeed = swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond; | ||
| //Note speed in x-direction(forward, as opposed to up) | ||
| double noteSpeedX = robotXSpeed + ShooterConstants.shooterNoteSpeedX; | ||
|
|
||
| //Getting speaker pose relative to alliance color | ||
| Pose2d speakerPose = (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue)? ShooterConstants.speakerPoseBlue : ShooterConstants.speakerPoseRed; | ||
| //Current robot pose | ||
| Pose2d robotPose = swerveSubsystem.getPose(); | ||
|
|
||
| //'Moving' speaker position based on robot speed | ||
| double speakerYTranslation = robotYSpeed * ((speakerPose.getX()-robotPose.getX()) / noteSpeedX); | ||
|
|
||
| //Calculated angle to rotate to | ||
| double angle = Math.atan2((speakerPose.getY() + speakerYTranslation) - robotPose.getY(), speakerPose.getX()-robotPose.getX()); | ||
|
|
||
| //Rotation PID Calculations | ||
| this.odometryRotation = rotationPID.calculate(robotPose.getRotation().getRadians(), angle); | ||
|
|
||
| addRequirements(swerveSubsystem); | ||
|
|
||
| } | ||
|
|
||
| @Override | ||
| public void execute() { | ||
|
|
||
| swerveSubsystem.drive( | ||
| forward.getAsDouble(), | ||
| sideways.getAsDouble(), | ||
| odometryRotation, | ||
| true, | ||
| true | ||
| ); | ||
|
|
||
| } | ||
| } |
Oops, something went wrong.