From 816a4c0aeaefad3cdfbf41c2d7650c391afa1a99 Mon Sep 17 00:00:00 2001 From: Ryan Manley Date: Tue, 8 Feb 2022 19:46:17 -0700 Subject: [PATCH] Added buttons --- src/main/java/frc4388/robot/Constants.java | 2 + .../java/frc4388/robot/RobotContainer.java | 22 +++++-- .../robot/commands/drive/SetShooterToOdo.java | 36 ++++++++++++ .../commands/drive/VisionUpdateOdometry.java | 42 +++++++++----- .../java/frc4388/robot/subsystems/Vision.java | 58 +++++++++++++++++++ vendordeps/photonlib.json | 41 +++++++++++++ 6 files changed, 182 insertions(+), 19 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/drive/SetShooterToOdo.java create mode 100644 vendordeps/photonlib.json diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2b0e4e9..97d01af 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -249,6 +249,8 @@ public final class Constants { } public static final class VisionConstants { + public static final String NAME = "limelight"; + public static final double FOV = 29.8; // Field of view of limelight public static final double TARGET_HEIGHT = 67.5; public static final double LIME_ANGLE = 24.7; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2bfff20..6033a5d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -49,6 +49,8 @@ import frc4388.robot.commands.climber.RunClimberWithTriggers; import frc4388.robot.commands.climber.RunLevelerWithJoystick; import frc4388.robot.commands.drive.DriveWithJoystick; import frc4388.robot.commands.drive.PlaySongDrive; +import frc4388.robot.commands.drive.SetShooterToOdo; +import frc4388.robot.commands.drive.VisionUpdateOdometry; import frc4388.robot.commands.intake.RunIntakeWithTriggers; import frc4388.robot.commands.shooter.CalibrateShooter; import frc4388.robot.commands.shooter.RunHoodWithJoystick; @@ -70,6 +72,7 @@ import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; import frc4388.robot.subsystems.ShooterHood; import frc4388.robot.subsystems.Storage; +import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Storage.StorageMode; import frc4388.utility.controller.ButtonFox; import frc4388.utility.controller.IHandController; @@ -98,6 +101,7 @@ public class RobotContainer { private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40); private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40); public final LimeLight m_robotLime = new LimeLight(); + public final Vision m_robotVision = new Vision(); /* Controllers */ public boolean isGS = false; @@ -221,10 +225,20 @@ public class RobotContainer { // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(0.2), m_robotShooterHood)); - new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); - // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood)); - // safety for climber and leveler - new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON).whenPressed(new InstantCommand(m_robotClimber::setSafetyPressed, m_robotClimber)).whenReleased(new InstantCommand(m_robotClimber::setSafetyNotPressed, m_robotClimber)); + + // VOP system testing + new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + .whenPressed(new SetShooterToOdo(m_robotShooterAim, m_robotDrive)); + + new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) + .whenPressed(new VisionUpdateOdometry(m_robotVision, m_robotShooterAim, m_robotDrive)); + + // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); + // // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood)); + // // safety for climber and leveler + // new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON).whenPressed(new InstantCommand(m_robotClimber::setSafetyPressed, m_robotClimber)).whenReleased(new InstantCommand(m_robotClimber::setSafetyNotPressed, m_robotClimber)); + + // starts tracking target new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON).whileHeld(new TrackTarget(m_robotShooterAim)).whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))).whenReleased(new InstantCommand(m_robotLime::limeOff)); // .whileHeld(new RunCommand(() -> m_robotShooterAim.runshooterRotatePID())); diff --git a/src/main/java/frc4388/robot/commands/drive/SetShooterToOdo.java b/src/main/java/frc4388/robot/commands/drive/SetShooterToOdo.java new file mode 100644 index 0000000..8404441 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/drive/SetShooterToOdo.java @@ -0,0 +1,36 @@ +package frc4388.robot.commands.drive; + +import org.opencv.core.Mat; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.ShooterAim; + +public class SetShooterToOdo extends CommandBase { + private ShooterAim m_shooterAim; + private Drive m_driveTrain; + + private double targetAngle; + + public SetShooterToOdo(ShooterAim shooterAim, Drive driveTrain) { + m_shooterAim = shooterAim; + m_driveTrain = driveTrain; + } + + @Override + public void initialize() { + double xPos = m_driveTrain.getPose().getX(); + double yPos = m_driveTrain.getPose().getY(); + targetAngle = Math.atan(yPos / xPos) - m_driveTrain.getPose().getRotation().getDegrees(); + } + + @Override + public void execute() { + m_shooterAim.runshooterRotatePID(targetAngle); + } + + @Override + public boolean isFinished() { + return Math.abs(m_shooterAim.getShooterRotatePosition() - targetAngle) < 5; + } +} diff --git a/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java b/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java index 2300e48..82dbd83 100644 --- a/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java +++ b/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java @@ -1,10 +1,13 @@ package frc4388.robot.commands.drive; +import java.util.ArrayList; + import com.ctre.phoenix.sensors.PigeonIMU; import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import org.ejml.simple.SimpleMatrix; import org.opencv.core.Mat; +import org.opencv.core.Point; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.geometry.Pose2d; @@ -21,9 +24,10 @@ import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.LimeLight; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.Vision; public class VisionUpdateOdometry extends CommandBase { - private LimeLight m_limeLight; + private Vision m_limeLight; private ShooterAim m_shooterAim; private Drive m_driveTrain; @@ -39,30 +43,39 @@ public class VisionUpdateOdometry extends CommandBase { * @param shooterAim replace with Turret subsystem for integration with 2022 * @param driveTrain replace with Swerve subsystem for integration with 2022 */ - public VisionUpdateOdometry(LimeLight limeLight, ShooterAim shooterAim, Drive driveTrain) { + public VisionUpdateOdometry(Vision limeLight, ShooterAim shooterAim, Drive driveTrain) { m_limeLight = limeLight; m_shooterAim = shooterAim; m_driveTrain = driveTrain; addRequirements(m_limeLight, m_shooterAim, m_driveTrain); - // Turn camera on but leave LEDs off - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); + // // Turn camera on but leave LEDs off + // NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); + // NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); } // Called when the command is initially scheduled. @Override public void initialize() { // Vision Processing Mode - m_limeLight.limeOn(); - m_limeLight.changePipeline(0); + m_limeLight.setLEDs(true); + // m_limeLight.changePipeline(0); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double[] xPoints = m_limeLight.getXArray(); - double[] yPoints = m_limeLight.getYArray(); + ArrayList points = m_limeLight.getTargetPoints(); + if(points.size() < 5) + System.out.println("not enough points"); + + double[] xPoints = new double[5]; + double[] yPoints = new double[5]; + + for(int i = 0; i < 5; i++) { + xPoints[i] = points.get(i).x; + yPoints[i] = points.get(i).y; + } double[] ellipseRadii = getEllipseRadii(xPoints, yPoints); @@ -82,13 +95,15 @@ public class VisionUpdateOdometry extends CommandBase { xPos = Math.cos(relativeAngle) * distance; yPos = Math.sin(relativeAngle) * distance; translate = new Translation2d(xPos, yPos); + + Pose2d pose = new Pose2d(translate, rotation); + m_driveTrain.setOdometry(pose); // Replace with adding new pose to Kalman filter } // http://www.lee-mac.com/5pointellipse.html // https://math.stackexchange.com/questions/163920/how-to-find-an-ellipse-given-five-points // https://towardsdatascience.com/understanding-singular-value-decomposition-and-its-application-in-data-science-388a54be95d - // https://www.desmos.com/calculator/ounqguxjac - // https://en.wikipedia.org/wiki/Five_points_determine_a_conic + // https://www.desmos.com/calculatoroe_points_determine_a_conic /* solves the determinant of the following matrix * | x0^2 x0y0 y0^2 x0 y0 1 | @@ -200,9 +215,6 @@ public class VisionUpdateOdometry extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - Pose2d pose = new Pose2d(translate, rotation); - m_driveTrain.setOdometry(pose); // Replace with adding new pose to Kalman filter - - return false; + return true; } } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index e69de29..19448bc 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.subsystems; +import java.util.ArrayList; +import java.util.List; + +import org.opencv.core.Point; +import org.photonvision.PhotonCamera; +import org.photonvision.common.hardware.VisionLEDMode; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.VisionConstants; + +public class Vision extends SubsystemBase { + private PhotonCamera m_camera; + + public Vision() { + m_camera = new PhotonCamera(VisionConstants.NAME); + } + + public ArrayList getTargetPoints() { + PhotonPipelineResult result = m_camera.getLatestResult(); + + if(!result.hasTargets()) + return null; + + ArrayList points = new ArrayList<>(); + + for(PhotonTrackedTarget target : result.getTargets()) { + List corners = target.getCorners(); + + double centerY = 0; + for(TargetCorner corner : corners) { + centerY += corner.y; + } + centerY /= corners.size(); + + for(TargetCorner corner : corners) { + if(corner.y <= centerY) + points.add(new Point(corner.x, corner.y)); + } + } + + return points; + } + + public void setLEDs(boolean on) { + m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff); + } +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..88ccb3f --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,41 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2022.1.4", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-cpp", + "version": "v2022.1.4", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxx86-64" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2022.1.4" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2022.1.4" + } + ] +} \ No newline at end of file