From 74e99d90d018cc23fbd2afbdd96f20f0605f9116 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Sat, 10 Aug 2024 12:22:34 +0800 Subject: [PATCH] More temprorary teleop fixes from testing --- src/main/java/frc/robot/Robot.java | 15 ++++++-------- src/main/java/frc/robot/Superstructure.java | 18 +++++++++-------- src/main/java/frc/robot/subsystems/Arm.java | 18 ++++++++--------- src/main/java/frc/robot/subsystems/Index.java | 11 +++++----- .../java/frc/robot/subsystems/Shooter.java | 20 +++++++++++++++++-- 5 files changed, 49 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index eaad93c..975edfa 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -66,6 +66,7 @@ public Robot() { m_scheduler.registerSubsystem(m_shooter); m_scheduler.registerSubsystem(m_climber); m_scheduler.registerSubsystem(m_intake); + m_scheduler.registerSubsystem(m_index); m_autoChooser.addOption( "WompWompKieran Blue", new WompWompKieran(m_drivetrain, false).followTrajectory()); @@ -89,21 +90,17 @@ public Robot() { m_intake.setDefaultCommand(m_superstructure.intake()); m_shooter.setDefaultCommand(m_shooter.stop()); m_index.setDefaultCommand(m_index.stop()); + // m_arm.setDefaultCommand(m_arm.goToSetpoint(Arm.Setpoint.kStowed)); + m_arm.setDefaultCommand(m_arm.manualControl(m_codriver::getLeftY)); m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake)); 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_codriver.rightBumper().whileTrue(m_intake.outake(8)); + m_codriver.leftBumper().whileTrue(m_superstructure.shoot()); - m_codriverX.whileTrue(m_intake.spinup(20)).onFalse(m_intake.stop()); + m_codriverX.whileTrue(m_superstructure.intake()).onFalse(m_intake.stop()); m_scheduler .getDefaultButtonLoop() .bind( diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 92d0751..8b0a025 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -22,16 +22,18 @@ public Superstructure(Shooter shooter, Intake intake, 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); + m_index.m_intaking.onTrue( + Commands.parallel( + m_intake.intake(5).until(m_index.m_hasNote).andThen(m_intake.stop()), + m_index.intake(-5).until(m_index.m_hasNote).andThen(m_index.stop()))); + return m_intake.intake(8).until(m_index.m_intaking); } - public Command shooter() { - return m_shooter.spinup(500).andThen(Commands.parallel(m_shooter.maintain(), m_index.shoot())); + public Command shoot() { + return m_shooter + .spinup(500) + .withTimeout(3) + .andThen(Commands.parallel(m_shooter.maintain(), m_index.shoot())); } public Command stop() { diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index b4e0f4d..ff8edb6 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -54,10 +54,10 @@ public enum 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 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); @@ -79,12 +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)); - 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); + // m_primaryMotor.setVoltage(0.5 + pid_output); }); } @@ -137,10 +137,10 @@ public Command goToSetpoint(Setpoint setpoint) { log_setpoint.append(setpoint.name()); switch (setpoint) { - case kAmp -> position = Math.PI / 2 + 0.1; - case kIntake -> position = 0; - case kSpeaker -> position = 0.2; - case kStowed -> position = 0; + case kAmp -> position = Math.PI / 2 + 4.8; + case kIntake -> position = 4.85; + case kSpeaker -> position = 4.85; + case kStowed -> position = 4.85; } return moveToPosition(position); diff --git a/src/main/java/frc/robot/subsystems/Index.java b/src/main/java/frc/robot/subsystems/Index.java index 2c1c9b3..59748d0 100644 --- a/src/main/java/frc/robot/subsystems/Index.java +++ b/src/main/java/frc/robot/subsystems/Index.java @@ -21,16 +21,17 @@ public class Index extends SubsystemBase { 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); + public final Trigger m_intaking = new Trigger(frontBeambreak::get).negate(); - 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(); + private final NetworkTable indexStats = NetworkTableInstance.getDefault().getTable("Index"); + private final BooleanPublisher m_ntHasNote = indexStats.getBooleanTopic("Has Note").publish(); + private final BooleanPublisher m_ntIntaking = indexStats.getBooleanTopic("Intaking").publish(); public Index() {} public Command shoot() { - return run(() -> m_motor.setVoltage(-8)).until(m_hasNote.negate()); + // return run(() -> m_motor.setVoltage(-8)).until(m_hasNote.negate()); + return run(() -> m_motor.setVoltage(-8)); } public Command intake(double voltage) { diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index c04af19..3a570c6 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -9,6 +9,9 @@ import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; +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.wpilibj.DataLogManager; @@ -27,15 +30,21 @@ public class Shooter extends SubsystemBase { private final DataLog m_log = DataLogManager.getLog(); private final DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/shooter/pid/output"); + private final NetworkTable shooterStats = NetworkTableInstance.getDefault().getTable("Shooter"); + private final DoublePublisher m_ntPidError = shooterStats.getDoubleTopic("PID/Error").publish(); + private final DoublePublisher m_ntPidOutput = shooterStats.getDoubleTopic("PID/Output").publish(); + private final DoublePublisher m_ntRotationalVelocity = + shooterStats.getDoubleTopic("RotationalVelocity").publish(); public final Trigger m_atSetpoint = new Trigger(m_pid::atSetpoint); /** Creates a new {@link Shooter} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */ - public Shooter() {} + public Shooter() { + m_pid.setTolerance(0.05, 0.05); + } /** Acheives and maintains speed. */ private Command achieveSpeeds(double speed) { - m_pid.reset(); m_pid.setSetpoint(speed); return run( () -> { @@ -43,6 +52,8 @@ private Command achieveSpeeds(double speed) { m_pid.calculate( -1 * Units.rotationsPerMinuteToRadiansPerSecond(m_encoder.getVelocity())); log_pid_output.append(output); + m_ntPidError.set(m_pid.getVelocityError()); + m_ntPidOutput.set(m_pid.getVelocityError()); m_motor.setVoltage(output); }); } @@ -73,4 +84,9 @@ public Command maintain() { public Command shoot() { return spinup(500).andThen(maintain()); } + + @Override + public void periodic() { + m_ntRotationalVelocity.set(m_encoder.getVelocity()); + } }