From e1d5b4101b0a636e9bc971b2b46f5b4edb72167b Mon Sep 17 00:00:00 2001 From: Chris Date: Tue, 12 Jan 2021 09:10:39 -0600 Subject: [PATCH 1/2] Added sim support to aim and range --- .../.wpilib/wpilib_preferences.json | 2 +- java/aim-and-range/build.gradle | 2 +- .../src/main/java/frc/robot/Robot.java | 43 +++++-- .../java/frc/robot/sim/DrivetrainSim.java | 108 ++++++++++++++++++ java/aim-and-range/vendordeps/photonlib.json | 8 +- 5 files changed, 145 insertions(+), 18 deletions(-) create mode 100644 java/aim-and-range/src/main/java/frc/robot/sim/DrivetrainSim.java diff --git a/java/aim-and-range/.wpilib/wpilib_preferences.json b/java/aim-and-range/.wpilib/wpilib_preferences.json index 3c89444..77d205e 100644 --- a/java/aim-and-range/.wpilib/wpilib_preferences.json +++ b/java/aim-and-range/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2020", + "projectYear": "2021", "teamNumber": 1 } diff --git a/java/aim-and-range/build.gradle b/java/aim-and-range/build.gradle index 9720092..f2af187 100644 --- a/java/aim-and-range/build.gradle +++ b/java/aim-and-range/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2020.3.2" + id "edu.wpi.first.GradleRIO" version "2021.1.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/java/aim-and-range/src/main/java/frc/robot/Robot.java b/java/aim-and-range/src/main/java/frc/robot/Robot.java index cf67e22..5b7ba70 100644 --- a/java/aim-and-range/src/main/java/frc/robot/Robot.java +++ b/java/aim-and-range/src/main/java/frc/robot/Robot.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.util.Units; +import frc.robot.sim.DrivetrainSim; /** * The VM is configured to automatically run this class, and to call the @@ -37,24 +38,24 @@ */ 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); - final double TARGET_HEIGHT_METERS = Units.feetToMeters(5); + static public final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); + static public final double TARGET_HEIGHT_METERS = Units.feetToMeters(5); // Angle between horizontal and the camera. - final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0); + static public final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0); // How far from the target we want to be - final double GOAL_RANGE_METERS = Units.feetToMeters(3); + final double GOAL_RANGE_METERS = Units.feetToMeters(10); // Change this to match the name of your camera PhotonCamera camera = new PhotonCamera("photonvision"); // PID constants should be tuned per robot - final double LINEAR_P = 0.1; + final double LINEAR_P = 2.0; final double LINEAR_D = 0.0; PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D); - final double ANGULAR_P = 0.1; - final double ANGULAR_D = 0.0; + final double ANGULAR_P = 0.03; + final double ANGULAR_D = 0.003; PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D); XboxController xboxController = new XboxController(0); @@ -77,13 +78,15 @@ public void teleopPeriodic() { if (result.hasTargets()) { // First calculate range double range = PhotonUtils.calculateDistanceToTargetMeters(CAMERA_HEIGHT_METERS, TARGET_HEIGHT_METERS, - CAMERA_PITCH_RADIANS, result.getBestTarget().getPitch()); + CAMERA_PITCH_RADIANS, Units.degreesToRadians(result.getBestTarget().getPitch())); // Use this range as the measurement we give to the PID controller. - forwardSpeed = forwardController.calculate(range, GOAL_RANGE_METERS); + // -1.0 required to ensure positive PID controller effort _increases_ range + forwardSpeed = -1.0 * forwardController.calculate(range, GOAL_RANGE_METERS); // Also calculate angular power - rotationSpeed = turnController.calculate(result.getBestTarget().getYaw(), 0); + // -1.0 required to ensure positive PID controller effort _increases_ yaw + rotationSpeed = -1.0 * turnController.calculate(result.getBestTarget().getYaw(), 0); } else { // If we have no targets, stay still. forwardSpeed = 0; @@ -91,11 +94,27 @@ public void teleopPeriodic() { } } else { // Manual Driver Mode - forwardSpeed = xboxController.getY(GenericHID.Hand.kRight); - rotationSpeed = xboxController.getX(GenericHID.Hand.kLeft); + forwardSpeed = -1.0* xboxController.getY(GenericHID.Hand.kLeft); + rotationSpeed = xboxController.getX(GenericHID.Hand.kRight); } // Use our forward/turn speeds to control the drivetrain drive.arcadeDrive(forwardSpeed, rotationSpeed); } + + + //////////////////////////////////////////////////// + // Simulation support + + DrivetrainSim dtSim; + + @Override + public void simulationInit(){ + dtSim = new DrivetrainSim(); + } + + @Override + public void simulationPeriodic(){ + dtSim.update(); + } } diff --git a/java/aim-and-range/src/main/java/frc/robot/sim/DrivetrainSim.java b/java/aim-and-range/src/main/java/frc/robot/sim/DrivetrainSim.java new file mode 100644 index 0000000..3392fc8 --- /dev/null +++ b/java/aim-and-range/src/main/java/frc/robot/sim/DrivetrainSim.java @@ -0,0 +1,108 @@ +package frc.robot.sim; + +import org.photonvision.SimVisionSystem; +import org.photonvision.SimVisionTarget; + +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.smartdashboard.Field2d; +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.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.numbers.N2; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Robot; + +/** + * Implementation of a simulation of robot physics, sensors, motor controllers + * Includes a Simulated PhotonVision system and one vision target. + * + * 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); + PWMSim rightLeader = new PWMSim(1); + + // Simulation Physics + // Configure these to match your drivetrain's physical dimensions + // and characterization results. + LinearSystem drivetrainSystem = LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); + DifferentialDrivetrainSim drivetrainSimulator = new DifferentialDrivetrainSim(drivetrainSystem, DCMotor.getCIM(2), + 8, Units.feetToMeters(2.0), Units.inchesToMeters(6.0/2.0), null); + + // Simulated Vision System. + // Configure these to match your PhotonVision Camera, + // pipeline, and LED setup. + double camDiagFOV = 150.0; // degrees + double camPitch = Units.radiansToDegrees(Robot.CAMERA_PITCH_RADIANS); // degrees + double camHeightOffGround = Robot.CAMERA_HEIGHT_METERS; // meters + double maxLEDRange = 20; // meters + int camResolutionWidth = 640; // pixels + int camResolutionHeight = 480; // pixels + double minTargetArea = 10; // square pixels + + SimVisionSystem simVision = new SimVisionSystem("photonvision", camDiagFOV, camPitch, new Transform2d(), + camHeightOffGround, maxLEDRange, camResolutionWidth, camResolutionHeight, minTargetArea); + + double targetWidth = 0.54; // meters + double targetHeight = 0.25; // meters + Pose2d farTargetPose = new Pose2d( + new Translation2d(Units.feetToMeters(54), Units.feetToMeters(10)), new Rotation2d(0.0)); + + + Field2d field = new Field2d(); + + public DrivetrainSim() { + simVision.addSimVisionTarget(new SimVisionTarget(farTargetPose, Robot.TARGET_HEIGHT_METERS, targetWidth, targetHeight)); + SmartDashboard.putData("Field", field); + } + + /** + * 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()) { + leftMotorCmd = leftLeader.getSpeed(); + rightMotorCmd = rightLeader.getSpeed(); + } + + drivetrainSimulator.setInputs(leftMotorCmd * RobotController.getInputVoltage(), + -rightMotorCmd * RobotController.getInputVoltage()); + drivetrainSimulator.update(0.02); + + // Update PhotonVision based on our new robot position. + simVision.processFrame(drivetrainSimulator.getPose()); + + field.setRobotPose(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); + } + +} diff --git a/java/aim-and-range/vendordeps/photonlib.json b/java/aim-and-range/vendordeps/photonlib.json index d2427a8..662d44e 100644 --- a/java/aim-and-range/vendordeps/photonlib.json +++ b/java/aim-and-range/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "2020.0.11", + "version": "2021.1.2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", "mavenUrls": [ "https://maven.photonvision.org/repository/internal" @@ -12,7 +12,7 @@ { "groupId": "org.photonvision.lib", "artifactId": "PhotonLib-cpp", - "version": "2020.0.11", + "version": "2021.1.2", "libName": "Photon", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision.lib", "artifactId": "PhotonLib-java", - "version": "2020.0.11" + "version": "2021.1.2" } ] -} +} \ No newline at end of file From 869b00e5ce2cc2edcfa864a14601f302a0eb1abb Mon Sep 17 00:00:00 2001 From: Chris Date: Tue, 12 Jan 2021 10:56:08 -0600 Subject: [PATCH 2/2] Revised target constants to actually match the 2020/2021 field --- .../src/main/java/frc/robot/Robot.java | 12 +++++++----- .../src/main/java/frc/robot/sim/DrivetrainSim.java | 14 +++++++++----- 2 files changed, 16 insertions(+), 10 deletions(-) diff --git a/java/aim-and-range/src/main/java/frc/robot/Robot.java b/java/aim-and-range/src/main/java/frc/robot/Robot.java index 5b7ba70..b979f10 100644 --- a/java/aim-and-range/src/main/java/frc/robot/Robot.java +++ b/java/aim-and-range/src/main/java/frc/robot/Robot.java @@ -37,11 +37,13 @@ * project. */ public class Robot extends TimedRobot { - // Constants such as camera and target height stored. Change per robot and goal! - static public final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); - static public final double TARGET_HEIGHT_METERS = Units.feetToMeters(5); - // Angle between horizontal and the camera. - static public final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0); + + // 2020 High goal target height above ground + static public final double TARGET_HEIGHT_METERS = Units.inchesToMeters(81.19); + + // Constants about how your camera is mounted to the robot + static public final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(15); // Angle "up" from horizontal + static public final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); // Height above floor // How far from the target we want to be final double GOAL_RANGE_METERS = Units.feetToMeters(10); diff --git a/java/aim-and-range/src/main/java/frc/robot/sim/DrivetrainSim.java b/java/aim-and-range/src/main/java/frc/robot/sim/DrivetrainSim.java index 3392fc8..db56169 100644 --- a/java/aim-and-range/src/main/java/frc/robot/sim/DrivetrainSim.java +++ b/java/aim-and-range/src/main/java/frc/robot/sim/DrivetrainSim.java @@ -46,7 +46,7 @@ public class DrivetrainSim { // Simulated Vision System. // Configure these to match your PhotonVision Camera, // pipeline, and LED setup. - double camDiagFOV = 150.0; // degrees + double camDiagFOV = 170.0; // degrees - assume wide-angle camera double camPitch = Units.radiansToDegrees(Robot.CAMERA_PITCH_RADIANS); // degrees double camHeightOffGround = Robot.CAMERA_HEIGHT_METERS; // meters double maxLEDRange = 20; // meters @@ -57,10 +57,14 @@ public class DrivetrainSim { SimVisionSystem simVision = new SimVisionSystem("photonvision", camDiagFOV, camPitch, new Transform2d(), camHeightOffGround, maxLEDRange, camResolutionWidth, camResolutionHeight, minTargetArea); - double targetWidth = 0.54; // meters - double targetHeight = 0.25; // meters - Pose2d farTargetPose = new Pose2d( - new Translation2d(Units.feetToMeters(54), Units.feetToMeters(10)), new Rotation2d(0.0)); + // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf page 208 + 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 + double targetHeight = Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters + // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf pages 4 and 5 + double tgtXPos = Units.feetToMeters(54); + double tgtYPos = Units.feetToMeters(27/2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0/2.0) ; + Pose2d farTargetPose = new Pose2d(new Translation2d(tgtXPos, tgtYPos), new Rotation2d(0.0)); Field2d field = new Field2d();