Make examples runnable, start work on examples (#235)

Co-authored-by: Chris <chrisgerth010592@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>

photonlib-java-examples/src/dev/native/cpp/main.cpp

@@ -0,0 +1,8 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+int main() {}

photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json

@@ -34,5 +34,17 @@
     "packagetoreplace": null,
     "dependencies": [],
     "foldername": "aimandrange"
+  },
+  {
+    "name": "SimPoseEstimation",
+    "description": "Integrate 3D vision processing mode results into estimation of robot pose on the field. Includes simulation support.",
+    "tags": [],
+    "gradlebase": "java",
+    "language": "java",
+    "commandversion": 1,
+    "mainclass": "Main",
+    "packagetoreplace": null,
+    "dependencies": [],
+    "foldername": "simposeest"
   }
 ]

photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/Main.java

@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonlib.examples.simposeest;
+
+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.
+*/
+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.
+    */
+    public static void main(String... args) {
+        RobotBase.startRobot(Robot::new);
+    }
+}

photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/AutoController.java

@@ -0,0 +1,104 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonlib.examples.simposeest.robot;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+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.
+*/
+public class AutoController {
+
+    private Trajectory trajectory;
+
+    private RamseteController ramsete = new RamseteController();
+
+    private Timer timer = new Timer();
+
+    boolean isRunning = false;
+
+    Trajectory.State desiredDtState;
+
+    public AutoController() {
+
+        // Change this trajectory if you need the robot to follow different paths.
+        trajectory =
+                TrajectoryGenerator.generateTrajectory(
+                        new Pose2d(2, 2, new Rotation2d()),
+                        List.of(),
+                        new Pose2d(6, 4, new Rotation2d()),
+                        new TrajectoryConfig(2, 2));
+    }
+
+    /** @return The starting (initial) pose of the currently-active trajectory */
+    public Pose2d getInitialPose() {
+        return trajectory.getInitialPose();
+    }
+
+    /** Starts the controller running. Call this at the start of autonomous */
+    public void startPath() {
+        timer.reset();
+        timer.start();
+        isRunning = true;
+    }
+
+    /** Stops the controller from generating commands */
+    public void stopPath() {
+        isRunning = false;
+        timer.reset();
+    }
+
+    /**
+    * 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();
+            desiredDtState = trajectory.sample(elapsed);
+        } else {
+            desiredDtState = new Trajectory.State();
+        }
+
+        return ramsete.calculate(curEstPose, desiredDtState);
+    }
+
+    /**
+    * @return The position which the auto controller is attempting to move the drivetrain to right
+    *     now.
+    */
+    public Pose2d getCurPose2d() {
+        return desiredDtState.poseMeters;
+    }
+}

photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java

@@ -0,0 +1,96 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonlib.examples.simposeest.robot;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.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.
+*/
+public class Constants {
+
+    //////////////////////////////////////////////////////////////////
+    // Drivetrain Physical
+    //////////////////////////////////////////////////////////////////
+    public static final double kMaxSpeed = 3.0; // 3 meters per second.
+    public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second.
+
+    public static final double kTrackWidth = 0.381 * 2;
+    public static final double kWheelRadius = 0.0508;
+    public static final int kEncoderResolution = 4096;
+
+    //////////////////////////////////////////////////////////////////
+    // Electrical IO
+    //////////////////////////////////////////////////////////////////
+    public static final int kGyroPin = 0;
+
+    public static final int kDtLeftEncoderPinA = 0;
+    public static final int kDtLeftEncoderPinB = 1;
+    public static final int kDtRightEncoderPinA = 2;
+    public static final int kDtRightEncoderPinB = 3;
+
+    public static final int kDtLeftLeaderPin = 1;
+    public static final int kDtLeftFollowerPin = 2;
+    public static final int kDtRightLeaderPin = 3;
+    public static final int kDtRightFollowerPin = 4;
+
+    //////////////////////////////////////////////////////////////////
+    // Vision Processing
+    //////////////////////////////////////////////////////////////////
+    // Name configured in the PhotonVision GUI for the camera
+    public static final String kCamName = "mainCam";
+
+    // Physical location of the camera on the robot, relative to the center of the
+    // robot.
+    public static final Transform2d kCameraToRobot =
+            new Transform2d(
+                    new Translation2d(-0.25, 0), // in meters
+                    new Rotation2d());
+
+    // See
+    // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
+    // page 208
+    public static final double targetWidth =
+            Units.inchesToMeters(41.30) - Units.inchesToMeters(6.70); // meters
+
+    // See
+    // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
+    // page 197
+    public static final double targetHeight =
+            Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters
+    public static final double targetHeightAboveGround = Units.inchesToMeters(81.19); // meters
+
+    // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
+    // pages 4 and 5
+    public static final double kFarTgtXPos = Units.feetToMeters(54);
+    public static final double kFarTgtYPos =
+            Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0);
+    public static final Pose2d kFarTargetPose =
+            new Pose2d(new Translation2d(kFarTgtXPos, kFarTgtYPos), new Rotation2d(0.0));
+
+    public static final SimVisionTarget kFarTarget =
+            new SimVisionTarget(kFarTargetPose, targetHeightAboveGround, targetWidth, targetHeight);
+}

photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Drivetrain.java

@@ -0,0 +1,132 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonlib.examples.simposeest.robot;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+
+/**
+* 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);
+    PWMVictorSPX leftFollower = new PWMVictorSPX(Constants.kDtLeftFollowerPin);
+    PWMVictorSPX rightLeader = new PWMVictorSPX(Constants.kDtRightLeaderPin);
+    PWMVictorSPX rightFollower = new PWMVictorSPX(Constants.kDtRightFollowerPin);
+
+    SpeedControllerGroup leftGroup = new SpeedControllerGroup(leftLeader, leftFollower);
+    SpeedControllerGroup rightGroup = new SpeedControllerGroup(rightLeader, rightFollower);
+
+    // Drivetrain wheel speed sensors
+    // Used both for speed control and pose estimation.
+    Encoder leftEncoder = new Encoder(Constants.kDtLeftEncoderPinA, Constants.kDtLeftEncoderPinB);
+    Encoder rightEncoder = new Encoder(Constants.kDtRightEncoderPinA, Constants.kDtRightEncoderPinB);
+
+    // Drivetrain Pose Estimation
+    DrivetrainPoseEstimator poseEst = new DrivetrainPoseEstimator();
+
+    // Kinematics - defines the physical size and shape of the drivetrain, which is
+    // required to convert from
+    // chassis speed commands to wheel speed commands.
+    DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(Constants.kTrackWidth);
+
+    // Closed-loop PIDF controllers for servoing each side of the drivetrain to a
+    // specific speed.
+    // Gains are for example purposes only - must be determined for your own robot!
+    SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(1, 3);
+    PIDController leftPIDController = new PIDController(8.5, 0, 0);
+    PIDController rightPIDController = new PIDController(8.5, 0, 0);
+
+    public Drivetrain() {
+        // Set the distance per pulse for the drive encoders. We can simply use the
+        // distance traveled for one rotation of the wheel divided by the encoder
+        // resolution.
+        leftEncoder.setDistancePerPulse(
+                2 * Math.PI * Constants.kWheelRadius / Constants.kEncoderResolution);
+        rightEncoder.setDistancePerPulse(
+                2 * Math.PI * Constants.kWheelRadius / Constants.kEncoderResolution);
+
+        leftEncoder.reset();
+        rightEncoder.reset();
+
+        rightGroup.setInverted(true);
+    }
+
+    /**
+    * 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 =
+                kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot));
+
+        // Calculate the feedback (PID) portion of our motor command, based on desired
+        // wheel speed
+        var leftOutput = leftPIDController.calculate(leftEncoder.getRate(), speeds.leftMetersPerSecond);
+        var rightOutput =
+                rightPIDController.calculate(rightEncoder.getRate(), speeds.rightMetersPerSecond);
+
+        // Calculate the feedforward (F) portion of our motor command, based on desired
+        // wheel speed
+        var leftFeedforward = feedforward.calculate(speeds.leftMetersPerSecond);
+        var rightFeedforward = feedforward.calculate(speeds.rightMetersPerSecond);
+
+        // Update the motor controllers with our new motor commands
+        leftGroup.setVoltage(leftOutput + leftFeedforward);
+        rightGroup.setVoltage(rightOutput + rightFeedforward);
+
+        // Update the pose estimator with the most recent sensor readings.
+        poseEst.update(
+                new DifferentialDriveWheelSpeeds(leftEncoder.getRate(), rightEncoder.getRate()),
+                leftEncoder.getDistance(),
+                rightEncoder.getDistance());
+    }
+
+    /**
+    * 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();
+        poseEst.resetToPose(pose);
+    }
+
+    /** @return The current best-guess at drivetrain Pose on the field. */
+    public Pose2d getCtrlsPoseEstimate() {
+        return poseEst.getPoseEst();
+    }
+}

photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java

@@ -0,0 +1,106 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonlib.examples.simposeest.robot;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.estimator.DifferentialDrivePoseEstimator;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N3;
+import edu.wpi.first.wpiutil.math.numbers.N5;
+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.
+*/
+public class DrivetrainPoseEstimator {
+
+    // Sensors used as part of the Pose Estimation
+    private final AnalogGyro gyro = new AnalogGyro(Constants.kGyroPin);
+    private PhotonCamera cam = new PhotonCamera(Constants.kCamName);
+    // Note - drivetrain encoders are also used. The Drivetrain class must pass us
+    // the relevant readings.
+
+    // Kalman Filter Configuration. These can be "tuned-to-taste" based on how much
+    // you trust your
+    // various sensors. Smaller numbers will cause the filter to "trust" the
+    // estimate from that particular
+    // component more than the others. This in turn means the particualr component
+    // will have a stronger
+    // influence on the final pose estimate.
+    Matrix<N5, N1> stateStdDevs = VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.05, 0.05);
+    Matrix<N3, N1> localMeasurementStdDevs = VecBuilder.fill(0.01, 0.01, Units.degreesToRadians(0.1));
+    Matrix<N3, N1> visionMeasurementStdDevs =
+            VecBuilder.fill(0.01, 0.01, Units.degreesToRadians(0.1));
+
+    private final DifferentialDrivePoseEstimator m_poseEstimator =
+            new DifferentialDrivePoseEstimator(
+                    gyro.getRotation2d(),
+                    new Pose2d(),
+                    stateStdDevs,
+                    localMeasurementStdDevs,
+                    visionMeasurementStdDevs);
+
+    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
+    */
+    public void update(
+            DifferentialDriveWheelSpeeds actWheelSpeeds, double leftDist, double rightDist) {
+
+        m_poseEstimator.update(gyro.getRotation2d(), actWheelSpeeds, leftDist, rightDist);
+
+        var res = cam.getLatestResult();
+        if (res.hasTargets()) {
+            double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis();
+            Transform2d camToTargetTrans = res.getBestTarget().getCameraToTarget();
+            Pose2d camPose = Constants.kFarTargetPose.transformBy(camToTargetTrans.inverse());
+            m_poseEstimator.addVisionMeasurement(
+                    camPose.transformBy(Constants.kCameraToRobot), imageCaptureTime);
+        }
+    }
+
+    /**
+    * 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());
+    }
+
+    /** @return The current best-guess at drivetrain position on the field. */
+    public Pose2d getPoseEst() {
+        return m_poseEstimator.getEstimatedPosition();
+    }
+}

photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/OperatorInterface.java

