diff --git a/simgui.json b/simgui.json index 55c0b83..fb4fc2f 100644 --- a/simgui.json +++ b/simgui.json @@ -51,6 +51,7 @@ "/LiveWindow/Ungrouped/Talon FX [8]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [9]": "Motor Controller", "/LiveWindow/Vision": "Subsystem", + "/LiveWindow/VisionOdometry": "Subsystem", "/SmartDashboard/Field": "Field2d", "/SmartDashboard/JVM Memory": "Command", "/SmartDashboard/Scheduler": "Scheduler", diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index 99413bc..573b060 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,3 +1,3 @@ Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) -0 ,16 ,12000 -999 ,28.8 ,12000 \ No newline at end of file +0 ,16 ,0 +999 ,28.8 ,1200 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 04134fc..085dbb0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -206,11 +206,27 @@ public final class Constants { } public static final class VisionConstants { - public static final double TURN_P_VALUE = 0.8; - public static final double X_ANGLE_ERROR = 0.5; - public static final double GRAV = 385.83; - public static final double TARGET_HEIGHT = 67.5; - public static final double FOV = 29.8; // Field of view limelight + // public static final double TURN_P_VALUE = 0.8; + // public static final double X_ANGLE_ERROR = 0.5; + // public static final double GRAV = 385.83; + // public static final double TARGET_HEIGHT = 67.5; + // public static final double FOV = 29.8; //Field of view limelight + public static final double LIME_ANGLE = 24.7; + + public static final String NAME = "photonCamera"; + + public static final double TARGET_HEIGHT = 8*12 + 8; // Convert to metric + public static final double TARGET_RADIUS = 4*12; // Convert to metric + public static final double H_FOV = 59.6; + public static final double V_FOV = 49.7; + public static final double LIME_VIXELS = 960; + public static final double LIME_HIXELS = 720; + public static final double TURRET_kP = 0.1; + + public static final double RANGE = 10; + + public static final double LIMELIGHT_RADIUS = 1.d; + public static final double SHOOTER_CORRECTION = 1.d; } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b7b81df..25e521b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -60,6 +60,7 @@ import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.AimToCenter; import frc4388.robot.commands.Shoot; +import frc4388.robot.commands.TrackTarget; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.Intake; @@ -69,6 +70,7 @@ import frc4388.robot.subsystems.Storage; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.Vision; +import frc4388.robot.subsystems.VisionOdometry; import frc4388.utility.LEDPatterns; import frc4388.utility.ListeningSendableChooser; import frc4388.utility.PathPlannerUtil; @@ -94,9 +96,9 @@ public class RobotContainer { private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter); private final LED m_robotLED = new LED(m_robotMap.LEDController); private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); - private final Hood m_robotHood = new Hood(); + private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); - private final Vision m_robotVision = new Vision(m_robotTurret, m_robotBoomBoom); + private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -130,8 +132,8 @@ public class RobotContainer { // Turret default command - // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, - // m_robotSwerveDrive)); + //m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); + m_robotTurret.setDefaultCommand(new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)); //Swerve Drive m_robotSwerveDrive.setDefaultCommand( @@ -225,6 +227,13 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)) .whenReleased(() -> m_robotStorage.runStorage(0.0)); + + //Shooter + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); + + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index f41012d..47ee185 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -13,6 +13,7 @@ import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_PigeonIMU; import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.DigitalInput; @@ -32,17 +33,15 @@ import frc4388.robot.subsystems.SwerveModule; public class RobotMap { public RobotMap() { - // configureLEDMotorControllers(); + configureLEDMotorControllers(); configureSwerveMotorControllers(); - // configureShooterMotorControllers(); + configureShooterMotorControllers(); } /* LED Subsystem */ public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - void configureLEDMotorControllers() { - - } + void configureLEDMotorControllers() {} /* Swerve Subsystem */ @@ -167,8 +166,12 @@ public class RobotMap { public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); + // turret subsystem public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); + // hood subsystem + public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + // Create motor CANSparkMax void configureShooterMotorControllers() { @@ -209,6 +212,9 @@ public class RobotMap { // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull // numbers out of our ass anymore + // hood subsystem + angleAdjusterMotor.restoreFactoryDefaults(); + angleAdjusterMotor.setIdleMode(IdleMode.kBrake); } diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index 45f0998..d0c706a 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -6,13 +6,16 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.VisionOdometry; public class AimToCenter extends CommandBase { /** Creates a new AimWithOdometry. */ Turret m_turret; SwerveDrive m_drive; + VisionOdometry m_visionOdometry; // use odometry to find x and y later double x; @@ -21,11 +24,12 @@ public class AimToCenter extends CommandBase { // public static Gains m_aimGains; - public AimToCenter(Turret turret, SwerveDrive drive) { + public AimToCenter(Turret turret, SwerveDrive drive, VisionOdometry visionOdometry) { // Use addRequirements() here to declare subsystem dependencies. m_turret = turret; m_drive = drive; - addRequirements(m_turret, m_drive); + m_visionOdometry = visionOdometry; + addRequirements(m_turret, m_drive, m_visionOdometry); } // Called when the command is initially scheduled. @@ -40,6 +44,15 @@ public class AimToCenter extends CommandBase { public void execute() { m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); m_turret.runshooterRotatePID(m_targetAngle); + + // Check if limelight is within range (comment out to disable vision odo) + if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){ + m_visionOdometry.updateOdometryWithVision(); + m_visionOdometry.setLEDs(true); + } + else{ + m_visionOdometry.setLEDs(false); + } } public static double angleToCenter(double x, double y, double gyro) { diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java index be04921..4899c71 100644 --- a/src/main/java/frc4388/robot/commands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/Shoot.java @@ -7,6 +7,7 @@ package frc4388.robot.commands; import edu.wpi.first.hal.simulation.SimulatorJNI; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; import edu.wpi.first.wpilibj.simulation.SimHooks; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.subsystems.BoomBoom; @@ -44,7 +45,8 @@ public class Shoot extends CommandBase { public double proportional, integral, derivative; public double time; public double output; - public double tolerance = 5.0; + public double tolerance; + public boolean isAimedInTolerance; // testing public DummySensor dummy = new DummySensor(0); @@ -75,6 +77,9 @@ public class Shoot extends CommandBase { derivative = 0; time = 0.02; + tolerance = 5.0; + isAimedInTolerance = false; + DummySensor.resetAll(); } @@ -83,6 +88,8 @@ public class Shoot extends CommandBase { */ public void updateError() { error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360; + isAimedInTolerance = (Math.abs(error) <= tolerance); + SmartDashboard.putBoolean("isAimed?", isAimedInTolerance); } // Called when the command is initially scheduled. @@ -154,7 +161,6 @@ public class Shoot extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - updateError(); - return Math.abs(error) <= tolerance; + return false; } } diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java new file mode 100644 index 0000000..ae70cb4 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -0,0 +1,121 @@ +// 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.commands; + +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; + +import org.opencv.core.Point; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.Constants.VisionConstants; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.VisionOdometry; + +public class TrackTarget extends CommandBase { + /** Creates a new TrackTarget. */ + Turret m_turret; + SwerveDrive m_drive; + VisionOdometry m_visionOdometry; + BoomBoom m_boomBoom; + Hood m_hood; + + // use odometry to find x and y later + double x; + double y; + double distance; + double vel; + double hood; + double average; + double output; + Pose2d pos = new Pose2d(); + ArrayList points = new ArrayList<>(); + + // public static Gains m_aimGains; + + public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, SwerveDrive drive, VisionOdometry visionOdometry) { + // Use addRequirements() here to declare subsystem dependencies. + m_turret = turret; + m_drive = drive; + m_boomBoom = boomBoom; + m_hood = hood; + m_visionOdometry = visionOdometry; + addRequirements(m_turret, m_drive, m_visionOdometry); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + x = 0; + y = 0; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + //m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); + m_visionOdometry.setLEDs(true); + points = m_visionOdometry.getTargetPoints(); + double pointTotal = 0; + for(Point point : points) + { + pointTotal = pointTotal + point.x; + } + average = pointTotal/points.size(); + output = average/VisionConstants.LIME_HIXELS * VisionConstants.TURRET_kP; + m_turret.runTurretWithInput(output); + try{ + pos = m_visionOdometry.getVisionOdometry(); + distance = Math.hypot(pos.getX(), pos.getY()); + } + catch (Exception e){ + } + vel = m_boomBoom.getVelocity(distance); + hood = m_boomBoom.getHood(distance); + m_boomBoom.runDrumShooterVelocityPID(vel); + m_hood.runAngleAdjustPID(hood); + //m_turret.runshooterRotatePID(m_targetAngle); + } + + /* public static double angleToCenter(double x, double y, double gyro) { + double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) + return angle; + }*/ + + /** + * Checks if in hardware deadzone (due to mechanical limitations). + * @param angle Angle to check. + * @return True if in hardware deadzone. + */ + public static boolean isHardwareDeadzone(double angle) { + return ((ShooterConstants.HARD_DEADZONE_LEFT > angle) || (angle > ShooterConstants.HARD_DEADZONE_RIGHT)); + } + + /** + * Checks if in digital deadzone (due to climber). + * @param angle Angle to check. + * @return True if in digital deadzone. + */ + public static boolean isDigitalDeadzone(double angle) { + return ((ShooterConstants.DIG_DEADZONE_LEFT < angle) && (angle < ShooterConstants.DIG_DEADZONE_RIGHT)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index b811a37..58c1e3d 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -24,13 +24,13 @@ import frc4388.utility.Gains; public class Hood extends SubsystemBase { public BoomBoom m_shooterSubsystem; - public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public CANSparkMax m_angleAdjusterMotor; public SparkMaxLimitSwitch m_hoodUpLimitSwitch; public SparkMaxLimitSwitch m_hoodDownLimitSwitch; - public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; - public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); + public static Gains m_angleAdjusterGains; + public RelativeEncoder m_angleEncoder; - public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); + public SparkMaxPIDController m_angleAdjusterPIDController; public boolean m_isHoodReady = false; @@ -39,8 +39,12 @@ public double m_fireAngle; /** Creates a new Hood. */ - public Hood() { - m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + public Hood(CANSparkMax angleAdjusterMotor) { + + m_angleAdjusterMotor = angleAdjusterMotor; + m_angleEncoder = m_angleAdjusterMotor.getEncoder(); + m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); + m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 1f4ff9b..3a33474 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -17,12 +17,14 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; +import frc4388.utility.VisionObscuredException; public class SwerveDrive extends SubsystemBase { @@ -241,20 +243,14 @@ public class SwerveDrive extends SubsystemBase { * Updates the field relative position of the robot. */ public void updateOdometry() { - m_poseEstimator.update(getRegGyro(), - modules[0].getState(), - modules[1].getState(), - modules[2].getState(), - modules[3].getState()); - - // Also apply vision measurements. We use 0.3 seconds in the past as an example - // -- on - // a real robot, this must be calculated based either on latency or timestamps. - // m_poseEstimator.addVisionMeasurement( - // m_poseEstimator.getEstimatedPosition(), - // Timer.getFPGATimestamp() - 0.1); + m_poseEstimator.update( getRegGyro(), + modules[0].getState(), + modules[1].getState(), + modules[2].getState(), + modules[3].getState()); } - + + /** * Resets pigeon. */ diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java new file mode 100644 index 0000000..162a897 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -0,0 +1,293 @@ +/*----------------------------------------------------------------------------*/ +/* 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.opencv.core.Point3; +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.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.VisionConstants; +import frc4388.utility.VisionObscuredException; + +/** Represents the limelight and odometry related functionality + * @author Daniel McGrath + */ +public class VisionOdometry extends SubsystemBase { + // roborio ip & port: 10.43.88.2:1735 + private PhotonCamera m_camera; + + private SwerveDrive m_drive; + private Turret m_shooter; + + private double latency = 0; + + /** Creates a new VisionOdometry + * + * @param drive The swerve drive subsystem + * @param shooter The turret subsystem + */ + public VisionOdometry(SwerveDrive drive, Turret shooter) { + m_camera = new PhotonCamera(VisionConstants.NAME); + m_drive = drive; + m_shooter = shooter; + } + + /** Gets the vision points from the limelight + *

+ * Breaks down targets into 4 corners and uses the top 2 points + * + * @return Vision points on the rim of the target in screen space + */ + public ArrayList getTargetPoints() { + PhotonPipelineResult result = m_camera.getLatestResult(); + latency = result.getLatencyMillis(); + + if(!result.hasTargets()) + return new ArrayList(); + + 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, VisionConstants.LIME_VIXELS - corner.y)); + } + } + + return points; + } + + /** Sets LEDs on or off (duh) + * + * @param on LED state + */ + public void setLEDs(boolean on) { + m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff); + } + + /** Gets estimated odometry based on limelight data + * + * @return The estimated odometry pose, including gyro rotation + * @throws VisionObscuredException + */ + public Pose2d getVisionOdometry() throws VisionObscuredException { + ArrayList screenPoints = getTargetPoints(); + + if(screenPoints.size() < 3) + throw new VisionObscuredException("Not enough vision points available"); + + ArrayList points3d = get3dPoints(screenPoints); + ArrayList points = topView(points3d); + + Point guess = averagePoint(points); + + for(int i = 0; i < 30; i++) { + guess = iterateGuess(guess, points); + } + + guess = correctGuessForCenter(guess, m_shooter.getBoomBoomAngleDegrees()); + guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees()); + + SmartDashboard.putNumber("Vision ODO x: ", guess.x); + SmartDashboard.putNumber("Vision ODO y: ", guess.y); + + Rotation2d rotation = new Rotation2d(Math.toDegrees(m_drive.getRegGyro().getDegrees())); + Pose2d odometryPose = new Pose2d(guess.x, guess.y, rotation); + + return odometryPose; + } + + public double getLatency() { + return latency; + } + public boolean getLEDs() { + if (m_camera.getLEDMode() == VisionLEDMode.kOff) return false; + return true; + } + + public void updateOdometryWithVision(){ + try { + m_drive.m_poseEstimator.addVisionMeasurement( + getVisionOdometry(), + Timer.getFPGATimestamp() - getLatency()); + } catch (VisionObscuredException err) { + err.printStackTrace(); + } + } + /** Reverse 3d projects target points from screen coordinates to 3d space + *

+ * Uses the known height of the target to project points + * + * @param points2d Vision points on the rim of the target in screen space + * @return Reverse 3d projected points + */ + public static final ArrayList get3dPoints(ArrayList points2d) { + ArrayList points3d = new ArrayList<>(); + + for(Point point2d : points2d) { + double y_rot = point2d.y / VisionConstants.LIME_VIXELS; + y_rot *= Math.toRadians(VisionConstants.V_FOV); + y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2; + y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); + + double x_rot = point2d.x / VisionConstants.LIME_HIXELS; + x_rot *= Math.toRadians(VisionConstants.H_FOV); + x_rot -= Math.toRadians(VisionConstants.H_FOV) / 2; + + double z = VisionConstants.TARGET_HEIGHT / Math.tan(y_rot); + double x = z * Math.tan(x_rot); + double y = VisionConstants.TARGET_HEIGHT; + + points3d.add(new Point3(x, y, z)); + } + + return points3d; + } + + /** Flattens 3d points from above + * + * @param points3d 3d points along the target rim + * @return An array of flattened 3d points + */ + public static final ArrayList topView(ArrayList points3d) { + ArrayList points = new ArrayList<>(); + + for(Point3 point3d : points3d) { + points.add(new Point(point3d.x, point3d.z)); + } + + return points; + } + + /** Finds the average point from an array of points + * + * @param points The points the average will be taken from + * @return The average point + */ + public static final Point averagePoint(ArrayList points) { + Point average = new Point(0, 0); + for(Point point : points) { + average.x += point.x; + average.y += point.y; + } + + average.x /= points.size(); + average.y /= points.size(); + + return average; + } + + /** Iterates the current guess for the vision center (relative to the limelight) + * based on points on the rim of the target + *

+ * The guess is iterated by finding the current average vector error between the guess + * and the circlePoints, assuming that the guess should be a constant radius from each point + * + * @param guess The current estimate for the vision center + * @param circlePoints Vision points along the rim of the target + * @return The guess after iteration + */ + public static final Point iterateGuess(Point guess, ArrayList circlePoints) { + Point totalDiff = new Point(0, 0); + + for(Point circlePoint : circlePoints) { + double angle = Math.atan((guess.y - circlePoint.y) / (guess.x - circlePoint.x)); + angle = correctQuadrent(angle, guess, circlePoint); + + Point estimate = new Point(); + estimate.x = VisionConstants.TARGET_RADIUS * Math.cos(angle) + guess.x; + estimate.y = VisionConstants.TARGET_RADIUS * Math.sin(angle) + guess.y; + + Point diff = new Point(estimate.x - circlePoint.x, estimate.y - circlePoint.y); + totalDiff.x += diff.x; + totalDiff.y += diff.y; + } + + totalDiff.x /= circlePoints.size(); + totalDiff.y /= circlePoints.size(); + + return new Point(guess.x - totalDiff.x, guess.y - totalDiff.y); + } + + /** Corrects odometry guess for shooter angle + * + * @param guess The current guess for the vision center + * @param shooterRotation The rotation to correct for + * @return The corrected odometry point + */ + public static final Point correctGuessForCenter(Point guess, double shooterRotation) { + Point corrected = new Point(guess.x, guess.y); + corrected.y += VisionConstants.LIMELIGHT_RADIUS; + + double dist = Math.hypot(guess.x, guess.y); + double angle = Math.tan(corrected.y / corrected.x); + angle += Math.toRadians(shooterRotation); + + corrected.x = dist * Math.cos(angle); + corrected.y = dist * Math.sin(angle); + + corrected.y += VisionConstants.SHOOTER_CORRECTION; + + return corrected; + } + + /** Corrects odometry guess for gyro angle + * + * @param guess The current guess for the vision center + * @param gyroRotation The rotation to correct for + * @return The corrected odometry point + */ + public static final Point correctGuessForGyro(Point guess, double gyroRotation) { + Point corrected = new Point(guess.x, guess.y); + + double dist = Math.hypot(guess.x, guess.y); + double angle = Math.tan(corrected.y / corrected.x); + angle += Math.toRadians(gyroRotation); + + corrected.x = dist * Math.cos(angle); + corrected.y = dist * Math.sin(angle); + + return corrected; + } + + /** Corrects the angle from the current center estimate to a point on the target rim + * for multiple quadrents + * + * @param angle The angle to be corrected + * @param guess The current guess for the vision center + * @param circlePoint A point along the target rim + * @return The angle corrected for the quadrent + */ + public static final double correctQuadrent(double angle, Point guess, Point circlePoint) { + if(circlePoint.x - guess.x < 0) { + return angle - Math.PI; + } + + return angle; + } +} diff --git a/src/main/java/frc4388/utility/VisionObscuredException.java b/src/main/java/frc4388/utility/VisionObscuredException.java new file mode 100644 index 0000000..cc33066 --- /dev/null +++ b/src/main/java/frc4388/utility/VisionObscuredException.java @@ -0,0 +1,38 @@ +package frc4388.utility; + +/** Exception that occurs if the limelight can't see enough points + * @author Daniel Thomas McGrath + */ +public class VisionObscuredException extends Exception { + /** + * Creates new VisionObscuredException with error text 'null' + */ + public VisionObscuredException() { + super("Unable to see sufficient vision points"); + } + + /** Creates new VisionObscuredException with error text message + * + * @param message Error text message + */ + public VisionObscuredException(String message) { + super(message); + } + + /** Creates new VisionObscuredException with error text message and detailed stack trace + * + * @param message Error text message + * @param cause Root cause of error + */ + public VisionObscuredException(String message, Throwable cause) { + super(message, cause); + } + + /** Creates new VisionObscuredException with error text 'null' and detailed stack trace + * + * @param cause Root cause of error + */ + public VisionObscuredException(Throwable cause) { + super("Unable to see sufficient vision points", cause); + } +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..9a31b1e --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,41 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2022.1.5", + "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.5", + "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.5" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2022.1.5" + } + ] +} \ No newline at end of file