Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-08 16:28:00 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #39 from Team4388/limelight

Browse Source 
Limelight --> Master
This commit is contained in:
Aarav Shah
2023-03-16 16:57:16 -06:00
committed by GitHub
parent 7c9160daf1 a04c0e841a
commit 9ede629072
7 changed files with 241 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+19
View File
@@ -150,4 +150,23 @@ public final class Constants {
public static final double RIGHT_AXIS_DEADBAND = 0.6;
public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing
}
public static final class VisionConstants {
public static final String NAME = "photonCamera";
public static final int LIME_HIXELS = 640;
public static final int LIME_VIXELS = 480;
public static final double H_FOV = 59.6;
public static final double V_FOV = 45.7;
public static final double LIME_HEIGHT = 6.0;
public static final double LIME_ANGLE = 55.0;
// public static final double HIGH_TARGET_HEIGHT = 46.0;
public static final double HIGH_TAPE_HEIGHT = 44.0;
// public static final double MID_TARGET_HEIGHT = 34.0;
public static final double MID_TAPE_HEIGHT = 24.0;
}
}
src/main/java/frc4388/robot/RobotContainer.java
+11 -10
View File
@@ -7,10 +7,6 @@
package frc4388.robot;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import edu.wpi.first.wpilibj.PWM;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -21,9 +17,10 @@ import frc4388.robot.commands.AutoBalance;
import frc4388.robot.commands.JoystickPlayback;
import frc4388.robot.subsystems.Arm;
import frc4388.robot.subsystems.Claw;
import frc4388.robot.subsystems.Limelight;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.commands.JoystickRecorder;
import frc4388.robot.commands.PivotCommand;
import frc4388.robot.commands.LimeAlign;
import frc4388.robot.commands.PlaybackChooser;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.XboxController;
@@ -50,6 +47,8 @@ public class RobotContainer {
public final Claw m_robotClaw = new Claw(m_robotMap.servo);
public final Limelight m_limeLight = new Limelight();
/* Controllers */
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -153,13 +152,15 @@ public class RobotContainer {
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
.onTrue(new PivotCommand(m_robotArm, 135));
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.onTrue(new PivotCommand(m_robotArm, 210));
.onTrue(new InstantCommand(() -> m_limeLight.toggleLEDs(), m_limeLight));
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
.whileTrue(new LimeAlign(m_robotSwerveDrive, m_limeLight));
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
src/main/java/frc4388/robot/commands/AutoBalance.java
-12
View File
@@ -38,16 +38,4 @@ public class AutoBalance extends PelvicInflammatoryDisease {
if (Math.abs(getError()) < 3) out2 = 0;
drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
}
@Override
public void initialize() {
super.initialize();
// this.gyro.reset();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/LimeAlign.java
+32 -16
View File
@@ -4,31 +4,47 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import org.photonvision.targeting.PhotonTrackedTarget;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.Limelight;
import frc4388.robot.subsystems.SwerveDrive;
public class LimeAlign extends CommandBase {
public LimeAlign(SwerveDrive drive) {
addRequirements(drive);
}
public class LimeAlign extends PelvicInflammatoryDisease {
// Called when the command is initially scheduled.
@Override
public void initialize() {
SwerveDrive drive;
Limelight lime;
public LimeAlign(SwerveDrive drive, Limelight lime) {
super(0.7, 0.1, 0.0, 0.0, 0);
this.drive = drive;
this.lime = lime;
addRequirements(drive, lime);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
public double getError() {
double err = 0.0;
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
try {
err = lime.getFirstTargetPoint().getYaw() / (VisionConstants.H_FOV / 2);
} catch (NullPointerException ex) {}
return err;
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
public void runWithOutput(double output) {
if (output > 0) {
output += 0.6;
} else if (output < 0) {
output -= 0.6;
}
drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true);
}
}
src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java
+4 -4
View File
@@ -4,7 +4,6 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.utility.Gains;
@@ -26,12 +25,13 @@ public abstract class PelvicInflammatoryDisease extends CommandBase {
/** produces the error from the setpoint */
public abstract double getError();
/** figure it out bitch */
public abstract void runWithOutput(double output);
// Called when the command is initially scheduled.
@Override
public void initialize() {
public final void initialize() {
output = 0;
}
@@ -39,7 +39,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
public final void execute() {
double error = getError();
cumError += error * .02; // 20 ms
double delta = error - prevError;
@@ -54,7 +54,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
public final boolean isFinished() {
return Math.abs(getError()) < tolerance;
}
}
src/main/java/frc4388/robot/subsystems/Limelight.java
+134
View File
@@ -0,0 +1,134 @@
// 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 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.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.VisionConstants;
import frc4388.utility.AbhiIsADumbass;
public class Limelight extends SubsystemBase {
private PhotonCamera cam;
private boolean lightOn;
/** Creates a new Limelight. */
public Limelight() {
cam = new PhotonCamera(VisionConstants.NAME);
cam.setDriverMode(false);
setToLimePipeline();
}
public void setLEDs(boolean on) {
lightOn = on;
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
}
public void toggleLEDs() {
lightOn = !lightOn;
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
}
public void setDriverMode(boolean driverMode) {
cam.setDriverMode(driverMode);
}
public void setToLimePipeline() {
cam.setPipelineIndex(1);
}
public void setToAprilPipeline() {
cam.setPipelineIndex(0);
}
public ArrayList<Point> getTargetPoints() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
ArrayList<Point> points = new ArrayList<>(2);
for(PhotonTrackedTarget target : result.getTargets()) {
List<TargetCorner> corners = target.getDetectedCorners();
double sumX = 0.0;
double sumY = 0.0;
double mx = 0.0;
double my = 0.0;
for (TargetCorner c : corners) {
sumX += c.x;
sumY += c.y;
}
mx = sumX / 4.0;
my = sumY / 4.0;
points.add(new Point(mx, my));
}
return points;
}
public PhotonTrackedTarget getFirstTargetPoint() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
return result.getBestTarget();
}
private double getPointAngle(Point point) {
return (VisionConstants.LIME_VIXELS - point.y) * (VisionConstants.V_FOV / VisionConstants.LIME_VIXELS);
}
public double getHorizontalDistanceToTarget(boolean high) {
ArrayList<Point> targetPoints = getTargetPoints();
if (targetPoints == null) return -1;
// Point highPoint = targetPoints.get(0).y <= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1);
// Point midPoint = targetPoints.get(0).y >= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1);
PhotonTrackedTarget tapePoint = getFirstTargetPoint();//high ? highPoint : midPoint;
double tapeHeight = VisionConstants.MID_TAPE_HEIGHT;//high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT;
double theta = 35.0 + tapePoint.getPitch();
double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT;
double horizontalDistanceToTarget = effectiveTapeHeight / Math.tan(Math.toRadians(theta));
return horizontalDistanceToTarget;
}
int ctr = 0;
@Override
public void periodic() {
// This method will be called once per scheduler run
if (ctr % 50 == 0) {
SmartDashboard.putNumber("Horizontal Distance", getHorizontalDistanceToTarget(false));
}
ctr++;
}
}
vendordeps/photonlib.json
+41
View File
@@ -0,0 +1,41 @@
{
"fileName": "photonlib.json",
"name": "photonlib",
"version": "v2023.4.2",
"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": "v2023.4.2",
"libName": "Photon",
"headerClassifier": "headers",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"windowsx86-64",
"linuxathena",
"linuxx86-64",
"osxuniversal"
]
}
],
"javaDependencies": [
{
"groupId": "org.photonvision",
"artifactId": "PhotonLib-java",
"version": "v2023.4.2"
},
{
"groupId": "org.photonvision",
"artifactId": "PhotonTargeting-java",
"version": "v2023.4.2"
}
]
}