@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonlib.examples.simposeest.robot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.SlewRateLimiter;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class OperatorInterface {
+    private XboxController opCtrl = new XboxController(0);
+
+    // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+    // to 1.
+    private SlewRateLimiter speedLimiter = new SlewRateLimiter(3);
+    private SlewRateLimiter rotLimiter = new SlewRateLimiter(3);
+
+    public OperatorInterface() {}
+
+    public double getFwdRevSpdCmd() {
+        // Get the x speed. We are inverting this because Xbox controllers return
+        // negative values when we push forward.
+        return -speedLimiter.calculate(opCtrl.getY(GenericHID.Hand.kLeft)) * Constants.kMaxSpeed;
+    }
+
+    public double getRotateSpdCmd() {
+        // Get the rate of angular rotation. We are inverting this because we want a
+        // positive value when we pull to the left (remember, CCW is positive in
+        // mathematics). Xbox controllers return positive values when you pull to
+        // the right by default.
+        return -rotLimiter.calculate(opCtrl.getX(GenericHID.Hand.kRight)) * Constants.kMaxAngularSpeed;
+    }
+
+    public boolean getSimKickCmd() {
+        return opCtrl.getXButtonPressed();
+    }
+}

photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/PoseTelemetry.java

@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonlib.examples.simposeest.robot;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/** Reports our expected, desired, and actual poses to dashboards */
+public class PoseTelemetry {
+
+    Field2d field = new Field2d();
+
+    Pose2d actPose = new Pose2d();
+    Pose2d desPose = new Pose2d();
+    Pose2d estPose = new Pose2d();
+
+    public PoseTelemetry() {
+        SmartDashboard.putData("Field", field);
+        update();
+    }
+
+    public void update() {
+        field.getObject("DesPose").setPose(desPose);
+        field.getObject("ActPose").setPose(actPose);
+        field.getObject("Robot").setPose(estPose);
+    }
+
+    public void setActualPose(Pose2d in) {
+        actPose = in;
+    }
+
+    public void setDesiredPose(Pose2d in) {
+        desPose = in;
+    }
+
+    public void setEstimatedPose(Pose2d in) {
+        estPose = in;
+    }
+}

photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Robot.java

@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonlib.examples.simposeest.robot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import org.photonlib.examples.simposeest.sim.DrivetrainSim;
+
+public class Robot extends TimedRobot {
+
+    AutoController autoCtrl = new AutoController();
+    Drivetrain dt = new Drivetrain();
+    OperatorInterface opInf = new OperatorInterface();
+
+    DrivetrainSim dtSim = new DrivetrainSim();
+
+    PoseTelemetry pt = new PoseTelemetry();
+
+    @Override
+    public void robotInit() {
+        // Flush NetworkTables every loop. This ensures that robot pose and other values
+        // are sent during every iteration.
+        setNetworkTablesFlushEnabled(true);
+    }
+
+    @Override
+    public void autonomousInit() {
+        resetOdometery();
+        autoCtrl.startPath();
+    }
+
+    @Override
+    public void autonomousPeriodic() {
+        ChassisSpeeds speeds = autoCtrl.getCurMotorCmds(dt.getCtrlsPoseEstimate());
+        dt.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond);
+        pt.setDesiredPose(autoCtrl.getCurPose2d());
+    }
+
+    @Override
+    public void teleopPeriodic() {
+        dt.drive(opInf.getFwdRevSpdCmd(), opInf.getRotateSpdCmd());
+    }
+
+    @Override
+    public void robotPeriodic() {
+        pt.setEstimatedPose(dt.getCtrlsPoseEstimate());
+        pt.update();
+    }
+
+    @Override
+    public void disabledPeriodic() {
+        dt.drive(0, 0);
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        if (opInf.getSimKickCmd()) {
+            dtSim.applyKick();
+        }
+        dtSim.update();
+        pt.setActualPose(dtSim.getCurPose());
+    }
+
+    private void resetOdometery() {
+        Pose2d startPose = autoCtrl.getInitialPose();
+        dtSim.resetPose(startPose);
+        dt.resetOdometry(startPose);
+    }
+}

photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java

