climb = new Climb();
spindexer = new Spindexer();
- case WaffleHouse:
+ case WaffleHouse: // AKA Betabot
turret = new Turret();
shooter = new Shooter();
hood = new Hood();
case Vertigo: // AKA "French Toast"
drive = new Drivetrain(vision, new GyroIOPigeon2());
- driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood);
+ driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer);
operator = new Operator(drive);
// Detected objects need access to the drivetrain
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.filter.LinearFilter;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Twist2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.Constants;
-import frc.robot.constants.FieldConstants;
-import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.subsystems.turret.ShotInterpolation;
-import frc.robot.subsystems.turret.Turret;
-import frc.robot.subsystems.turret.TurretConstants;
-// import frc.robot.util.FieldZone;
-
-public class TurretAutoShoot extends Command {
- private Turret turret;
- private Drivetrain drivetrain;
-
- private double turretSetpoint;
-
- private Pose2d drivepose;
-
- private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
- private Rotation2d lastTurretAngle;
- private Rotation2d turretAngle;
- private double turretVelocity;
- private final double phaseDelay = 0.03;
-
- public TurretAutoShoot(Turret turret, Drivetrain drivetrain) {
- this.turret = turret;
- this.drivetrain = drivetrain;
- drivepose = drivetrain.getPose();
-
- addRequirements(turret);
- }
-
- public void updateTurretSetpoint(Pose2d drivepose) {
-
- Translation2d target = FieldConstants.getHubTranslation().toTranslation2d();
- Pose2d turretPosition = drivepose.transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(),TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d()));
- double turretToTargetDistance = target.getDistance(turretPosition.getTranslation());
-
- // If the robot is moving, adjust the target position based on velocity
- ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
- ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
-
- double turretVelocityX =
- fieldRelVel.vxMetersPerSecond
- + fieldRelVel.omegaRadiansPerSecond
- * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.cos(drivepose.getRotation().getRadians())
- - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.sin(drivepose.getRotation().getRadians()));
- double turretVelocityY =
- fieldRelVel.vyMetersPerSecond
- + fieldRelVel.omegaRadiansPerSecond
- * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.cos(drivepose.getRotation().getRadians())
- - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.sin(drivepose.getRotation().getRadians()));
-
- // Account for imparted velocity by robot (turret) to offset
- double timeOfFlight;
- Pose2d lookaheadPose = turretPosition;
- double lookaheadTurretToTargetDistance = turretToTargetDistance;
-
- // Loop (20) until lookahreadTurretToTargetDistance converges
- for (int i = 0; i < 20; i++) {
- timeOfFlight = ShotInterpolation.timeOfFlightMap.get(lookaheadTurretToTargetDistance);
- double offsetX = turretVelocityX * timeOfFlight;
- double offsetY = turretVelocityY * timeOfFlight;
- lookaheadPose =
- new Pose2d(
- turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)),
- turretPosition.getRotation());
- lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation());
- }
- turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();
- if (lastTurretAngle == null) {
- lastTurretAngle = turretAngle;
- }
- turretVelocity =
- turretAngleFilter.calculate(
- turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
- lastTurretAngle = turretAngle;
-
- // Add 180 since drivetrain is backwards
- double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() + Math.PI);
- turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint), -180.0, 180.0);
- }
-
- public void updateDrivePose(){
- ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
- drivepose = drivetrain.getPose().exp(
- new Twist2d(
- robotRelVel.vxMetersPerSecond * phaseDelay,
- robotRelVel.vyMetersPerSecond * phaseDelay,
- robotRelVel.omegaRadiansPerSecond * phaseDelay));
- }
-
- @Override
- public void initialize() {
- updateDrivePose();
- updateTurretSetpoint(drivepose);
- turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2));
- }
-
- @Override
- public void execute() {
- updateDrivePose();
- updateTurretSetpoint(drivepose);
- turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2));
- }
-
- @Override
- public void end(boolean interrupted) {
- // Set the turret to a safe position when the command ends
- turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
- }
-
-}
-
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.controls.PS5ControllerDriverConfig;
-import frc.robot.subsystems.turret.Turret;
-
-public class TurretJoyStickAim extends Command{
- private Turret turret;
- private PS5ControllerDriverConfig driver;
-
- public TurretJoyStickAim(Turret turret, PS5ControllerDriverConfig driver){
- this.turret = turret;
- this.driver = driver;
- }
-
- Rotation2d rotation2d = new Rotation2d(driver.getRawSideTranslation(), driver.getRawForwardTranslation());
- double angle = Units.radiansToDegrees(MathUtil.angleModulus(rotation2d.getDegrees()));
-
- @Override
- public void execute() {
- //turret.setSetpoint(angle, 0);
- }
-
-}
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.util.Units;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.Robot;
-import frc.robot.commands.gpm.SimpleAutoShoot;
-import frc.robot.commands.gpm.TurretAutoShoot;
-import frc.robot.commands.gpm.TurretJoyStickAim;
+import frc.robot.commands.gpm.AutoShootCommand;
import frc.robot.constants.Constants;
-import frc.robot.constants.FieldConstants;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.shooter.Shooter;
-import frc.robot.subsystems.shooter.ShooterConstants;
import frc.robot.subsystems.spindexer.Spindexer;
import frc.robot.subsystems.turret.Turret;
import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.hood.HoodConstants;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.intake.IntakeConstants;
import lib.controllers.PS5Controller;
private Pose2d alignmentPose = null;
private Command turretAutoShoot;
- private Command simpleTurretAutoShoot;
- private TurretJoyStickAim turretJoyStickAim;
+ private Command autoShoot;
private boolean intakeBoolean = true;
- public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood) {
+ public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood, Intake intake, Spindexer spindexer) {
super(drive);
this.shooter = shooter;
this.turret = turret;
this.hood = hood;
+ this.intake = intake;
+ this.spindexer = spindexer;
}
public void configureControls() {
}));
}
- // driver.get(PS5Button.LB).onTrue(
- // new SequentialCommandGroup(
- // new InstantCommand(()-> shooter.setShooter(-ShooterConstants.SHOOTER_VELOCITY)),
- // new WaitCommand(0.8),
- // new InstantCommand(()-> shooter.setFeeder(ShooterConstants.FEEDER_RUN_POWER))
- // )
- // ).onFalse(
- // new InstantCommand(() -> {
- // shooter.setFeeder(0);
- // shooter.setShooter(0);
- // }));
- //driver.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> shooter.setShooter(ShooterConstants.SHOOTER_VELOCITY))).onFalse(new InstantCommand(() -> shooter.setShooter(0)));
-
- // driver.get(PS5Button.SQUARE).onTrue(
- // new InstantCommand(()->{
- // if (simpleTurretAutoShoot != null && simpleTurretAutoShoot.isScheduled()){
- // simpleTurretAutoShoot.cancel();
- // } else{
- // simpleTurretAutoShoot = new SimpleAutoShoot(turret, getDrivetrain(), shooter);
- // CommandScheduler.getInstance().schedule(simpleTurretAutoShoot);
- // }
- // })
- // );
-
-
- driver.get(PS5Button.SQUARE).onTrue(new InstantCommand(() -> hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0)));
- driver.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0)));
- driver.get(PS5Button.LB).onTrue(new InstantCommand(() -> hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0)));
-
-
- // driver.get(PS5Button.CIRCLE).onTrue(
- // new InstantCommand(()->{
- // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(180)), 0);
- // })
- // ).onFalse(
- // new InstantCommand(()->{
- // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(-170)), 0);
- // })
- // );
-
- // driver.get(PS5Button.CROSS).onTrue(
- // new InstantCommand(()->{
- // if(turretJoyStickAim == null || !turretJoyStickAim.isScheduled()){
- // turretJoyStickAim = new TurretJoyStickAim(turret, this);
- // turretJoyStickAim.schedule();
- // }
- // })
- // ).onFalse(
- // new InstantCommand(()->{
- // if(turretJoyStickAim.isScheduled()){
- // turretJoyStickAim.cancel();
- // }
- // })
- // );
-
- }
-
- public void setAlignmentPose(){
- Translation2d drivepose = getDrivetrain().getPose().getTranslation();
- // uses tag #??
- int tagNumber = 17;
- Translation2d tagpose = FieldConstants.field.getTagPose(tagNumber).get().toPose2d().getTranslation();
- double YDifference = tagpose.getY() - drivepose.getY();
- double XDifference = tagpose.getX() - drivepose.getX();
- double angle = Math.atan(YDifference/XDifference);
- alignmentPose = new Pose2d(drivepose.getX(), drivepose.getY(), new Rotation2d(angle));
- System.out.println("Alignment angle: " + Units.radiansToDegrees(angle));
+ driver.get(PS5Button.SQUARE).onTrue(
+ new InstantCommand(()->{
+ if (autoShoot != null && autoShoot.isScheduled()){
+ autoShoot.cancel();
+ } else{
+ autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer);
+ CommandScheduler.getInstance().schedule(autoShoot);
+ }
+ })
+ );
+
}
@Override