From 76240af4cdff03ca7e2766a280f82274ef57db54 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Thu, 7 Mar 2024 20:38:41 -0500 Subject: [PATCH] the issue is our ToF sensor --- src/main/java/frc/robot/Robot.java | 24 ++--- src/main/java/frc/robot/RobotContainer.java | 93 ++++++++++--------- .../robot/subsystems/shooter/ShooterIO.java | 1 + .../subsystems/shooter/ShooterIOKraken.java | 9 +- .../subsystems/shooter/ShooterSubsystem.java | 2 +- 5 files changed, 70 insertions(+), 59 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cc114c87..4c636812 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -48,11 +48,11 @@ public void robotInit() { String logPath = "/media/sda1/aoide"; - DataLogManager.start(logPath); - DriverStation.startDataLog(DataLogManager.getLog()); +// DataLogManager.start(logPath); +// DriverStation.startDataLog(DataLogManager.getLog()); - SignalLogger.setPath(logPath); - SignalLogger.start(); +// SignalLogger.setPath(logPath); +// SignalLogger.start(); // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. @@ -60,14 +60,14 @@ public void robotInit() { pdp = new PowerDistribution(); - final StringLogEntry entry = new StringLogEntry(DataLogManager.getLog(), "/ntlog"); - NetworkTableInstance.getDefault() - .addLogger( - 0, - 100, - event -> - entry.append( - event.logMessage.filename + ":" + event.logMessage.line + ":" + event.logMessage.message)); +// final StringLogEntry entry = new StringLogEntry(DataLogManager.getLog(), "/ntlog"); +// NetworkTableInstance.getDefault() +// .addLogger( +// 0, +// 100, +// event -> +// entry.append( +// event.logMessage.filename + ":" + event.logMessage.line + ":" + event.logMessage.message)); } /** This function is called periodically during all modes. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 22eaf5ad..dd74e89b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -59,9 +59,9 @@ public class RobotContainer { // Subsystems private final DriveSubsystem m_driveSubsystem; -// private final ShooterSubsystem m_shooter; -// public final ArmSubsystem m_armSubsystem; -// private final ClimberSubsystem m_climber; + private final ShooterSubsystem m_shooter; + public final ArmSubsystem m_armSubsystem; + private final ClimberSubsystem m_climber; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -81,13 +81,14 @@ public RobotContainer() { new ModuleIOTalonFX(Constants.DriveConstants.FL_MOD_CONSTANTS), new ModuleIOTalonFX(Constants.DriveConstants.FR_MOD_CONSTANTS), new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS), - new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS), - new VisionSubsystem[]{ - new VisionSubsystem("RightCamera", DriveConstants.RIGHT_CAMERA_TRANSFORMATION) - }); -// m_shooter = new ShooterSubsystem(new ShooterIOKraken()); -// m_armSubsystem = new ArmSubsystem(new ArmIOKraken(), m_driveSubsystem::getVisionPose); -// m_climber = new ClimberSubsystem(new ClimberIOKraken() {}); + new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS) +// new VisionSubsystem[]{ +// new VisionSubsystem("RightCamera", DriveConstants.RIGHT_CAMERA_TRANSFORMATION) +// } + ); + m_shooter = new ShooterSubsystem(new ShooterIOKraken() {}); + m_armSubsystem = new ArmSubsystem(new ArmIOKraken()); + m_climber = new ClimberSubsystem(new ClimberIOKraken() {}); } case PROTO_ARM -> { m_driveSubsystem = new DriveSubsystem( @@ -96,9 +97,9 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); -// m_shooter = new ShooterSubsystem(new ShooterIO() {}); -// m_armSubsystem = new ArmSubsystem(new ArmIOPrototype()); -// m_climber = new ClimberSubsystem(new ClimberIO() {}); + m_shooter = new ShooterSubsystem(new ShooterIO() {}); + m_armSubsystem = new ArmSubsystem(new ArmIOPrototype()); + m_climber = new ClimberSubsystem(new ClimberIO() {}); } case PROTO_SHOOTER -> { m_driveSubsystem = new DriveSubsystem( @@ -108,9 +109,9 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); -// m_shooter = new ShooterSubsystem(new ShooterIOKraken()); -// m_climber = new ClimberSubsystem(new ClimberIO() {}); -// m_armSubsystem = new ArmSubsystem(new ArmIO() {}); + m_shooter = new ShooterSubsystem(new ShooterIOKraken()); + m_climber = new ClimberSubsystem(new ClimberIO() {}); + m_armSubsystem = new ArmSubsystem(new ArmIO() {}); } case SIM -> { // Sim robot, instantiate physics sim IO implementations @@ -121,9 +122,9 @@ public RobotContainer() { new ModuleIOSim(DriveConstants.FR_MOD_CONSTANTS), new ModuleIOSim(DriveConstants.BL_MOD_CONSTANTS), new ModuleIOSim(DriveConstants.BR_MOD_CONSTANTS)); -// m_shooter = new ShooterSubsystem(new ShooterIOSim()); -// m_armSubsystem = new ArmSubsystem(new ArmIOSim(), m_driveSubsystem::getPose); -// m_climber = new ClimberSubsystem(new ClimberIO() {}); + m_shooter = new ShooterSubsystem(new ShooterIOSim()); + m_armSubsystem = new ArmSubsystem(new ArmIOSim()); + m_climber = new ClimberSubsystem(new ClimberIO() {}); } default -> { // Replayed robot, disable IO implementations @@ -134,9 +135,9 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); -// m_shooter = new ShooterSubsystem(new ShooterIO() {}); -// m_armSubsystem = new ArmSubsystem(new ArmIO() {}); -// m_climber = new ClimberSubsystem(new ClimberIO() {}); + m_shooter = new ShooterSubsystem(new ShooterIO() {}); + m_armSubsystem = new ArmSubsystem(new ArmIO() {}); + m_climber = new ClimberSubsystem(new ClimberIO() {}); } } @@ -144,12 +145,12 @@ public RobotContainer() { autoChooser = new SendableChooser<>(); // Set up feedforward characterization - autoChooser.addOption( - "Drive FF Characterization", - new FeedForwardCharacterization( - m_driveSubsystem, - m_driveSubsystem::runCharacterizationVolts, - m_driveSubsystem::getCharacterizationVelocity)); +// autoChooser.addOption( +// "Drive FF Characterization", +// new FeedForwardCharacterization( +// m_driveSubsystem, +// m_driveSubsystem::runCharacterizationVolts, +// m_driveSubsystem::getCharacterizationVelocity)); // Configure the button bindings configureButtonBindings(); @@ -183,22 +184,22 @@ private void configureButtonBindings() { double centerDistance = 1.34 - Units.inchesToMeters(3.0); - m_driveSubsystem.setDefaultCommand( - DriveCommands.joystickDrive( - m_driveSubsystem, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); - controller - .start() - .onTrue( - Commands.runOnce( - () -> - m_driveSubsystem.setPose(AllianceFlipUtil.apply( - new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(240.25), 5.55), - Rotation2d.fromDegrees(180.0)))), - m_driveSubsystem) - .ignoringDisable(true)); +// m_driveSubsystem.setDefaultCommand( +// DriveCommands.joystickDrive( +// m_driveSubsystem, +// () -> -controller.getLeftY(), +// () -> -controller.getLeftX(), +// () -> -controller.getRightX())); +// controller +// .start() +// .onTrue( +// Commands.runOnce( +// () -> +// m_driveSubsystem.setPose(AllianceFlipUtil.apply( +// new Pose2d(new Translation2d(centerDistance + Units.inchesToMeters(240.25), 5.55), +// Rotation2d.fromDegrees(180.0)))), +// m_driveSubsystem) +// .ignoringDisable(true)); } /** @@ -214,7 +215,7 @@ private void configureDashboard() { // commandTab.add("Disable Arm Brake", m_armSubsystem.enableBrakeMode(false)); // commandTab.add("Enable Arm Brake", m_armSubsystem.enableBrakeMode(true)); - +/* commandTab.add("Center Robot Pose", Commands.runOnce( () -> m_driveSubsystem.setPose( @@ -290,6 +291,8 @@ private void configureDashboard() { Rotation2d.fromDegrees(180.0)))), m_driveSubsystem) .ignoringDisable(true)); + + */ } /** diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 15dd4e85..b9d44135 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -245,6 +245,7 @@ default void setKickerVoltage(double voltage) {} default void setIntakeVoltage(double voltage) {} default void setLeftVelocityRpm(double rpm) {} default void setRightVelocityRpm(double rpm) {} + default boolean hasPiece() {return false;} default void updateInputs(ShooterIOInputs inputs) {} default void stop() {} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java index 77c6b107..a0a5e376 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java @@ -60,6 +60,8 @@ public ShooterIOKraken() { m_tof = new TimeOfFlight(28); m_tof.setRangingMode(TimeOfFlight.RangingMode.Short, 25); + m_tof.pidGet(); + // general motor configs TalonFXConfiguration shooterConfig = new TalonFXConfiguration(); shooterConfig.CurrentLimits.SupplyCurrentLimitEnable = true; @@ -188,12 +190,17 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.intakeTemperature = m_intakeTemperatureSignal.getValueAsDouble(); inputs.indexerTemperature = m_indexerTemperatureSignal.getValueAsDouble(); - inputs.tofDistanceIn = m_tof.getRange(); +// inputs.tofDistanceIn = m_tof.getRange(); m_leftProperty.updateIfChanged(); m_rightProperty.updateIfChanged(); } +// @Override +// public boolean hasPiece() { +// return m_tof.getRange() < 60; +// } + @Override public void setMotorVoltageTL(double voltage) { m_leftTalon.setVoltage(voltage); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 9793763c..c3e7ce50 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -68,7 +68,7 @@ public Command runShooterVelocity(boolean runKicker) { } public boolean hasPiece() { - return m_inputs.tofDistanceIn < 60; + return m_io.hasPiece(); } /** Command Factories */