@@ -0,0 +1,168 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonlib.examples.simposeest.sim;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
+import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.PWMSim;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+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.
+*/
+public class DrivetrainSim {
+
+    // Simulated Sensors
+    AnalogGyroSim gyroSim = new AnalogGyroSim(Constants.kGyroPin);
+    EncoderSim leftEncoderSim = EncoderSim.createForChannel(Constants.kDtLeftEncoderPinA);
+    EncoderSim rightEncoderSim = EncoderSim.createForChannel(Constants.kDtRightEncoderPinA);
+
+    // Simulated Motor Controllers
+    PWMSim leftLeader = new PWMSim(Constants.kDtLeftLeaderPin);
+    PWMSim leftFollower = new PWMSim(Constants.kDtLeftFollowerPin);
+    PWMSim rightLeader = new PWMSim(Constants.kDtRightLeaderPin);
+    PWMSim rightFollower = new PWMSim(Constants.kDtRightFollowerPin);
+
+    // Simulation Physics
+    // Configure these to match your drivetrain's physical dimensions
+    // and characterization results.
+    LinearSystem<N2, N2, N2> drivetrainSystem =
+            LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
+    DifferentialDrivetrainSim drivetrainSimulator =
+            new DifferentialDrivetrainSim(
+                    drivetrainSystem,
+                    DCMotor.getCIM(2),
+                    8,
+                    Constants.kTrackWidth,
+                    Constants.kWheelRadius,
+                    null);
+
+    // Simulated Vision System.
+    // Configure these to match your PhotonVision Camera,
+    // pipeline, and LED setup.
+    double camDiagFOV = 75.0; // degrees
+    double camPitch = 15.0; // degrees
+    double camHeightOffGround = 0.85; // meters
+    double maxLEDRange = 20; // meters
+    int camResolutionWidth = 640; // pixels
+    int camResolutionHeight = 480; // pixels
+    double minTargetArea = 10; // square pixels
+
+    SimVisionSystem simVision =
+            new SimVisionSystem(
+                    Constants.kCamName,
+                    camDiagFOV,
+                    camPitch,
+                    Constants.kCameraToRobot,
+                    camHeightOffGround,
+                    maxLEDRange,
+                    camResolutionWidth,
+                    camResolutionHeight,
+                    minTargetArea);
+
+    public DrivetrainSim() {
+        simVision.addSimVisionTarget(Constants.kFarTarget);
+    }
+
+    /**
+    * 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;
+
+        if (DriverStation.getInstance().isEnabled() && !RobotController.isBrownedOut()) {
+            // If the motor controllers are enabled...
+            // Roughly model the effect of leader and follower motor pushing on the same
+            // gearbox.
+            // Note if the software is incorrect and drives them against each other, speed
+            // will be
+            // roughly matching the physical situation, but current draw will _not_ be
+            // accurate.
+            leftMotorCmd = (leftLeader.getSpeed() + leftFollower.getSpeed()) / 2.0;
+            rightMotorCmd = (rightLeader.getSpeed() + rightFollower.getSpeed()) / 2.0;
+        }
+
+        // Update the physics simulation
+        drivetrainSimulator.setInputs(
+                leftMotorCmd * RobotController.getInputVoltage(),
+                -rightMotorCmd * RobotController.getInputVoltage());
+        drivetrainSimulator.update(0.02);
+
+        // Update our sensors based on the simulated motion of the robot
+        leftEncoderSim.setDistance((drivetrainSimulator.getLeftPositionMeters()));
+        leftEncoderSim.setRate((drivetrainSimulator.getLeftVelocityMetersPerSecond()));
+        rightEncoderSim.setDistance((drivetrainSimulator.getRightPositionMeters()));
+        rightEncoderSim.setRate((drivetrainSimulator.getRightVelocityMetersPerSecond()));
+        gyroSim.setAngle(
+                -drivetrainSimulator
+                        .getHeading()
+                        .getDegrees()); // Gyros have an inverted reference frame for
+        // angles, so multiply by -1.0;
+
+        // Update PhotonVision based on our new robot position.
+        simVision.processFrame(drivetrainSimulator.getPose());
+    }
+
+    /**
+    * 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);
+    }
+
+    /** @return The simulated robot's position, in meters. */
+    public Pose2d getCurPose() {
+        return drivetrainSimulator.getPose();
+    }
+
+    /**
+    * 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
+                        .getPose()
+                        .transformBy(new Transform2d(new Translation2d(0, 0.5), new Rotation2d()));
+        drivetrainSimulator.setPose(newPose);
+    }
+}