mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
@@ -19,18 +19,18 @@ package org.photonlib.examples.aimandrange;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
|
||||
@@ -26,11 +26,11 @@ import org.photonvision.PhotonCamera;
|
||||
import org.photonvision.PhotonUtils;
|
||||
|
||||
/**
|
||||
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||
* the package after creating this project, you must also update the build.gradle file in the
|
||||
* project.
|
||||
*/
|
||||
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||
* the package after creating this project, you must also update the build.gradle file in the
|
||||
* project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
// Constants such as camera and target height stored. Change per robot and goal!
|
||||
final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
|
||||
|
||||
@@ -19,18 +19,18 @@ package org.photonlib.examples.aimattarget;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
|
||||
@@ -25,11 +25,11 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
|
||||
import org.photonvision.PhotonCamera;
|
||||
|
||||
/**
|
||||
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||
* the package after creating this project, you must also update the build.gradle file in the
|
||||
* project.
|
||||
*/
|
||||
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||
* the package after creating this project, you must also update the build.gradle file in the
|
||||
* project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
// Constants such as camera and target height stored. Change per robot and goal!
|
||||
final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
|
||||
|
||||
@@ -19,18 +19,18 @@ package org.photonlib.examples.getinrange;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
|
||||
@@ -26,11 +26,11 @@ import org.photonvision.PhotonCamera;
|
||||
import org.photonvision.PhotonUtils;
|
||||
|
||||
/**
|
||||
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||
* the package after creating this project, you must also update the build.gradle file in the
|
||||
* project.
|
||||
*/
|
||||
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||
* the package after creating this project, you must also update the build.gradle file in the
|
||||
* project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
// Constants such as camera and target height stored. Change per robot and goal!
|
||||
final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
|
||||
|
||||
@@ -19,18 +19,18 @@ package org.photonlib.examples.simaimandrange;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
|
||||
@@ -27,11 +27,11 @@ import org.photonvision.PhotonCamera;
|
||||
import org.photonvision.PhotonUtils;
|
||||
|
||||
/**
|
||||
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||
* the package after creating this project, you must also update the build.gradle file in the
|
||||
* project.
|
||||
*/
|
||||
* The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||
* the package after creating this project, you must also update the build.gradle file in the
|
||||
* project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
// 2020 High goal target height above ground
|
||||
public static final double TARGET_HEIGHT_METERS = Units.inchesToMeters(81.19);
|
||||
|
||||
@@ -36,12 +36,12 @@ import org.photonvision.SimVisionSystem;
|
||||
import org.photonvision.SimVisionTarget;
|
||||
|
||||
/**
|
||||
* Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated
|
||||
* PhotonVision system and one vision target.
|
||||
*
|
||||
* <p>This class and its methods are only relevant during simulation. While on the real robot, the
|
||||
* real motors/sensors/physics are used instead.
|
||||
*/
|
||||
* Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated
|
||||
* PhotonVision system and one vision target.
|
||||
*
|
||||
* <p>This class and its methods are only relevant during simulation. While on the real robot, the
|
||||
* real motors/sensors/physics are used instead.
|
||||
*/
|
||||
public class DrivetrainSim {
|
||||
// Simulated Motor Controllers
|
||||
PWMSim leftLeader = new PWMSim(0);
|
||||
@@ -108,9 +108,9 @@ public class DrivetrainSim {
|
||||
}
|
||||
|
||||
/**
|
||||
* Perform all periodic drivetrain simulation related tasks to advance our simulation of robot
|
||||
* physics forward by a single 20ms step.
|
||||
*/
|
||||
* Perform all periodic drivetrain simulation related tasks to advance our simulation of robot
|
||||
* physics forward by a single 20ms step.
|
||||
*/
|
||||
public void update() {
|
||||
double leftMotorCmd = 0;
|
||||
double rightMotorCmd = 0;
|
||||
@@ -132,11 +132,11 @@ public class DrivetrainSim {
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the simulation back to a pre-defined pose Useful to simulate the action of placing the
|
||||
* robot onto a specific spot in the field (IE, at the start of each match).
|
||||
*
|
||||
* @param pose
|
||||
*/
|
||||
* Resets the simulation back to a pre-defined pose Useful to simulate the action of placing the
|
||||
* robot onto a specific spot in the field (IE, at the start of each match).
|
||||
*
|
||||
* @param pose
|
||||
*/
|
||||
public void resetPose(Pose2d pose) {
|
||||
drivetrainSimulator.setPose(pose);
|
||||
}
|
||||
|
||||
@@ -20,18 +20,18 @@ import edu.wpi.first.wpilibj.RobotBase;
|
||||
import org.photonlib.examples.simposeest.robot.Robot;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
|
||||
* you are doing, do not modify this file except to change the parameter class to the startRobot
|
||||
* call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
|
||||
@@ -27,11 +27,11 @@ import edu.wpi.first.wpilibj.Timer;
|
||||
import java.util.List;
|
||||
|
||||
/**
|
||||
* Implements logic to convert a set of desired waypoints (ie, a trajectory) and the current
|
||||
* estimate of where the robot is at (ie, the estimated Pose) into motion commands for a drivetrain.
|
||||
* The Ramaste controller is used to smoothly move the robot from where it thinks it is to where it
|
||||
* thinks it ought to be.
|
||||
*/
|
||||
* Implements logic to convert a set of desired waypoints (ie, a trajectory) and the current
|
||||
* estimate of where the robot is at (ie, the estimated Pose) into motion commands for a drivetrain.
|
||||
* The Ramaste controller is used to smoothly move the robot from where it thinks it is to where it
|
||||
* thinks it ought to be.
|
||||
*/
|
||||
public class AutoController {
|
||||
private Trajectory trajectory;
|
||||
|
||||
@@ -72,13 +72,13 @@ public class AutoController {
|
||||
}
|
||||
|
||||
/**
|
||||
* Given the current estimate of the robot's position, calculate drivetrain speed commands which
|
||||
* will best-execute the active trajectory. Be sure to call `startPath()` prior to calling this
|
||||
* method.
|
||||
*
|
||||
* @param curEstPose Current estimate of drivetrain pose on the field
|
||||
* @return The commanded drivetrain motion
|
||||
*/
|
||||
* Given the current estimate of the robot's position, calculate drivetrain speed commands which
|
||||
* will best-execute the active trajectory. Be sure to call `startPath()` prior to calling this
|
||||
* method.
|
||||
*
|
||||
* @param curEstPose Current estimate of drivetrain pose on the field
|
||||
* @return The commanded drivetrain motion
|
||||
*/
|
||||
public ChassisSpeeds getCurMotorCmds(Pose2d curEstPose) {
|
||||
if (isRunning) {
|
||||
double elapsed = timer.get();
|
||||
@@ -91,9 +91,9 @@ public class AutoController {
|
||||
}
|
||||
|
||||
/**
|
||||
* @return The position which the auto controller is attempting to move the drivetrain to right
|
||||
* now.
|
||||
*/
|
||||
* @return The position which the auto controller is attempting to move the drivetrain to right
|
||||
* now.
|
||||
*/
|
||||
public Pose2d getCurPose2d() {
|
||||
return desiredDtState.poseMeters;
|
||||
}
|
||||
|
||||
@@ -24,11 +24,11 @@ import edu.wpi.first.math.util.Units;
|
||||
import org.photonvision.SimVisionTarget;
|
||||
|
||||
/**
|
||||
* Holding class for all physical constants that must be used throughout the codebase. These values
|
||||
* should be set by one of a few methods: 1) Talk to your mechanical and electrical teams and
|
||||
* determine how the physical robot is being built and configured. 2) Read the game manual and look
|
||||
* at the field drawings 3) Match with how your vision coprocessor is configured.
|
||||
*/
|
||||
* Holding class for all physical constants that must be used throughout the codebase. These values
|
||||
* should be set by one of a few methods: 1) Talk to your mechanical and electrical teams and
|
||||
* determine how the physical robot is being built and configured. 2) Read the game manual and look
|
||||
* at the field drawings 3) Match with how your vision coprocessor is configured.
|
||||
*/
|
||||
public class Constants {
|
||||
//////////////////////////////////////////////////////////////////
|
||||
// Drivetrain Physical
|
||||
|
||||
@@ -27,10 +27,10 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
|
||||
|
||||
/**
|
||||
* Implements a controller for the drivetrain. Converts a set of chassis motion commands into motor
|
||||
* controller PWM values which attempt to speed up or slow down the wheels to match the desired
|
||||
* speed.
|
||||
*/
|
||||
* Implements a controller for the drivetrain. Converts a set of chassis motion commands into motor
|
||||
* controller PWM values which attempt to speed up or slow down the wheels to match the desired
|
||||
* speed.
|
||||
*/
|
||||
public class Drivetrain {
|
||||
// PWM motor controller output definitions
|
||||
PWMVictorSPX leftLeader = new PWMVictorSPX(Constants.kDtLeftLeaderPin);
|
||||
@@ -77,12 +77,12 @@ public class Drivetrain {
|
||||
}
|
||||
|
||||
/**
|
||||
* Given a set of chassis (fwd/rev + rotate) speed commands, perform all periodic tasks to assign
|
||||
* new outputs to the motor controllers.
|
||||
*
|
||||
* @param xSpeed Desired chassis Forward or Reverse speed (in meters/sec). Positive is forward.
|
||||
* @param rot Desired chassis rotation speed in radians/sec. Positive is counter-clockwise.
|
||||
*/
|
||||
* Given a set of chassis (fwd/rev + rotate) speed commands, perform all periodic tasks to assign
|
||||
* new outputs to the motor controllers.
|
||||
*
|
||||
* @param xSpeed Desired chassis Forward or Reverse speed (in meters/sec). Positive is forward.
|
||||
* @param rot Desired chassis rotation speed in radians/sec. Positive is counter-clockwise.
|
||||
*/
|
||||
public void drive(double xSpeed, double rot) {
|
||||
// Convert our fwd/rev and rotate commands to wheel speed commands
|
||||
DifferentialDriveWheelSpeeds speeds =
|
||||
@@ -111,12 +111,12 @@ public class Drivetrain {
|
||||
}
|
||||
|
||||
/**
|
||||
* Force the pose estimator and all sensors to a particular pose. This is useful for indicating to
|
||||
* the software when you have manually moved your robot in a particular position on the field (EX:
|
||||
* when you place it on the field at the start of the match).
|
||||
*
|
||||
* @param pose
|
||||
*/
|
||||
* Force the pose estimator and all sensors to a particular pose. This is useful for indicating to
|
||||
* the software when you have manually moved your robot in a particular position on the field (EX:
|
||||
* when you place it on the field at the start of the match).
|
||||
*
|
||||
* @param pose
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
leftEncoder.reset();
|
||||
rightEncoder.reset();
|
||||
|
||||
@@ -31,10 +31,10 @@ import edu.wpi.first.wpilibj.Timer;
|
||||
import org.photonvision.PhotonCamera;
|
||||
|
||||
/**
|
||||
* Performs estimation of the drivetrain's current position on the field, using a vision system,
|
||||
* drivetrain encoders, and a gyroscope. These sensor readings are fused together using a Kalman
|
||||
* filter. This in turn creates a best-guess at a Pose2d of where our drivetrain is currently at.
|
||||
*/
|
||||
* Performs estimation of the drivetrain's current position on the field, using a vision system,
|
||||
* drivetrain encoders, and a gyroscope. These sensor readings are fused together using a Kalman
|
||||
* filter. This in turn creates a best-guess at a Pose2d of where our drivetrain is currently at.
|
||||
*/
|
||||
public class DrivetrainPoseEstimator {
|
||||
// Sensors used as part of the Pose Estimation
|
||||
private final AnalogGyro gyro = new AnalogGyro(Constants.kGyroPin);
|
||||
@@ -65,12 +65,12 @@ public class DrivetrainPoseEstimator {
|
||||
public DrivetrainPoseEstimator() {}
|
||||
|
||||
/**
|
||||
* Perform all periodic pose estimation tasks.
|
||||
*
|
||||
* @param actWheelSpeeds Current Speeds (in m/s) of the drivetrain wheels
|
||||
* @param leftDist Distance (in m) the left wheel has traveled
|
||||
* @param rightDist Distance (in m) the right wheel has traveled
|
||||
*/
|
||||
* Perform all periodic pose estimation tasks.
|
||||
*
|
||||
* @param actWheelSpeeds Current Speeds (in m/s) of the drivetrain wheels
|
||||
* @param leftDist Distance (in m) the left wheel has traveled
|
||||
* @param rightDist Distance (in m) the right wheel has traveled
|
||||
*/
|
||||
public void update(
|
||||
DifferentialDriveWheelSpeeds actWheelSpeeds, double leftDist, double rightDist) {
|
||||
m_poseEstimator.update(gyro.getRotation2d(), actWheelSpeeds, leftDist, rightDist);
|
||||
@@ -86,12 +86,12 @@ public class DrivetrainPoseEstimator {
|
||||
}
|
||||
|
||||
/**
|
||||
* Force the pose estimator to a particular pose. This is useful for indicating to the software
|
||||
* when you have manually moved your robot in a particular position on the field (EX: when you
|
||||
* place it on the field at the start of the match).
|
||||
*
|
||||
* @param pose
|
||||
*/
|
||||
* Force the pose estimator to a particular pose. This is useful for indicating to the software
|
||||
* when you have manually moved your robot in a particular position on the field (EX: when you
|
||||
* place it on the field at the start of the match).
|
||||
*
|
||||
* @param pose
|
||||
*/
|
||||
public void resetToPose(Pose2d pose) {
|
||||
m_poseEstimator.resetPosition(pose, gyro.getRotation2d());
|
||||
}
|
||||
|
||||
@@ -34,12 +34,12 @@ import org.photonlib.examples.simposeest.robot.Constants;
|
||||
import org.photonvision.SimVisionSystem;
|
||||
|
||||
/**
|
||||
* Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated
|
||||
* PhotonVision system and one vision target.
|
||||
*
|
||||
* <p>This class and its methods are only relevant during simulation. While on the real robot, the
|
||||
* real motors/sensors/physics are used instead.
|
||||
*/
|
||||
* Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated
|
||||
* PhotonVision system and one vision target.
|
||||
*
|
||||
* <p>This class and its methods are only relevant during simulation. While on the real robot, the
|
||||
* real motors/sensors/physics are used instead.
|
||||
*/
|
||||
public class DrivetrainSim {
|
||||
// Simulated Sensors
|
||||
AnalogGyroSim gyroSim = new AnalogGyroSim(Constants.kGyroPin);
|
||||
@@ -94,9 +94,9 @@ public class DrivetrainSim {
|
||||
}
|
||||
|
||||
/**
|
||||
* Perform all periodic drivetrain simulation related tasks to advance our simulation of robot
|
||||
* physics forward by a single 20ms step.
|
||||
*/
|
||||
* Perform all periodic drivetrain simulation related tasks to advance our simulation of robot
|
||||
* physics forward by a single 20ms step.
|
||||
*/
|
||||
public void update() {
|
||||
double leftMotorCmd = 0;
|
||||
double rightMotorCmd = 0;
|
||||
@@ -135,11 +135,11 @@ public class DrivetrainSim {
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the simulation back to a pre-defined pose Useful to simulate the action of placing the
|
||||
* robot onto a specific spot in the field (IE, at the start of each match).
|
||||
*
|
||||
* @param pose
|
||||
*/
|
||||
* Resets the simulation back to a pre-defined pose Useful to simulate the action of placing the
|
||||
* robot onto a specific spot in the field (IE, at the start of each match).
|
||||
*
|
||||
* @param pose
|
||||
*/
|
||||
public void resetPose(Pose2d pose) {
|
||||
drivetrainSimulator.setPose(pose);
|
||||
}
|
||||
@@ -150,11 +150,11 @@ public class DrivetrainSim {
|
||||
}
|
||||
|
||||
/**
|
||||
* For testing purposes only! Applies an unmodeled, undetected offset to the pose Similar to if
|
||||
* you magically kicked your robot to the side in a way the encoders and gyro didn't measure.
|
||||
*
|
||||
* <p>This distrubance should be corrected for once a vision target is in view.
|
||||
*/
|
||||
* For testing purposes only! Applies an unmodeled, undetected offset to the pose Similar to if
|
||||
* you magically kicked your robot to the side in a way the encoders and gyro didn't measure.
|
||||
*
|
||||
* <p>This distrubance should be corrected for once a vision target is in view.
|
||||
*/
|
||||
public void applyKick() {
|
||||
Pose2d newPose =
|
||||
drivetrainSimulator
|
||||
|
||||
Reference in New Issue
Block a user