diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 762013d..5ab8e24 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -133,7 +133,7 @@ public static class ArmConstants { public static final double ARM_STOW_DEGREES = -83.0; public static final double ARM_STOW_2_DEGREES = -100.0; public static final double ARM_AMP_SHOOTING_DEGREES = -27.8; - public static final double ARM_SPEAKER_SHOOTING_DEGREES = -97.822 - 5 + 4 - 6; + public static final double ARM_SPEAKER_SHOOTING_DEGREES = -97.822 - 5 + 4 - 6 -1; public static final double ARM_INTAKE_DEGREES = -109 - 1.5; public static final int LEFT_MOTOR_ID = 5; @@ -180,7 +180,7 @@ public static class IntakeShooterConstants { } public static class RobotMovementConstants { - public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(3); + public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(5); public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(1); public static final double ROTATION_PID_P = 0.21; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index af7434c..25d08b4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,8 +11,8 @@ import frc.robot.commands.CancelCommands; import frc.robot.commands.ChassisRemoteControl; import frc.robot.commands.HangControl; +import frc.robot.commands.AimAtAngle; import frc.robot.commands.ArmRotateTo; -import frc.robot.commands.AutoRotateTo; import frc.robot.commands.SetLightstripColorFor; import frc.robot.commands.SpinFlywheelShooter; import frc.robot.subsystems.ChassisDriveInputs; @@ -40,6 +40,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -142,6 +143,11 @@ public RobotContainer() { SmartDashboard.putData("Auto Chooser", autoChooser); SmartDashboard.putData("ArmUp", new ArmRotateTo(arm, ArmConstants.ARM_START_DEGREES)); + SmartDashboard.putData("ZeroYaw", new InstantCommand(drivetrain::zeroYaw)); + + SmartDashboard.putData(drivetrain); + SmartDashboard.putData(arm); + SmartDashboard.putData(intakeShooter); SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber); @@ -224,8 +230,10 @@ public void setUpDriveController() { xbox.y().onTrue(Commands.runOnce(inputs::toggleFieldRelative)); - xbox.rightBumper().onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(0), true)); - xbox.leftBumper().onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(-90), true)); + // xbox.rightBumper().onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(0), true)); + // xbox.leftBumper().onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(-90), true)); + xbox.rightBumper().whileFalse(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(0))); + xbox.leftBumper().whileFalse(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(-90))); if (vision != null) xbox.x().onTrue(Commands.runOnce(vision::toggleUsing, vision)); diff --git a/src/main/java/frc/robot/commands/AimAtAngle.java b/src/main/java/frc/robot/commands/AimAtAngle.java new file mode 100644 index 0000000..44e1dbb --- /dev/null +++ b/src/main/java/frc/robot/commands/AimAtAngle.java @@ -0,0 +1,82 @@ +package frc.robot.commands; + +import frc.robot.Constants.RobotMovementConstants; +import frc.robot.subsystems.ChassisDriveInputs; +import frc.robot.subsystems.SwerveDrivetrain; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; + +/** Command to automatically aim at a angle */ +public class AimAtAngle extends Command { + private final SwerveDrivetrain drivetrain; + private final ChassisDriveInputs chassisDriveInputs; + + private final PIDController rotatePID; + + /** + * Create a new AimAtAngle command. Tries to constants face in angle while still + * allowing driver to control robot. + * + * @param drivetrain the drivetrain of the robot + */ + public AimAtAngle(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriveInputs, Rotation2d direction) { + this.drivetrain = drivetrain; + + this.chassisDriveInputs = chassisDriveInputs; + + rotatePID = new PIDController( + RobotMovementConstants.ROTATION_PID_P, + RobotMovementConstants.ROTATION_PID_I, + RobotMovementConstants.ROTATION_PID_D); + rotatePID.enableContinuousInput(-Math.PI, Math.PI); + rotatePID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS); + rotatePID.setSetpoint(direction.getRadians()); + + addRequirements(drivetrain, chassisDriveInputs); + } + + + @Override + public void initialize() { + drivetrain.toDefaultStates(); + } + + @Override + public void execute() { + + double rotationSpeed = 0; + if (!rotatePID.atSetpoint()) { + rotationSpeed = rotatePID.calculate(drivetrain.getHeading().getRadians()); + } + + double xSpeed = 0; + double ySpeed = 0; + boolean fieldRelative = false; + if (chassisDriveInputs != null) { + xSpeed = chassisDriveInputs.getX(); + ySpeed = chassisDriveInputs.getY(); + fieldRelative = chassisDriveInputs.isFieldRelative(); + } + + ChassisSpeeds desiredSpeeds = new ChassisSpeeds(xSpeed, ySpeed, 0); + if (fieldRelative) desiredSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(desiredSpeeds, drivetrain.getHeading()); + desiredSpeeds.omegaRadiansPerSecond = rotationSpeed; + + drivetrain.setDesiredState(desiredSpeeds, false); + + drivetrain.updateSmartDashboard(); + } + + @Override + public boolean isFinished() { + return (chassisDriveInputs == null) && (rotatePID.atSetpoint()); + } + + @Override + public void end(boolean interrupted) { + drivetrain.stop(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AutoDriveTo.java b/src/main/java/frc/robot/commands/AutoDriveTo.java index f8a1884..603e078 100644 --- a/src/main/java/frc/robot/commands/AutoDriveTo.java +++ b/src/main/java/frc/robot/commands/AutoDriveTo.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.RobotMovementConstants; public class AutoDriveTo extends Command { @@ -84,8 +83,8 @@ public void execute() { @Override public boolean isFinished() { boolean isEnded = xMovePID.atSetpoint() && yMovePID.atSetpoint(); - SmartDashboard.putBoolean("xEndpoint", xMovePID.atSetpoint()); - SmartDashboard.putBoolean("yEndpoint", yMovePID.atSetpoint()); + // SmartDashboard.putBoolean("xEndpoint", xMovePID.atSetpoint()); + // SmartDashboard.putBoolean("yEndpoint", yMovePID.atSetpoint()); return isEnded; } diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index a71ecf8..9ab4fa2 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -84,7 +84,6 @@ public static Command shoot2StartingAuto(SwerveDrivetrain drivetrain, Arm arm, I new AutoDriveTo(drivetrain, new Translation2d(driveDistanceForNote1, 0))), new WaitCommand(0.25), intakeFromFloorEnd(arm, shooter), - new AutoRotateTo(drivetrain, new Rotation2d(0)), Commands.race( Commands.waitSeconds(3), new AutoDriveTo(drivetrain, new Translation2d(-driveDistanceForNote1, 0))), @@ -146,14 +145,14 @@ public static Command shootInSpeaker(SwerveDrivetrain drivetrain, Arm arm, Intak } return Commands.sequence( - Commands.parallel( - new SpinFlywheelShooterForTime(shooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_SPEAKER, 0.4), - new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES) - ), - new SpinIntakeGrabbers(shooter, IntakeShooterConstants.INTAKE_GRABBER_SPEED_SPEAKER), - new WaitCommand(0.2), - new SpinFlywheelShooter(shooter, 0), - new SpinIntakeGrabbers(shooter, 0)); + Commands.parallel( + new SpinFlywheelShooterForTime(shooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_SPEAKER, + 1.5), + new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES)), + new SpinIntakeGrabbers(shooter, IntakeShooterConstants.INTAKE_GRABBER_SPEED_SPEAKER), + new WaitCommand(0.2), + new SpinFlywheelShooter(shooter, 0), + new SpinIntakeGrabbers(shooter, 0)); } public static Command intakeFromFloorStart(Arm arm, IntakeShooter shooter) { diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java index 3c4ea95..c5deac2 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java @@ -413,6 +413,13 @@ public Rotation3d getHeading3d() { return gyro.getRotation3d(); } + /** + * Reset gyro yaw + */ + public void zeroYaw() { + gyro.zeroYaw(); + } + /** * Set amount to add to gyro position for field relative drive and * SmartDashboard display