diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 531294e..04da3d6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -11,25 +11,31 @@ public final class Constants { public static final int codriverport = 1; public static final int shooterPort = 31; + public static final int indexerPort = 21; public static final double shooterP = 0.5; public static final double shooterI = 0; public static final double shooterD = 0; - public static final int climberPort = 32; + public static final int climberPort = 10; public static final double climberP = 0.35; public static final double climberI = 0; public static final double climberD = 0; - public static final int intakePort = 35; + public static final int intakePort = 32; + public static final int intakeFrontBeambreak = 6; + public static final int intakeBackBeambreak = 5; + public static final double intakeP = 0.01; + public static final double intakeI = 0; + public static final double intakeD = 0; - public static final double armP = 21.0; + public static final double armP = 10.5; public static final double armI = 0; - public static final double armD = 0.015; + public static final double armD = 0; public static final double armS = 0; public static final double armG = 0; public static final double armV = 0; public static final double armA = 0; - public static final int armLeadPort = 21; + public static final int armLeadPort = 9; public static final int armFollowerPort = 26; public static final int armEncoderPort = 3; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 53f7633..bf92be9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,24 +12,28 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandRobot; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.autos.Centre2541; -import frc.robot.autos.Centre26541; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.autos.WompWompKieran; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Climber; import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Index; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; import java.util.HashMap; public class Robot extends CommandRobot { private final CommandXboxController m_driver = new CommandXboxController(Constants.driverport); + private final CommandXboxController m_codriver = + new CommandXboxController(Constants.codriverport); private final Arm m_arm = new Arm(); private final Shooter m_shooter = new Shooter(); private final Climber m_climber = new Climber(); private final Intake m_intake = new Intake(); + private final Index m_index = new Index(); + private final Superstructure m_superstructure = new Superstructure(m_shooter, m_intake, m_index); private static final CommandSwerveDrivetrain m_drivetrain = TunerConstants.DriveTrain; private final SwerveRequest.FieldCentric m_drive = @@ -40,6 +44,8 @@ public class Robot extends CommandRobot { private final SwerveRequest.SwerveDriveBrake m_brake = new SwerveRequest.SwerveDriveBrake(); private final Telemetry m_logger = new Telemetry(); + private final Trigger m_codriverX = m_codriver.x(); + public Robot() { HashMap aliases = new HashMap<>(); aliases.put(31, "Shooter"); @@ -59,14 +65,6 @@ public Robot() { m_scheduler.registerSubsystem(m_climber); m_scheduler.registerSubsystem(m_intake); - m_autoChooser.addOption( - "Centre2_5_4_1 Blue", new Centre2541(m_drivetrain, false).followTrajectory()); - m_autoChooser.addOption( - "Centre2_5_4_1 Red", new Centre2541(m_drivetrain, true).followTrajectory()); - m_autoChooser.addOption( - "Centre2_6_5_4_1 Red", new Centre26541(m_drivetrain, true).followTrajectory()); - m_autoChooser.addOption( - "Centre2_6_5_4_1 Blue", new Centre26541(m_drivetrain, false).followTrajectory()); m_autoChooser.addOption( "WompWompKieran Blue", new WompWompKieran(m_drivetrain, false).followTrajectory()); m_autoChooser.addOption( @@ -86,8 +84,33 @@ public Robot() { .withRotationalRate( Utils.deadzone( -m_driver.getRightX() * Constants.DrivebaseMaxAngularRate)))); + m_intake.setDefaultCommand(m_superstructure.intake()); + m_shooter.setDefaultCommand(m_shooter.stop()); + m_index.setDefaultCommand(m_index.stop()); m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake)); - m_driver.x().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative())); + m_driver.y().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative())); + + m_codriver.a().onTrue(m_climber.climb()); + m_codriver + .rightTrigger() + .onTrue(m_arm.goToSetpoint(Arm.Setpoint.kAmp).andThen(m_intake.outake(8))) + .onFalse(m_arm.goToSetpoint(Arm.Setpoint.kStowed)); + m_codriver + .leftTrigger() + .onTrue(m_arm.goToSetpoint(Arm.Setpoint.kSpeaker).andThen(m_shooter.shoot())) + .onFalse(m_arm.goToSetpoint(Arm.Setpoint.kStowed)); + + m_codriverX.whileTrue(m_intake.spinup(20)).onFalse(m_intake.stop()); + m_scheduler + .getDefaultButtonLoop() + .bind( + () -> { + if (m_codriver.getHID().getYButton()) { + m_intake.removeDefaultCommand(); + } + }); + m_codriver.y().onTrue(m_superstructure.stop()); + m_codriver.b().whileTrue(m_superstructure.outake()); } } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java new file mode 100644 index 0000000..92d0751 --- /dev/null +++ b/src/main/java/frc/robot/Superstructure.java @@ -0,0 +1,44 @@ +// Copyright (c) 2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +package frc.robot; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.Index; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Shooter; + +public class Superstructure { + private final Shooter m_shooter; + private final Intake m_intake; + private final Index m_index; + + public Superstructure(Shooter shooter, Intake intake, Index index) { + m_shooter = shooter; + m_intake = intake; + m_index = index; + } + + public Command intake() { + var lol = + m_index.m_intaking.onTrue( + Commands.parallel( + m_intake.intake(3).until(m_index.m_hasNote).andThen(m_intake.stop()), + m_index.intake(-3).until(m_index.m_hasNote).andThen(m_index.stop()))); + return m_intake.intake(8).until(lol); + } + + public Command shooter() { + return m_shooter.spinup(500).andThen(Commands.parallel(m_shooter.maintain(), m_index.shoot())); + } + + public Command stop() { + return Commands.parallel(m_shooter.stop(), m_intake.stop(), m_index.stop()); + } + + public Command outake() { + return Commands.parallel(m_intake.outake(-8), m_index.outake(8)); + } +} diff --git a/src/main/java/frc/robot/autos/OneNote.java b/src/main/java/frc/robot/autos/OneNote.java index e66ee65..4f399b1 100644 --- a/src/main/java/frc/robot/autos/OneNote.java +++ b/src/main/java/frc/robot/autos/OneNote.java @@ -32,6 +32,6 @@ public void configureBindings() { .onTrue( m_shooter .spinup(300) - .andThen(Commands.parallel(m_intake.pass(), m_shooter.maintain()))); + .andThen(Commands.parallel(m_intake.spinup(200), m_shooter.maintain()))); } } diff --git a/src/main/java/frc/robot/autos/TwoNote.java b/src/main/java/frc/robot/autos/TwoNote.java index d9474c9..1402eb5 100644 --- a/src/main/java/frc/robot/autos/TwoNote.java +++ b/src/main/java/frc/robot/autos/TwoNote.java @@ -8,19 +8,20 @@ import com.choreo.lib.Choreo; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.Intake; +// import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; public class TwoNote implements Auto { Shooter m_shooter; - Intake m_intake; + // Intake m_intake; Command m_pathfollower; public TwoNote( - CommandSwerveDrivetrain drivetrain, Shooter shooter, Intake intake, boolean isRed) { + // CommandSwerveDrivetrain drivetrain, Shooter shooter, Intake intake, boolean isRed) { + CommandSwerveDrivetrain drivetrain, Shooter shooter, boolean isRed) { m_shooter = shooter; - m_intake = intake; + // m_intake = intake; m_pathfollower = drivetrain.followTrajectory("example-two-note", isRed); } @@ -42,7 +43,7 @@ private Command shoot() { @Override public void configureBindings() { Choreo.event("first-shot").onTrue(shoot()); - Choreo.event("intake").onTrue(m_intake.intake()); + // Choreo.event("intake").onTrue(m_intake.spinUntilBeamBreak(200)); Choreo.event("second-shot").onTrue(shoot()); } } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 99e4304..b4e0f4d 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -8,13 +8,15 @@ import com.revrobotics.CANSparkMax; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; @@ -50,16 +52,25 @@ public enum Setpoint { private final DoubleLogEntry log_ff_output = new DoubleLogEntry(m_log, "/arm/ff/output"); private final StringLogEntry log_setpoint = new StringLogEntry(m_log, "/arm/setpoint"); + private final NetworkTable driveStats = NetworkTableInstance.getDefault().getTable("Arm"); + private final DoublePublisher output = driveStats.getDoubleTopic("Output").publish(); + private final DoublePublisher ff_output = driveStats.getDoubleTopic("FF Output").publish(); + private final DoublePublisher pid_output = driveStats.getDoubleTopic("PID Output").publish(); + private final DoublePublisher pid_error = driveStats.getDoubleTopic("PID Error").publish(); + private final DoublePublisher pid_setpoint = driveStats.getDoubleTopic("PID Setpoint").publish(); + private final DoublePublisher angle = driveStats.getDoubleTopic("Angle").publish(); public final Trigger m_atSetpoint = new Trigger(m_pid::atSetpoint); /** Creates a new {@link Arm} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */ public Arm() { m_followerMotor.follow(m_primaryMotor); + m_encoder.reset(); + m_encoder.setPositionOffset(0.1); } /** Achieves and maintains speed for the primary motor. */ private Command achievePosition(double position) { - return Commands.run( + return run( () -> { var pid_output = m_pid.calculate(m_encoder.getAbsolutePosition() * 2 * Math.PI, position); log_pid_output.append(pid_output); @@ -68,7 +79,12 @@ private Command achievePosition(double position) { log_ff_output.append(ff_output); log_ff_position_setpoint.append(position); log_ff_velocity_setpoint.append((5676 / 250)); - m_primaryMotor.setVoltage(-1 * ff_output + pid_output); + output.set(ff_output + pid_output); + this.ff_output.set(ff_output); + this.pid_output.set(0.5 + pid_output); + this.pid_error.set(m_pid.getPositionError()); + this.pid_setpoint.set(m_pid.getSetpoint()); + m_primaryMotor.setVoltage(0.5 + pid_output * -1); }); } @@ -83,11 +99,7 @@ public Command stop() { * @return a {@link Command} to get to the desired position. */ private Command moveToPosition(double position) { - return achievePosition(position) - .until( - () -> - m_pid.atSetpoint() - && ((m_encoder.getAbsolutePosition() * 2 * Math.PI) - position) < 0.001); + return achievePosition(position).until(m_pid::atSetpoint); } /** @@ -106,7 +118,11 @@ public Command maintain() { * @return A {@link Command} to control the arm manually. */ public Command manualControl(DoubleSupplier speed) { - return Commands.run(() -> m_primaryMotor.set(speed.getAsDouble()), this); + return run( + () -> { + output.set(speed.getAsDouble()); + m_primaryMotor.set(speed.getAsDouble()); + }); } /* @@ -121,12 +137,17 @@ public Command goToSetpoint(Setpoint setpoint) { log_setpoint.append(setpoint.name()); switch (setpoint) { - case kAmp -> position = 5.34; - case kIntake -> position = 3.7; - case kSpeaker -> position = 3.7; - case kStowed -> position = 3.7; + case kAmp -> position = Math.PI / 2 + 0.1; + case kIntake -> position = 0; + case kSpeaker -> position = 0.2; + case kStowed -> position = 0; } return moveToPosition(position); } + + @Override + public void periodic() { + angle.set(m_encoder.getAbsolutePosition() * 2 * Math.PI); + } } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index c9eac09..6c6464d 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -12,7 +12,6 @@ import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -36,7 +35,7 @@ public Climber() {} * @return A {@link Command} to climb the robot. */ public Command climb() { - return Commands.run( + return run( () -> { var output = m_pid.calculate(Units.rotationsToRadians(m_encoder.getPosition() * -1), -Math.PI); diff --git a/src/main/java/frc/robot/subsystems/Index.java b/src/main/java/frc/robot/subsystems/Index.java new file mode 100644 index 0000000..2c1c9b3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Index.java @@ -0,0 +1,53 @@ +// Copyright (c) 2024 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +package frc.robot.subsystems; + +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.networktables.BooleanPublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants; + +public class Index extends SubsystemBase { + private final CANSparkMax m_motor = new CANSparkMax(Constants.indexerPort, MotorType.kBrushless); + private final DigitalInput frontBeambreak = new DigitalInput(Constants.intakeFrontBeambreak); + private final DigitalInput backBeamBreak = new DigitalInput(Constants.intakeBackBeambreak); + + public final Trigger m_hasNote = new Trigger(backBeamBreak::get); + public final Trigger m_intaking = new Trigger(frontBeambreak::get); + + private final NetworkTable driveStats = NetworkTableInstance.getDefault().getTable("Index"); + private final BooleanPublisher m_ntHasNote = driveStats.getBooleanTopic("Has Note").publish(); + private final BooleanPublisher m_ntIntaking = driveStats.getBooleanTopic("Intaking").publish(); + + public Index() {} + + public Command shoot() { + return run(() -> m_motor.setVoltage(-8)).until(m_hasNote.negate()); + } + + public Command intake(double voltage) { + return run(() -> m_motor.setVoltage(voltage)).until(m_hasNote); + } + + public Command outake(double voltage) { + return run(() -> m_motor.setVoltage(voltage)); + } + + public Command stop() { + return runOnce(() -> m_motor.stopMotor()); + } + + @Override + public void periodic() { + m_ntHasNote.set(m_hasNote.getAsBoolean()); + m_ntIntaking.set(m_intaking.getAsBoolean()); + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 07c4353..4e2d245 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -6,46 +6,42 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import edu.wpi.first.util.datalog.DoubleLogEntry; -import edu.wpi.first.wpilibj.DataLogManager; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; public class Intake extends SubsystemBase { private final CANSparkMax m_motor = new CANSparkMax(Constants.intakePort, MotorType.kBrushless); + private final RelativeEncoder m_encoder = m_motor.getEncoder(); - private final DoubleLogEntry log_output = - new DoubleLogEntry(DataLogManager.getLog(), "/intake/output"); + private final PIDController m_pid = + new PIDController(Constants.intakeP, Constants.intakeI, Constants.intakeD); public Intake() {} - public Command intake() { - return Commands.run( - () -> { - log_output.append(4); - m_motor.setVoltage(4); - }) - .withTimeout(2) - .andThen(runOnce(() -> m_motor.setVoltage(0))); + public Command achieveSpeed(double speed) { + return run( + () -> + m_motor.setVoltage( + m_pid.calculate(Units.rotationsToRadians(m_encoder.getVelocity()), speed))); + } + + public Command spinup(double speed) { + return achieveSpeed(speed).until(m_pid::atSetpoint); } public Command stop() { - return runOnce(() -> m_motor.set(0)); + return run(() -> m_motor.stopMotor()); } - public Command outake() { - return Commands.run( - () -> { - log_output.append(-4); - m_motor.setVoltage(-4); - }) - .withTimeout(4) - .andThen(runOnce(() -> m_motor.setVoltage(0))); + public Command outake(double voltage) { + return run(() -> m_motor.setVoltage(voltage)); } - public Command pass() { - return intake(); + public Command intake(double voltage) { + return run(() -> m_motor.setVoltage(voltage)); } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 3d21858..c04af19 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -13,23 +13,22 @@ import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; /** Our Crescendo shooter Subsystem */ public class Shooter extends SubsystemBase { - private final PIDController m_pid = - new PIDController(Constants.shooterP, Constants.shooterI, Constants.shooterD); private final CANSparkMax m_motor = new CANSparkMax(Constants.shooterPort, MotorType.kBrushless); private final RelativeEncoder m_encoder = m_motor.getEncoder(); + private final PIDController m_pid = + new PIDController(Constants.shooterP, Constants.shooterI, Constants.shooterD); + private final DataLog m_log = DataLogManager.getLog(); private final DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/shooter/pid/output"); public final Trigger m_atSetpoint = new Trigger(m_pid::atSetpoint); - ; /** Creates a new {@link Shooter} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */ public Shooter() {} @@ -38,7 +37,7 @@ public Shooter() {} private Command achieveSpeeds(double speed) { m_pid.reset(); m_pid.setSetpoint(speed); - return Commands.run( + return run( () -> { var output = m_pid.calculate( @@ -70,4 +69,8 @@ public Command stop() { public Command maintain() { return achieveSpeeds(m_pid.getSetpoint()); } + + public Command shoot() { + return spinup(500).andThen(maintain()); + } }