From b45ebfee79cbeb79e6418562987d47a041d0cc84 Mon Sep 17 00:00:00 2001 From: Computer 0 <48131223+TheGamer1002@users.noreply.github.com> Date: Thu, 27 Apr 2023 17:23:28 -0400 Subject: [PATCH] thursday the 27th of april 2023 --- final-final-final.json | 16 +++- src/main/java/frc/robot/RobotContainer.java | 4 + .../java/frc/robot/commands/TrackPiece.java | 7 ++ .../java/frc/robot/commands/TrackPole.java | 62 +++++++++++++ .../frc/robot/subsystems/PoleTracking.java | 86 +++++++++++++++++++ .../java/frc/robot/subsystems/Vision.java | 2 + 6 files changed, 175 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/TrackPole.java create mode 100644 src/main/java/frc/robot/subsystems/PoleTracking.java diff --git a/final-final-final.json b/final-final-final.json index a9bde5a..80202bd 100644 --- a/final-final-final.json +++ b/final-final-final.json @@ -199,6 +199,19 @@ "_glyph": 148, "_showGlyph": false } + }, + "30,15": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///limelight/pipeline", + "_title": "limelight/pipeline", + "_glyph": 148, + "_showGlyph": false + } } } } @@ -278,8 +291,7 @@ "Y-axis/Automatic bounds": true, "Y-axis/Upper bound": 1.0, "Y-axis/Lower bound": -1.0, - "Y-axis/Unit": "ul", - "Visible data/SmartDashboard/stuff again": true + "Y-axis/Unit": "ul" } }, "17,11": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6511734..99372ec 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -47,6 +47,8 @@ public class RobotContainer { private final PID m_PID = new PID(); private final PDP m_PDP = new PDP(); private final Vision m_Vision = new Vision(m_Tank, m_Claw); + private final PoleTracking m_PoleTracking = new PoleTracking(m_Tank); + // INSTANTIATES ALL COMMANDS private final MoveClawCommand m_MoveClawCommand = new MoveClawCommand(m_Claw); @@ -63,6 +65,7 @@ public class RobotContainer { private final PlaceCubeSecondLevelCommand m_PlaceCubeSecondLevelCommand = new PlaceCubeSecondLevelCommand(m_Tank, m_Arm, m_Claw); private final TrackPiece m_TrackPiece = new TrackPiece(m_Vision, m_Tank, m_Claw); + private final TrackPole m_TrackPole = new TrackPole(m_PoleTracking, m_Tank); // private final XTracking m_XTracking = new XTracking(m_Tank, null, m_Target); // private final YTracking m_YTracking = new YTr @@ -125,6 +128,7 @@ private void configureButtonBindings() { new JoystickButton(m_armController2, XboxController.Button.kY.value).whileTrue(m_PlaceConeSecondLevelCommand); new JoystickButton(m_armController2, XboxController.Button.kX.value).whileTrue(m_TrackPiece); + new JoystickButton(m_armController2, XboxController.Button.kB.value).whileTrue(m_TrackPole); // new JoystickButton(m_armController2, XboxController.Button.kB.value) // .whileTrue(m_PlaceConeSecondLevelCommand); diff --git a/src/main/java/frc/robot/commands/TrackPiece.java b/src/main/java/frc/robot/commands/TrackPiece.java index 915d0a1..b1b80ba 100644 --- a/src/main/java/frc/robot/commands/TrackPiece.java +++ b/src/main/java/frc/robot/commands/TrackPiece.java @@ -4,6 +4,7 @@ package frc.robot.commands; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.RobotContainer; @@ -32,6 +33,10 @@ public void initialize() { SmartDashboard.putString("a", "pid on"); m_Claw.closeClaw(); + + // set pipeline to 2 + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setDouble(2); + } @Override @@ -49,6 +54,8 @@ public void end(boolean interrupted) { m_Tank.arcadeDrive(0,0); SmartDashboard.putString("a", "pid off"); m_Claw.stopClaw(); + + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setDouble(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/TrackPole.java b/src/main/java/frc/robot/commands/TrackPole.java new file mode 100644 index 0000000..1e34e31 --- /dev/null +++ b/src/main/java/frc/robot/commands/TrackPole.java @@ -0,0 +1,62 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Claw; +import frc.robot.subsystems.PoleTracking; +import frc.robot.subsystems.Tank; +import frc.robot.subsystems.Vision; + +public class TrackPole extends CommandBase { + /** Creates a new TrackPiece. */ + private PoleTracking m_PoleTracking; + private Tank m_Tank; + public TrackPole(PoleTracking poleTracking, Tank tank) { + // Use addRequirements() here to declare subsystem dependencies. + m_PoleTracking = poleTracking; + m_Tank = tank; + addRequirements(m_PoleTracking, m_Tank); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + //m_Tank.arcadeDrive(0.05, m_Vision.getController().calculate(m_Vision.getMeasurement(), m_Vision.getSetpoint())); + m_PoleTracking.enable(); + SmartDashboard.putString("a", "pid on"); + + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setDouble(1); + + } + + @Override + public void execute() { + //m_Tank.setAllMotors(-0.2); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setDouble(1); + RobotContainer.rumbleGabeController(1); + + SmartDashboard.putNumber(getName(), m_PoleTracking.getController().calculate(m_PoleTracking.getMeasurement(), m_PoleTracking.getSetpoint())); + } + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.rumbleGabeController(0); + m_PoleTracking.disable(); + m_Tank.arcadeDrive(0,0); + SmartDashboard.putString("a", "pid off"); + + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setDouble(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return m_PoleTracking.atSetpoint(); + } +} diff --git a/src/main/java/frc/robot/subsystems/PoleTracking.java b/src/main/java/frc/robot/subsystems/PoleTracking.java new file mode 100644 index 0000000..847e62d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PoleTracking.java @@ -0,0 +1,86 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.PIDSubsystem; +import frc.robot.RobotContainer; + +public class PoleTracking extends PIDSubsystem { + /** Creates a new Vision. */ + private static double kP = 0.1; + private static double kI = 0; + private static double kD = 0; + + public PIDController pidController; + public Tank m_tank; + + public PoleTracking(Tank tank) { + super( + // The PIDController used by the subsystem + new PIDController(kP, kI, kD)); + + addChild(getName(), m_controller); + m_tank = tank; + m_controller.setSetpoint(0); + m_controller.setTolerance(1); + m_controller.setIntegratorRange(-0.43, 0.43); + + Shuffleboard.getTab(getName()).add(m_controller); + + } + + @Override + public void useOutput(double output, double setpoint) { + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setDouble(1); + + if (0.50 < output) { + output = 0.50; + } else if (-0.50 > output) { + output = -0.50; + } else if (0.1 < output && output < 0.40) { + output = 0.40; + } else if (-0.40 < output && output < -0.1) { + output = -0.40; + } + + m_tank.arcadeDrive(0, -output * 1); + + } + + @Override + public double getMeasurement() { + // Return the process variable measurement here + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0.0); + } + + public boolean atSetpoint() { + return m_controller.atSetpoint(); + } + + @Override + public void setSetpoint(double setpoint) { + m_controller.setSetpoint(setpoint); + } + + public void setPID(double p, double i, double d) { + m_controller.setPID(p, i, d); + } + + public void setTolerance(double tolerance) { + m_controller.setTolerance(tolerance); + } + + @Override + public void periodic() { + super.periodic(); + SmartDashboard.putNumber("calculated turning output from vision (pole)", + m_controller.calculate(getMeasurement(), m_controller.getSetpoint())); + } + +} diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index b47fb37..5540f37 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -34,11 +34,13 @@ public Vision(Tank tank, Claw claw) { m_controller.setIntegratorRange(-0.43, 0.43); Shuffleboard.getTab(getName()).add(m_controller); + } @Override public void useOutput(double output, double setpoint) { + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setDouble(2); double ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0.0);