diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 07a734a5..cf56b477 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -21,10 +21,12 @@ import frc.team2412.robot.commands.intake.FeederInCommand; import frc.team2412.robot.commands.intake.IntakeRejectCommand; import frc.team2412.robot.commands.launcher.FullTargetCommand; +import frc.team2412.robot.commands.launcher.ManualAngleCommand; import frc.team2412.robot.commands.launcher.SetAngleAmpLaunchCommand; -import frc.team2412.robot.commands.launcher.SetAngleCommand; import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; +import frc.team2412.robot.commands.launcher.SetPivotCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; +import frc.team2412.robot.util.TrapAlign; public class Controls { public static class ControlConstants { @@ -47,8 +49,9 @@ public static class ControlConstants { // Launcher private final Trigger launcherAmpPresetButton; private final Trigger launcherSubwooferPresetButton; + private final Trigger launcherLowerPresetButton; // private final Trigger launcherPodiumPresetButton; - // private final Trigger launcherTrapPresetButton; + private final Trigger launcherTrapPresetButton; private final Trigger launcherLaunchButton; private final Subsystems s; @@ -60,9 +63,10 @@ public Controls(Subsystems s) { launcherAmpPresetButton = codriveController.x(); launcherSubwooferPresetButton = codriveController.a(); + launcherLowerPresetButton = codriveController.y(); // launcherPodiumPresetButton = codriveController.povLeft(); - // launcherTrapPresetButton = codriveController.y(); - launcherLaunchButton = codriveController.leftBumper(); + launcherTrapPresetButton = codriveController.rightTrigger(); + launcherLaunchButton = codriveController.rightBumper(); // intake controls (confirmed with driveteam) driveIntakeInButton = driveController.a(); driveIntakeStopButton = driveController.b(); @@ -114,7 +118,7 @@ private void bindDrivebaseControls() { () -> Rotation2d.fromRotations(driveController.getRightX()))); driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro)); driveController.rightStick().onTrue(new InstantCommand(s.drivebaseSubsystem::toggleXWheels)); - // driveController + // driveController x // .back() // .onTrue( // new InstantCommand( @@ -145,12 +149,14 @@ private void bindLauncherControls() { CommandScheduler.getInstance() .setDefaultCommand( s.launcherSubsystem, - new SetAngleCommand( + new ManualAngleCommand( s.launcherSubsystem, () -> MathUtil.applyDeadband(codriveController.getLeftY(), 0.1) * LauncherSubsystem.ANGLE_MAX_SPEED)); + launcherLowerPresetButton.onTrue( + new SetPivotCommand(s.launcherSubsystem, LauncherSubsystem.RETRACTED_ANGLE)); launcherSubwooferPresetButton.onTrue( new SetAngleLaunchCommand( s.launcherSubsystem, @@ -166,8 +172,8 @@ private void bindLauncherControls() { s.launcherSubsystem, LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, LauncherSubsystem.AMP_AIM_ANGLE)); - // launcherTrapPresetButton.onTrue( - // TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem)); + launcherTrapPresetButton.onTrue( + TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem)); codriveController.b().whileTrue(s.launcherSubsystem.run(s.launcherSubsystem::stopLauncher)); diff --git a/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java b/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java index eab92cfb..fe254e5f 100644 --- a/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java +++ b/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java @@ -2,8 +2,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.team2412.robot.commands.launcher.SetAngleCommand; -import frc.team2412.robot.commands.launcher.SetLaunchSpeedCommand; +import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; import frc.team2412.robot.commands.launcher.StopLauncherCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; @@ -15,12 +14,11 @@ public LauncherDiagnosticCommand(LauncherSubsystem launcherSubsystem) { this.launcherSubsystem = launcherSubsystem; this.Angle = launcherSubsystem.getAngle(); addCommands( - new SetAngleCommand(launcherSubsystem, () -> 45), + new SetAngleLaunchCommand(launcherSubsystem, 500, Angle + 10), new WaitCommand(2), - new SetAngleCommand(launcherSubsystem, () -> 90), + new SetAngleLaunchCommand(launcherSubsystem, 500, Angle + 20), new WaitCommand(1), - new SetAngleCommand(launcherSubsystem, () -> Angle), - new SetLaunchSpeedCommand(launcherSubsystem, 100), + new SetAngleLaunchCommand(launcherSubsystem, 500, Angle), new WaitCommand(5), new StopLauncherCommand(launcherSubsystem)); } diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java similarity index 59% rename from src/main/java/frc/team2412/robot/commands/launcher/SetAngleCommand.java rename to src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java index 37ea476d..d7b04974 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java @@ -5,21 +5,19 @@ import java.util.function.DoubleSupplier; // this command adjusts the angle using a speed that is input by the user -public class SetAngleCommand extends Command { +public class ManualAngleCommand extends Command { private final LauncherSubsystem launcherSubsystem; - private final DoubleSupplier launcherAngleSpeed; + private final DoubleSupplier launcherAngle; - public SetAngleCommand(LauncherSubsystem launcherSubsystem, DoubleSupplier angleSpeed) { + public ManualAngleCommand(LauncherSubsystem launcherSubsystem, DoubleSupplier angleSpeed) { this.launcherSubsystem = launcherSubsystem; - this.launcherAngleSpeed = angleSpeed; + this.launcherAngle = angleSpeed; addRequirements(launcherSubsystem); } @Override public void execute() { - if (launcherAngleSpeed.getAsDouble() != 0.0) { - launcherSubsystem.setAngleSpeed(launcherAngleSpeed.getAsDouble()); - } + launcherSubsystem.setAngleManual(launcherAngle.getAsDouble()); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleAmpLaunchCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SetAngleAmpLaunchCommand.java index e6e740ca..b081498b 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleAmpLaunchCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/SetAngleAmpLaunchCommand.java @@ -22,6 +22,7 @@ public SetAngleAmpLaunchCommand(LauncherSubsystem launcherSubsystem, double spee public void initialize() { launcherSubsystem.setAngle(launcherAngle); launcherSubsystem.ampLaunch(launcherSpeed); + launcherSubsystem.manualSetpoint(launcherAngle); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java new file mode 100644 index 00000000..482a2111 --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/launcher/SetPivotCommand.java @@ -0,0 +1,26 @@ +package frc.team2412.robot.commands.launcher; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.subsystems.LauncherSubsystem; + +public class SetPivotCommand extends Command { + private final LauncherSubsystem launcherSubsystem; + private final double angle; + + public SetPivotCommand(LauncherSubsystem launcherSubsystem, double angle) { + this.launcherSubsystem = launcherSubsystem; + this.angle = angle; + addRequirements(launcherSubsystem); + } + + @Override + public void initialize() { + launcherSubsystem.setAngle(angle); + } + + @Override + public boolean isFinished() { + return MathUtil.isNear(angle, launcherSubsystem.getAngle(), LauncherSubsystem.ANGLE_TOLERANCE); + } +} diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 886b5d14..f3c2cdce 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -166,9 +166,13 @@ private void resetMotors() { configureMotor(intakeMotorRight, true); configureMotor(ingestMotor, false); - configureMotor(indexMotorUpper, 40, true); + configureMotor(indexMotorUpper, 40, false); - configureMotor(feederMotor, 40, true); + configureMotor(feederMotor, 40, false); + indexMotorUpper + .getForwardLimitSwitch(com.revrobotics.SparkLimitSwitch.Type.kNormallyOpen) + .enableLimitSwitch(false); + indexMotorUpper.burnFlash(); } public void intakeSet(double speed) { diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index d0af5720..524568eb 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -10,6 +10,7 @@ import com.revrobotics.SparkAbsoluteEncoder.Type; import com.revrobotics.SparkPIDController; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; @@ -36,7 +37,12 @@ public class LauncherSubsystem extends SubsystemBase { public static final int AMP_AIM_ANGLE = 335; public static final int SUBWOOFER_AIM_ANGLE = 298; public static final int PODIUM_AIM_ANGLE = 39; - public static final int TRAP_AIM_ANGLE = 290; + public static final int TRAP_AIM_ANGLE = 317; + public static final double MANUAL_MODIFIER = 0.02; + public static final double RETRACTED_ANGLE = 255; + // offset for FF so parallel to floor is 0 + public static final double FF_PIVOT_OFFSET = 250; + // MOTOR VALUES // max Free Speed: 6784 RPM private static final int MAX_FREE_SPEED_RPM = 6784; @@ -59,12 +65,12 @@ public class LauncherSubsystem extends SubsystemBase { private final RelativeEncoder launcherBottomEncoder; private final SparkAbsoluteEncoder launcherAngleEncoder; private final SparkPIDController launcherAngleOnePIDController; - + // private final SparkPIDController launcherAngleTwoPIDController; + private final ArmFeedforward launcherPivotFF = new ArmFeedforward(0.40434, 0.096771, 0.0056403); // arm FF values: // Ks: 0.40434 // Kv: 0.096771 // Ka: 0.0056403 - private final SparkPIDController launcherTopPIDController; private final SparkPIDController launcherBottomPIDController; private final SimpleMotorFeedforward launcherTopFeedforward = @@ -74,6 +80,7 @@ public class LauncherSubsystem extends SubsystemBase { private double rpmSetpoint; private double angleSetpoint; + private double manualAngleSetpoint; private GenericEntry setLauncherSpeedEntry; @@ -89,6 +96,8 @@ public class LauncherSubsystem extends SubsystemBase { private GenericEntry launcherIsAtSpeed; + private GenericEntry launcherAngleManual; + // Constructors public LauncherSubsystem() { @@ -104,6 +113,7 @@ public LauncherSubsystem() { launcherTopEncoder = launcherTopMotor.getEncoder(); launcherBottomEncoder = launcherBottomMotor.getEncoder(); launcherAngleEncoder = launcherAngleOneMotor.getAbsoluteEncoder(Type.kDutyCycle); + manualAngleSetpoint = launcherAngleEncoder.getPosition(); // PID controllers // Create launcherTopPIDController and launcherTopMotor] @@ -140,8 +150,8 @@ public void configMotors() { launcherAngleOneMotor.setSmartCurrentLimit(60); launcherAngleTwoMotor.setSmartCurrentLimit(60); - launcherAngleOneMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, 0.95f); - launcherAngleOneMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, 0.71f); + launcherAngleOneMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kForward, 0.96f); + launcherAngleOneMotor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, 0.80f); launcherAngleOneMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); launcherAngleOneMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); @@ -152,7 +162,6 @@ public void configMotors() { launcherAngleOnePIDController.setI(0); launcherAngleOnePIDController.setD(0.066248); launcherAngleOnePIDController.setOutputRange(-ANGLE_MAX_SPEED, ANGLE_MAX_SPEED); - launcherTopPIDController.setP(7.7633E-05); launcherTopPIDController.setI(0); launcherTopPIDController.setD(0); @@ -209,7 +218,11 @@ public double getAngle() { public void setAngle(double launcherAngle) { angleSetpoint = launcherAngle; launcherAngleOnePIDController.setReference( - Units.degreesToRotations(angleSetpoint), ControlType.kPosition); + Units.degreesToRotations(angleSetpoint), + ControlType.kPosition, + 0, + launcherPivotFF.calculate(Units.degreesToRadians(launcherAngle - FF_PIVOT_OFFSET), 0)); + manualAngleSetpoint = launcherAngle; // launcherAngleTwoPIDController.setReference( // Units.degreesToRotations(angleSetpoint), ControlType.kPosition); } @@ -234,10 +247,25 @@ public double getAngleSpeed() { return launcherAngleEncoder.getVelocity(); } - public void setAngleSpeed(double Speed) { - // launcherAngleOnePIDController.setReference(Speed, ControlType.kVelocity); - // launcherAngleTwoPIDController.setReference(Speed, ControlType.kVelocity); - launcherAngleOneMotor.set(Speed); + public void setAngleManual(double joystickInput) { + manualAngleSetpoint = + MathUtil.clamp(manualAngleSetpoint + joystickInput * MANUAL_MODIFIER, 0.80f, 0.96f); + + if (Units.degreesToRotations(getAngle()) > 0.80 + && Units.degreesToRotations(getAngle()) < 0.96) { + launcherAngleOnePIDController.setReference( + manualAngleSetpoint, + ControlType.kPosition, + 0, + launcherPivotFF.calculate( + Units.degreesToRadians( + Units.rotationsToDegrees(manualAngleSetpoint) - FF_PIVOT_OFFSET), + 0)); + } + } + + public void manualSetpoint(double setpoint) { + manualAngleSetpoint = setpoint; } private void initShuffleboard() { @@ -292,6 +320,12 @@ private void initShuffleboard() { .withProperties(Map.of("Min", -MAX_FREE_SPEED_RPM, "Max", MAX_FREE_SPEED_RPM)) .withPosition(5, 0) .getEntry(); + launcherAngleManual = + Shuffleboard.getTab("Launcher") + .add("Launcher manual increase", 0) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); Shuffleboard.getTab("Launcher") .add(new SparkPIDWidget(launcherAngleOnePIDController, "launcherAnglePID")) .withPosition(2, 0); @@ -313,6 +347,7 @@ public void periodic() { launcherTopFlywheelTemp.setDouble(launcherTopMotor.getMotorTemperature()); launcherBottomFlyWheelTemp.setDouble(launcherTopMotor.getMotorTemperature()); launcherIsAtSpeed.setBoolean(isAtSpeed()); + launcherAngleManual.setDouble(manualAngleSetpoint); } private SysIdRoutine getArmSysIdRoutine() { diff --git a/src/main/java/frc/team2412/robot/util/TrapAlign.java b/src/main/java/frc/team2412/robot/util/TrapAlign.java index 167fa8d9..5b921fba 100644 --- a/src/main/java/frc/team2412/robot/util/TrapAlign.java +++ b/src/main/java/frc/team2412/robot/util/TrapAlign.java @@ -21,29 +21,35 @@ public class TrapAlign { // trap that faces source new Pose2d(new Translation2d(4.3, 3.09), Rotation2d.fromDegrees(-300)), // trap that faces mid - new Pose2d(new Translation2d(6, 4), Rotation2d.fromDegrees(180)) + // DO THIS ONE FIRST + // brute force the X lol + new Pose2d(new Translation2d(5.8, 4.10), Rotation2d.fromDegrees(180)) }; private static final Pose2d[] RED_TRAP_POSES = { // trap that faces amp - new Pose2d(new Translation2d(12.3, 5.14), Rotation2d.fromDegrees(-60)), + new Pose2d(new Translation2d(12.3, 5.14), Rotation2d.fromDegrees(-120)), // trap that faces source - new Pose2d(new Translation2d(12.3, 3.09), Rotation2d.fromDegrees(120)), + new Pose2d(new Translation2d(12.3, 3.09), Rotation2d.fromDegrees(300)), // trap that faces mid - new Pose2d(new Translation2d(10, 4), Rotation2d.fromDegrees(0)) + // DO THIS ONE FIRST + // brute force the X lol + new Pose2d(new Translation2d(10.8, 4.10), Rotation2d.fromDegrees(0)) }; private static Command trapAlign(DrivebaseSubsystem drivebaseSubsystem) { Pose2d robotPose = drivebaseSubsystem.getPose(); - Pose2d trapPose = - robotPose.nearest( - List.of( - (DriverStation.getAlliance().get().equals(Alliance.Blue)) - ? BLUE_TRAP_POSES - : RED_TRAP_POSES)); - + boolean isBlue; + if (!DriverStation.getAlliance().isEmpty()) { + isBlue = DriverStation.getAlliance().get().equals(Alliance.Blue); + } else { + isBlue = false; + } + // figures out which trap to go to + Pose2d trapPose = robotPose.nearest(List.of((isBlue) ? BLUE_TRAP_POSES : RED_TRAP_POSES)); + // sets the point for the path to go to List bezierPoints = PathPlannerPath.bezierFromPoses(robotPose, trapPose); - + // this is flipped PathPlannerPath path = new PathPlannerPath( bezierPoints, @@ -53,6 +59,11 @@ private static Command trapAlign(DrivebaseSubsystem drivebaseSubsystem) { DrivebaseSubsystem.MAX_ANGULAR_VELOCITY, DrivebaseSubsystem.MAX_ANGULAR_ACCELERAITON), new GoalEndState(0.0, trapPose.getRotation())); + // path.flipPath(); Returns path except it's flipped + // this unflips it + if (!isBlue) { + path = path.flipPath(); + } return AutoBuilder.followPath(path); } @@ -79,6 +90,7 @@ public void initialize() { trapCommand.initialize(); launcherSubsystem.setAngle(LauncherSubsystem.TRAP_AIM_ANGLE); launcherSubsystem.launch(LauncherSubsystem.TRAP_SHOOT_SPEED_RPM); + launcherSubsystem.manualSetpoint(LauncherSubsystem.TRAP_AIM_ANGLE); } @Override