Skip to content

Commit

Permalink
added more things to make photonvision work
Browse files Browse the repository at this point in the history
  • Loading branch information
@HeeistHo
HeeistHo committed Jun 27, 2023
1 parent f9dd838 commit 6941458
Show file tree
Hide file tree
Showing 5 changed files with 128 additions and 88 deletions.
33 changes: 33 additions & 0 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package frc.robot;

import java.io.IOException;
import java.nio.file.Path;
import java.nio.file.Paths;

Expand All @@ -8,12 +9,16 @@
import com.ctre.phoenix.led.CANdle.LEDStripType;
import com.revrobotics.SparkMaxLimitSwitch;

import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
Expand Down Expand Up @@ -195,6 +200,34 @@ public static final class LimelightConstants {
public static final double CUBE_OFFSET = 0.0; // TODO find this value
}

public static final class PhotonConstants {
public static final String FRONT_NAME = "photon-front";
public static final String BACK_NAME = "photon-back";
public static final Pose3d FRONT_POSE = new Pose3d(.1, 0.28, 0.72, new Rotation3d(0, 0, 0));
public static final Pose3d BACK_POSE = new Pose3d(.1, 0.28, 0.83, new Rotation3d(0, 10, 180));
public static final double CAMERA_HEIGHT = 0.72;
public static final double TARGET_HEIGHT = Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19);
public static final double TARGET_WIDTH = Units.inchesToMeters(41.30) - Units.inchesToMeters(6.70);
public static final Transform3d ROBOT_TO_CAM =
new Transform3d(
new Translation3d(.1, .28, .72),
new Rotation3d(
0, 0,
0)); // Cam mounted facing forward, half a meter forward of center, half a meter up
// from center.
public static AprilTagFieldLayout FIELD_LAYOUT;

static {
try {
FIELD_LAYOUT = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField();
}
catch(IOException e) {
// TODO decide what you want to do if the layout fails to load
}
}

}

