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

lime stuff works

Browse Source
This commit is contained in:
aarav18
2023-03-16 16:25:26 -06:00
parent ff2c34a26e
commit 6b2ca9e433
4 changed files with 62 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1 -1
View File
@@ -159,6 +159,6 @@ public final class Constants {
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 = 32.0;
public static final double MID_TAPE_HEIGHT = 24.0;
}
}
src/main/java/frc4388/robot/RobotContainer.java
+9 -4
View File
@@ -20,7 +20,7 @@ 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;
@@ -144,10 +144,15 @@ public class RobotContainer {
// new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> m_limeLight.toggleLEDs(), m_limeLight));
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.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/LimeAlign.java
+16 -8
View File
@@ -4,20 +4,20 @@
package frc4388.robot.commands;
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;
import frc4388.utility.AbhiIsADumbass;
public class LimeAlign extends PelvicInflammatoryDisease {
SwerveDrive drive;
Limelight lime;
public LimeAlign(SwerveDrive drive, Limelight lime) {
super(0.1, 0.0, 0.0, 0.0, 10);
super(0.7, 0.1, 0.0, 0.0, 0);
this.drive = drive;
this.lime = lime;
@@ -27,16 +27,24 @@ public class LimeAlign extends PelvicInflammatoryDisease {
@Override
public double getError() {
double err = 0.0;
try {
return lime.getTargetPoints().get(0).x - (VisionConstants.LIME_HIXELS / 2);
} catch (AbhiIsADumbass abhiIsADumbass) {
abhiIsADumbass.printStackTrace();
return 0;
}
err = lime.getFirstTargetPoint().getYaw() / (VisionConstants.H_FOV / 2);
} catch (NullPointerException ex) {}
return err;
}
@Override
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/subsystems/Limelight.java
+36 -16
View File
@@ -23,13 +23,13 @@ public class Limelight extends SubsystemBase {
private PhotonCamera cam;
private boolean lightOn;
private boolean isConnected = false;
/** Creates a new Limelight. */
public Limelight() {
cam = new PhotonCamera(VisionConstants.NAME);
isConnected = cam.isConnected();
cam.setDriverMode(false);
setToLimePipeline();
}
public void setLEDs(boolean on) {
@@ -46,16 +46,24 @@ public class Limelight extends SubsystemBase {
cam.setDriverMode(driverMode);
}
public ArrayList<Point> getTargetPoints() throws AbhiIsADumbass {
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()) throw new AbhiIsADumbass();
if (!result.hasTargets()) return null;
ArrayList<Point> points = new ArrayList<>(2);
for(PhotonTrackedTarget target : result.getTargets()){
for(PhotonTrackedTarget target : result.getTargets()) {
List<TargetCorner> corners = target.getDetectedCorners();
double sumX = 0.0;
@@ -77,38 +85,50 @@ public class Limelight extends SubsystemBase {
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) throws AbhiIsADumbass {
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);
Point tapePoint = targetPoints.get(0);//high ? highPoint : midPoint;
PhotonTrackedTarget tapePoint = getFirstTargetPoint();//high ? highPoint : midPoint;
double tapeHeight = VisionConstants.MID_TAPE_HEIGHT;//high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT;
double theta = VisionConstants.LIME_ANGLE + getPointAngle(tapePoint);
double theta = 35.0 + tapePoint.getPitch();
double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT;
double distanceToTape = effectiveTapeHeight / Math.sin(Math.toRadians(theta));
double horizontalDistanceToTarget = Math.sqrt(Math.pow(distanceToTape, 2) - Math.pow(effectiveTapeHeight, 2));
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
try {
System.out.println(getHorizontalDistanceToTarget(false));
} catch(AbhiIsADumbass abhiIsADumbass) {
abhiIsADumbass.printStackTrace();
}
if (ctr % 50 == 0) {
SmartDashboard.putNumber("Horizontal Distance", getHorizontalDistanceToTarget(false));
}
ctr++;
}
}