diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a69ffb5..f008111 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -44,6 +44,7 @@ import frc.robot.commands.Shooter.PidSetRpm; import frc.robot.commands.Shooter.ShooterModeChange; import frc.robot.commands.Shooter.ShooterSetDegree; +import frc.robot.commands.Shooter.VisionShooter; import frc.robot.commands.Swerve.SwerveJoystickCmd; import frc.robot.commands.autonomous.AutoNoteShoot; import frc.robot.commands.common.FeedingPosition; @@ -56,6 +57,7 @@ import frc.robot.subsystems.FeederSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.JoystickSubsystem; +import frc.robot.subsystems.NetworkSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; //import edu.wpi.first.math.trajectory.TrajectoryGenerator; @@ -69,8 +71,10 @@ public class RobotContainer { private final XboxController driverJoytick = new XboxController(1); private final XboxController subJoytick = new XboxController(3); private final FeederSubsystem m_feeder = new FeederSubsystem(); - private final ArmSubsystem m_arm = new ArmSubsystem(); + private final ArmSubsystem m_arm = new ArmSubsystem(); private final JoystickSubsystem m_joystick = new JoystickSubsystem(); + NetworkSubsystem m_network = new NetworkSubsystem(); + public int autonomous_case; public RobotContainer() { @@ -150,8 +154,8 @@ private void configureBindings() { new JoystickButton(subJoytick, 2).whileFalse(new InstantCommand(()->m_intake.StopNoteMotor())); //buton 3 Shooter Manuel subbuffer -new JoystickButton(subJoytick, 3).whileTrue(new ShooterSetDegree(m_shooter, ()->70.0)); -new JoystickButton(subJoytick, 3).whileTrue(new InstantCommand(()-> m_shooter.ShooterThrowMotorOutput(-0.9))); +new JoystickButton(subJoytick, 3).whileTrue(new VisionShooter(m_shooter, m_network)); + new JoystickButton(subJoytick, 3).whileTrue(new InstantCommand(()-> m_shooter.ShooterThrowMotorOutput(-0.9))); new JoystickButton(subJoytick, 3).whileFalse(new InstantCommand(()-> m_shooter.ShooterThrow1MotorStop())); new JoystickButton(subJoytick, 3).whileFalse(new InstantCommand(()-> m_shooter.ShooterThrow2MotorStop())); diff --git a/src/main/java/frc/robot/commands/Shooter/VisionShooter.java b/src/main/java/frc/robot/commands/Shooter/VisionShooter.java index dfa767f..01ed742 100644 --- a/src/main/java/frc/robot/commands/Shooter/VisionShooter.java +++ b/src/main/java/frc/robot/commands/Shooter/VisionShooter.java @@ -1,9 +1,7 @@ package frc.robot.commands.Shooter; -import java.util.function.DoubleSupplier; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.PIDCommand; import frc.robot.Constants; import frc.robot.subsystems.NetworkSubsystem; @@ -11,22 +9,27 @@ public class VisionShooter extends PIDCommand { - public VisionShooter(ShooterSubsystem m_shooter, NetworkSubsystem m_network , DoubleSupplier angle) { + public VisionShooter(ShooterSubsystem m_shooter, NetworkSubsystem m_network) { super( - new PIDController(0.04, - 0.03, + new PIDController(0.004, + 0, Constants.values.shooter.PidShooterAngleKD), () -> m_network.getY(), - () -> 0, + () -> -100, output -> { - - m_shooter.ShooterAngleMotorOutput(output * -.13); + /* try { + m_shooter.ShooterAngleMotorOutput(output * .13); + Thread.sleep(150); // 2 saniye durakla + m_shooter.ShooterAngleMotorStop(); + } catch (InterruptedException e) { + e.printStackTrace(); + }*/ + m_shooter.ShooterAngleMotorOutput(output * .13); }); addRequirements(m_shooter); - getController().setTolerance(Constants.values.shooter.PidShooterAngleTolerance); - SmartDashboard.putNumber("pid shooter gonderilen pozisyon", angle.getAsDouble()); + getController().setTolerance(5); } diff --git a/src/main/java/frc/robot/subsystems/NetworkSubsystem.java b/src/main/java/frc/robot/subsystems/NetworkSubsystem.java index 65ab6b7..44abed0 100644 --- a/src/main/java/frc/robot/subsystems/NetworkSubsystem.java +++ b/src/main/java/frc/robot/subsystems/NetworkSubsystem.java @@ -3,15 +3,18 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.ConnectionInfo; +// import edu.wpi.first.networktables. import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class NetworkSubsystem extends SubsystemBase { double apriltag_x_value; double apriltag_y_value; - public NetworkTableInstance inst = NetworkTableInstance.create(); + public NetworkTableInstance inst = NetworkTableInstance.getDefault(); //table public NetworkTable table = inst.getTable("vision"); @@ -23,7 +26,9 @@ public class NetworkSubsystem extends SubsystemBase { public NetworkTableEntry apriltag_y = networkTable.getEntry("apriltag_y"); public NetworkSubsystem() { - inst.startServer("10.76.72.2"); + // inst.setServerTeam(7672); + //inst.startServer("10.76.72.2"); + inst.setServerTeam(7672); } public double getX(){ @@ -38,6 +43,9 @@ public double getY(){ public void periodic() { apriltag_x_value = apriltag_x.getDouble(0.0); apriltag_y_value = apriltag_y.getDouble(0.0); + SmartDashboard.putNumber("apriltag x value", apriltag_x_value); + SmartDashboard.putNumber("apriltag y value", apriltag_y_value); + SmartDashboard.putBoolean("connection",inst.isConnected()); }