public static final class ArmConstants {
// Motor configuration constants
public static final boolean MOTOR_INVERT = true;
Expand Down
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/Maps.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
import frc.robot.commands.AutoBalance;
import frc.robot.commands.Collect;
import frc.robot.commands.HoldPower;
import frc.robot.commands.ThrowCube;
// import frc.robot.commands.ThrowCube;
import frc.robot.subsystems.Arm;
import frc.robot.subsystems.Collector;
import frc.robot.subsystems.Drivetrain;
Expand Down Expand Up @@ -57,13 +57,13 @@ public static HashMap<String, Command> getPathMap(Drivetrain drivetrain, ServoTu
eventMap.put("Score-Slow", new InstantCommand(() -> collector.setPower(-.50))); //Lower power for no roll out
eventMap.put("Score", new InstantCommand(() -> collector.setPower(-1d)));
eventMap.put("Auto-Balance", new AutoBalance(drivetrain));
eventMap.put("Throw-Cube", new ThrowCube(lift, arm, collector).withTimeout(4));
// eventMap.put("Throw-Cube", new ThrowCube(lift, arm, collector).withTimeout(4));
// eventMap.put("Throw-Cube", new ThrowCube(lift, arm, collector));
eventMap.put("Turn-On-Vision", new InstantCommand(() -> VisionBase.enableVision()));
eventMap.put("Turn-Off-Vision", new InstantCommand(() -> VisionBase.disableVision()));
eventMap.put("Pos1", new InstantCommand(() -> drivetrain.setAprilTagTarget(1)));
eventMap.put("Pos2", new InstantCommand(() -> drivetrain.setAprilTagTarget(2)));
eventMap.put("Pos3", new InstantCommand(() -> drivetrain.setAprilTagTarget(3)));
// eventMap.put("Pos1", new InstantCommand(() -> drivetrain.setAprilTagTarget(1)));
// eventMap.put("Pos2", new InstantCommand(() -> drivetrain.setAprilTagTarget(2)));
// eventMap.put("Pos3", new InstantCommand(() -> drivetrain.setAprilTagTarget(3)));
// eventMap.put("ResetTag", new InstantCommand(() -> drivetrain.setAprilTagTargetAll()));

return eventMap;
Expand Down
20 changes: 12 additions & 8 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,8 @@
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import frc.robot.subsystems.LimelightFront;

import frc.robot.subsystems.PhotonBack;
import frc.robot.subsystems.PhotonFront;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.subsystems.Collector.GamePiece;
Expand All @@ -40,7 +41,7 @@
import frc.robot.commands.AprilTagLineUp;
import frc.robot.commands.SafeToScoreLED;
import frc.robot.commands.SingleSubstationAlign;
import frc.robot.commands.Lift.ReverseDoubleSubStationCollect;
// import frc.robot.commands.Lift.ReverseDoubleSubStationCollect;
import frc.robot.commands.Lift.DoubleSubstationCollect;
import frc.robot.commands.Lift.Ground;
import frc.robot.commands.Lift.HighScore;
Expand All @@ -55,6 +56,7 @@
import frc.robot.Constants.AutoAlignConstants;
import frc.robot.Constants.AutonomousConstants;
import frc.robot.Constants.LimelightConstants;
import frc.robot.Constants.PhotonConstants;
import frc.thunder.auto.Autonomous;
import frc.thunder.auto.AutonomousCommandFactory;
import frc.thunder.filter.JoystickFilter;
Expand All @@ -66,11 +68,13 @@

public class RobotContainer extends LightningContainer {

private static final LimelightFront frontLimelight = new LimelightFront(LimelightConstants.FRONT_NAME, LimelightConstants.FRONT_POSE);
private static final LimelightBack backLimelight = new LimelightBack(LimelightConstants.BACK_NAME, LimelightConstants.BACK_POSE);
// private static final LimelightFront frontLimelight = new LimelightFront(LimelightConstants.FRONT_NAME, LimelightConstants.FRONT_POSE);
// private static final LimelightBack backLimelight = new LimelightBack(LimelightConstants.BACK_NAME, LimelightConstants.BACK_POSE);
private static final PhotonFront frontCamera = new PhotonFront(PhotonConstants.FRONT_NAME);
private static final PhotonBack backCamera = new PhotonBack(PhotonConstants.BACK_NAME);

// Creating our main subsystems
private static final Drivetrain drivetrain = new Drivetrain(backLimelight, frontLimelight);
private static final Drivetrain drivetrain = new Drivetrain(backCamera, frontCamera);
private static final Arm arm = new Arm();
private static final Wrist wrist = new Wrist(arm);
private static final Elevator elevator = new Elevator();
Expand Down Expand Up @@ -111,7 +115,7 @@ protected void configureButtonBindings() {
new Trigger(driver::getLeftBumper).onTrue(new InstantCommand(() -> collector.setGamePiece(GamePiece.CUBE)));

// new Trigger(driver::getYButton).onTrue(new InstantCommand(() -> drivetrain.moveToDesiredPose(autoFactory), drivetrain)).onFalse(new InstantCommand(drivetrain::stop, drivetrain));
new Trigger(driver::getYButton).whileTrue(new SingleSubstationAlign(drivetrain, frontLimelight, collector));
// new Trigger(driver::getYButton).whileTrue(new SingleSubstationAlign(drivetrain, frontLimelight, collector));

// SET DRIVE PODS TO 45
new Trigger(driver::getXButton).whileTrue(new RunCommand(() -> drivetrain.stop(), drivetrain));
Expand All @@ -132,8 +136,8 @@ protected void configureButtonBindings() {
new Trigger(driver::getStartButton).onTrue(new InstantCommand(servoturn::flickServo));

// AutoAlign based on cone or cube
new Trigger(driver::getBButton).whileTrue(new ConditionalCommand(new AprilTagLineUp(drivetrain, frontLimelight, collector), new RetroLineUp(drivetrain, frontLimelight, collector),
() -> collector.getGamePiece() == GamePiece.CUBE));
// new Trigger(driver::getBButton).whileTrue(new ConditionalCommand(new AprilTagLineUp(drivetrain, frontLimelight, collector), new RetroLineUp(drivetrain, frontLimelight, collector),
// () -> collector.getGamePiece() == GamePiece.CUBE));

//AUTOBALANCE
// new Trigger(driver::getBButton).whileTrue(new AutoBalance(drivetrain));
Expand Down
150 changes: 76 additions & 74 deletions src/main/java/frc/robot/subsystems/Drivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,8 @@
import frc.thunder.shuffleboard.LightningShuffleboard;
import frc.thunder.shuffleboard.LightningShuffleboardPeriodic;

import org.photonvision.PhotonCamera;

/**
* The drivetrain subsystem
*/
Expand Down Expand Up @@ -140,8 +142,8 @@ public class Drivetrain extends SubsystemBase {
// Chassis speeds for the robot
private ChassisSpeeds outputChassisSpeeds = new ChassisSpeeds();

private LimelightBack limelightBack;
private LimelightFront limelightFront;
private PhotonFront frontCamera;
private PhotonBack backCamera;

private boolean hasLimitChanged = false;

Expand All @@ -157,9 +159,9 @@ public class Drivetrain extends SubsystemBase {
private boolean initialSync = false;
private double initialTimeStamp = 0;

public Drivetrain(LimelightBack limelightBack, LimelightFront limelightFront) {
this.limelightBack = limelightBack;
this.limelightFront = limelightFront;
public Drivetrain(PhotonBack backCamera, PhotonFront frontCamera) {
this.frontCamera = frontCamera;
this.backCamera = backCamera;
/**
* Creates a new Drivetrain.
*
Expand Down Expand Up @@ -242,7 +244,7 @@ public void periodic() {

// Update our odometry
updateOdometry();
updateVision();
// updateVision();

periodicShuffleboard.loop();
periodicShuffleboardAuto.loop();
Expand Down Expand Up @@ -381,79 +383,79 @@ public void updateOdometry() {

private boolean firstTime = true;

public void updateVision() {
if (VisionBase.isVisionEnabled()) {
visionPose2d = null;
double latency = 0;
double tagDistance = 0;
if (limelightFront.hasVision()) {
visionPose2d = limelightFront.getRobotPose();
latency = limelightFront.getLatencyBotPoseBlue();
tagDistance = limelightFront.getTagDistance();
if (firstTime) {
setInitialPose(visionPose2d);
firstTime = false;
}
} else if (limelightBack.hasVision()) {
visionPose2d = limelightBack.getRobotPose();
latency = limelightBack.getLatencyBotPoseBlue();
tagDistance = limelightBack.getTagDistance();
if (firstTime) {
setInitialPose(visionPose2d);

firstTime = false;
}
}

if (visionPose2d == null || visionPose2d.getX() > 23.04 || visionPose2d.getY() > 8.02 || visionPose2d.getX() < 0 || visionPose2d.getY() < 0 || tagDistance > 4) {
return;
}

double currTime = Timer.getFPGATimestamp();
LightningShuffleboard.setDouble("Drivetrain", "Velocity between points", pose.getTranslation().getDistance(visionPose2d.getTranslation()) / (currTime - lastTime));
if (pose.getTranslation().getDistance(visionPose2d.getTranslation()) / (currTime - lastTime) > DrivetrainConstants.MAX_VELOCITY_METERS_PER_SECOND) {
return;
}

// if (tagDistance != -1) {
// double distanceBasedDev = VisionConstants.visionStandardDevMap.get(tagDistance + getDriveVelocity());
// poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(distanceBasedDev, distanceBasedDev, distanceBasedDev));
// }

visionPose2d = new Pose2d(visionPose2d.getX(), pose.getY(), getHeading());
poseEstimator.addVisionMeasurement(visionPose2d, Timer.getFPGATimestamp() - latency - .0472);
pose = poseEstimator.getEstimatedPosition();

lastKnownGoodVisionX = visionPose2d.getX();
lastKnownGoodVisionY = visionPose2d.getY();
lastKnownGoodVisionRotation = visionPose2d.getRotation().getDegrees();
lastTime = currTime;

LightningShuffleboard.setDouble("Drivetrain", "Accepted vision X", lastKnownGoodVisionX);
LightningShuffleboard.setDouble("Drivetrain", "Accepted vision Y", lastKnownGoodVisionY);
LightningShuffleboard.setDouble("Drivetrain", "Accepted vision Rotation", lastKnownGoodVisionRotation);
}
}
// public void updateVision() {
// if (VisionBase.isVisionEnabled()) {
// visionPose2d = null;
// double latency = 0;
// double tagDistance = 0;
// if (frontCamera.hasVision()) {
// visionPose2d = limelightFront.getRobotPose();
// latency = limelightFront.getLatencyBotPoseBlue();
// tagDistance = limelightFront.getTagDistance();
// if (firstTime) {
// setInitialPose(visionPose2d);
// firstTime = false;
// }
// } else if (limelightBack.hasVision()) {
// visionPose2d = limelightBack.getRobotPose();
// latency = limelightBack.getLatencyBotPoseBlue();
// tagDistance = limelightBack.getTagDistance();
// if (firstTime) {
// setInitialPose(visionPose2d);

// firstTime = false;
// }
// }

// if (visionPose2d == null || visionPose2d.getX() > 23.04 || visionPose2d.getY() > 8.02 || visionPose2d.getX() < 0 || visionPose2d.getY() < 0 || tagDistance > 4) {
// return;
// }

// double currTime = Timer.getFPGATimestamp();
// LightningShuffleboard.setDouble("Drivetrain", "Velocity between points", pose.getTranslation().getDistance(visionPose2d.getTranslation()) / (currTime - lastTime));
// if (pose.getTranslation().getDistance(visionPose2d.getTranslation()) / (currTime - lastTime) > DrivetrainConstants.MAX_VELOCITY_METERS_PER_SECOND) {
// return;
// }

// // if (tagDistance != -1) {
// // double distanceBasedDev = VisionConstants.visionStandardDevMap.get(tagDistance + getDriveVelocity());
// // poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(distanceBasedDev, distanceBasedDev, distanceBasedDev));
// // }

// visionPose2d = new Pose2d(visionPose2d.getX(), pose.getY(), getHeading());
// poseEstimator.addVisionMeasurement(visionPose2d, Timer.getFPGATimestamp() - latency - .0472);
// pose = poseEstimator.getEstimatedPosition();

// lastKnownGoodVisionX = visionPose2d.getX();
// lastKnownGoodVisionY = visionPose2d.getY();
// lastKnownGoodVisionRotation = visionPose2d.getRotation().getDegrees();
// lastTime = currTime;

// LightningShuffleboard.setDouble("Drivetrain", "Accepted vision X", lastKnownGoodVisionX);
// LightningShuffleboard.setDouble("Drivetrain", "Accepted vision Y", lastKnownGoodVisionY);
// LightningShuffleboard.setDouble("Drivetrain", "Accepted vision Rotation", lastKnownGoodVisionRotation);
// }
// }

/**
* ta
*
* @param pos Pos of tag 1 at C nodes
*/
public void setAprilTagTarget(int pos) {
if (DriverStation.getAlliance() == Alliance.Blue) {
pos += 6;
} else {
pos += 3;
}
limelightBack.setPipelineNum(pos);
limelightFront.setPipelineNum(pos);
}
// public void setAprilTagTarget(int pos) {
// if (DriverStation.getAlliance() == Alliance.Blue) {
// pos += 6;
// } else {
// pos += 3;
// }
// limelightBack.setPipelineNum(pos);
// limelightFront.setPipelineNum(pos);
// }

public void setAprilTagTargetAll() {
limelightBack.setPipelineNum(0);
limelightFront.setPipelineNum(0);
}
// public void setAprilTagTargetAll() {
// limelightBack.setPipelineNum(0);
// limelightFront.setPipelineNum(0);
// }

/**
* 2 Method to set states of modules.
Expand Down Expand Up @@ -493,8 +495,8 @@ private void initializeShuffleboard() {
new Pair<String, Object>("desired X", (DoubleSupplier) () -> desiredPose.getX()), new Pair<String, Object>("desired Y", (DoubleSupplier) () -> desiredPose.getY()),
new Pair<String, Object>("desired Z", (DoubleSupplier) () -> desiredPose.getRotation().getDegrees()));

periodicShuffleboardAuto = new LightningShuffleboardPeriodic("Autonomous", new Pair<String, Object>("has vision", (BooleanSupplier) () -> limelightBack.hasVision()),
new Pair<String, Object>("Vison GOOD", (BooleanSupplier) () -> !firstTime));
// periodicShuffleboardAuto = new LightningShuffleboardPeriodic("Autonomous", new Pair<String, Object>("has vision", (BooleanSupplier) () -> limelightBack.hasVision()),
// new Pair<String, Object>("Vison GOOD", (BooleanSupplier) () -> !firstTime));
}

/**
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/subsystems/LimelightFront.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,8 @@ public LimelightFront(String limelightName, Pose3d cameraPose) {
@SuppressWarnings("unchecked")
private void initializeShuffleboard() {
periodicShuffleboard =
new LightningShuffleboardPeriodic("Vision", 0.17, new Pair<String, Object>(limelightName + "Vision bot pose TX", (DoubleSupplier) () -> LimelightHelpers.getBotPose(limelightName)[0]),
new LightningShuffleboardPeriodic("Vision", 0.17,
new Pair<String, Object>(limelightName + "Vision bot pose TX", (DoubleSupplier) () -> LimelightHelpers.getBotPose(limelightName)[0]),
new Pair<String, Object>(limelightName + "Vision bot pose TY", (DoubleSupplier) () -> LimelightHelpers.getBotPose(limelightName)[1]),
new Pair<String, Object>(limelightName + "Vision bot pose RZ", (DoubleSupplier) () -> LimelightHelpers.getBotPose(limelightName)[5]),
new Pair<String, Object>(limelightName + "Vision bot pose Blue TX", (DoubleSupplier) () -> LimelightHelpers.getBotPose_wpiBlue(limelightName)[0]),
Expand Down

0 comments on commit 6941458

Please sign in to comment.