Skip to content

Commit

Permalink
Merge branch 'master' into 2024
Browse files Browse the repository at this point in the history
  • Loading branch information
mcm001 authored Oct 15, 2023
2 parents d7411f7 + 82e3da6 commit 5cb9e85
Show file tree
Hide file tree
Showing 4 changed files with 259 additions and 120 deletions.
Original file line number Diff line number Diff line change
@@ -0,0 +1,91 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/

package frc.robot;

import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.util.Units;

public class Constants {
// ---------- Vision
// Constants about how your camera is mounted to the robot
public static final double CAMERA_PITCH_RADIANS =
Units.degreesToRadians(15); // Angle "up" from horizontal
public static final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); // Height above floor

// How far from the target we want to be
public static final double GOAL_RANGE_METERS = Units.feetToMeters(10);

// Where the 2020 High goal target is located on the field
// See
// https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system
// and https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
// (pages 4 and 5)
public static final Pose3d TARGET_POSE =
new Pose3d(
new Translation3d(
Units.feetToMeters(52.46),
Units.inchesToMeters(94.66),
Units.inchesToMeters(89.69)), // (center of vision target)
new Rotation3d(0.0, 0.0, Math.PI));
// ----------

// ---------- Drivetrain
public static final int LEFT_MOTOR_CHANNEL = 0;
public static final int RIGHT_MOTOR_CHANNEL = 1;

// PID constants should be tuned per robot
public static final double LINEAR_P = 0.5;
public static final double LINEAR_I = 0;
public static final double LINEAR_D = 0.1;

public static final double ANGULAR_P = 0.03;
public static final double ANGULAR_I = 0;
public static final double ANGULAR_D = 0.003;

// Ratio to multiply joystick inputs by
public static final double DRIVESPEED = 0.75;

// The following properties are necessary for simulation:

// Distance from drivetrain left wheels to right wheels
public static final double TRACKWIDTH_METERS = Units.feetToMeters(2.0);
public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(6.0);

// The motors used in the gearbox for one drivetrain side
public static final DCMotor DRIVE_MOTORS = DCMotor.getCIM(2);
// The gearbox reduction, or how many motor rotations per wheel rotation
public static final double GEARING = 8.0;

// The drivetrain feedforward values
public static final double LINEAR_KV = 2.0;
public static final double LINEAR_KA = 0.5;

public static final double ANGULAR_KV = 2.25;
public static final double ANGULAR_KA = 0.3;
// ----------
}
Original file line number Diff line number Diff line change
Expand Up @@ -24,13 +24,16 @@

package frc.robot;

import static frc.robot.Constants.*;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
import frc.robot.sim.DrivetrainSim;
import frc.robot.sim.VisionSim;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonUtils;

Expand All @@ -41,34 +44,18 @@
* project.
*/
public class Robot extends TimedRobot {
// 2020 High goal target height above ground
public static final double TARGET_HEIGHT_METERS = Units.inchesToMeters(81.19);

// Constants about how your camera is mounted to the robot
public static final double CAMERA_PITCH_RADIANS =
Units.degreesToRadians(15); // Angle "up" from horizontal
public static 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);

// 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.5;
final double LINEAR_D = 0.1;
PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);

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);

// Drive motors
PWMVictorSPX leftMotor = new PWMVictorSPX(0);
PWMVictorSPX rightMotor = new PWMVictorSPX(1);
PWMVictorSPX leftMotor = new PWMVictorSPX(LEFT_MOTOR_CHANNEL);
PWMVictorSPX rightMotor = new PWMVictorSPX(RIGHT_MOTOR_CHANNEL);
DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);

@Override
Expand All @@ -92,7 +79,7 @@ public void teleopPeriodic() {
double range =
PhotonUtils.calculateDistanceToTargetMeters(
CAMERA_HEIGHT_METERS,
TARGET_HEIGHT_METERS,
TARGET_POSE.getZ(),
CAMERA_PITCH_RADIANS,
Units.degreesToRadians(result.getBestTarget().getPitch()));

Expand All @@ -110,8 +97,8 @@ public void teleopPeriodic() {
}
} else {
// Manual Driver Mode
forwardSpeed = -xboxController.getLeftY() * 0.75;
rotationSpeed = -xboxController.getRightX() * 0.75;
forwardSpeed = -xboxController.getLeftY() * DRIVESPEED;
rotationSpeed = -xboxController.getRightX() * DRIVESPEED;
}

// Use our forward/turn speeds to control the drivetrain
Expand All @@ -122,14 +109,17 @@ public void teleopPeriodic() {
// Simulation support

DrivetrainSim dtSim;
VisionSim visionSim;

@Override
public void simulationInit() {
dtSim = new DrivetrainSim(leftMotor.getChannel(), rightMotor.getChannel(), camera);
dtSim = new DrivetrainSim(leftMotor, rightMotor);
visionSim = new VisionSim("main", camera);
}

@Override
public void simulationPeriodic() {
dtSim.update();
visionSim.update(dtSim.getPose());
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -24,119 +24,49 @@

package frc.robot.sim;

import static frc.robot.Constants.*;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.simulation.PWMSim;
import frc.robot.Robot;
import java.util.List;
import org.photonvision.PhotonCamera;
import org.photonvision.estimation.TargetModel;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
import org.photonvision.simulation.VisionTargetSim;

/**
* Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated
* PhotonVision system and one vision target.
* This class handles the simulation of robot physics, sensors, and motor controllers.
*
* <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;
PWMSim rightLeader;

// Simulation Physics
// Configure these to match your drivetrain's physical dimensions
// and characterization results.
double trackwidthMeters = Units.feetToMeters(2.0);
LinearSystem<N2, N2, N2> drivetrainSystem =
LinearSystemId.identifyDrivetrainSystem(2.0, 0.5, 2.25, 0.3, trackwidthMeters);
DifferentialDrivetrainSim drivetrainSimulator =
// ----- Simulation specific constants
// Drivetrain plant and simulation. This will calculate how the robot pose changes over time as
// motor voltages are applied.
private static final LinearSystem<N2, N2, N2> drivetrainSystem =
LinearSystemId.identifyDrivetrainSystem(
LINEAR_KV, LINEAR_KA, ANGULAR_KV, ANGULAR_KA, TRACKWIDTH_METERS);
private static final DifferentialDrivetrainSim drivetrainSimulator =
new DifferentialDrivetrainSim(
drivetrainSystem,
DCMotor.getCIM(2),
8,
trackwidthMeters,
Units.inchesToMeters(6.0 / 2.0),
DRIVE_MOTORS,
GEARING,
TRACKWIDTH_METERS,
WHEEL_DIAMETER_METERS / 2.0,
null);
// -----

// Simulated Vision System.
// Configure these to match your PhotonVision Camera,
// pipeline, and LED setup.
double camDiagFOV = 100.0; // degrees
double camPitch = Robot.CAMERA_PITCH_RADIANS; // degrees
double camHeightOffGround = Robot.CAMERA_HEIGHT_METERS; // meters
double minTargetArea = 0.1; // percentage (0 - 100)
double maxLEDRange = 20; // meters
int camResolutionWidth = 640; // pixels
int camResolutionHeight = 480; // pixels
PhotonCameraSim cameraSim;

VisionSystemSim visionSim = new VisionSystemSim("main");

// See
// https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
// page 208
TargetModel targetModel =
new TargetModel(
List.of(
new Translation3d(0, Units.inchesToMeters(-9.819867), Units.inchesToMeters(-8.5)),
new Translation3d(0, Units.inchesToMeters(9.819867), Units.inchesToMeters(-8.5)),
new Translation3d(0, Units.inchesToMeters(19.625), Units.inchesToMeters(8.5)),
new Translation3d(0, Units.inchesToMeters(-19.625), Units.inchesToMeters(8.5))));
// 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);
Pose3d farTargetPose =
new Pose3d(
new Translation3d(tgtXPos, tgtYPos, Robot.TARGET_HEIGHT_METERS),
new Rotation3d(0.0, 0.0, Math.PI));

public DrivetrainSim(int leftMotorPort, int rightMotorPort, PhotonCamera camera) {
leftLeader = new PWMSim(leftMotorPort);
rightLeader = new PWMSim(rightMotorPort);
// PWM handles for getting commanded motor controller speeds
private final PWMSim leftLeader;
private final PWMSim rightLeader;

// Make the vision target visible to this simulated field.
var visionTarget = new VisionTargetSim(farTargetPose, targetModel);
visionSim.addVisionTargets(visionTarget);

// Create simulated camera properties. These can be set to mimic your actual camera.
var cameraProp = new SimCameraProperties();
cameraProp.setCalibration(
camResolutionWidth, camResolutionHeight, Rotation2d.fromDegrees(camDiagFOV));
cameraProp.setCalibError(0.2, 0.05);
cameraProp.setFPS(25);
cameraProp.setAvgLatencyMs(30);
cameraProp.setLatencyStdDevMs(4);
// Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible
// targets.
cameraSim = new PhotonCameraSim(camera, cameraProp, minTargetArea, maxLEDRange);

// Add the simulated camera to view the targets on this simulated field.
visionSim.addCamera(
cameraSim,
new Transform3d(
new Translation3d(0.25, 0, Robot.CAMERA_HEIGHT_METERS),
new Rotation3d(0, -Robot.CAMERA_PITCH_RADIANS, 0)));

cameraSim.enableDrawWireframe(true);
public DrivetrainSim(PWMMotorController leftController, PWMMotorController rightController) {
leftLeader = new PWMSim(leftController);
rightLeader = new PWMSim(rightController);
}

/**
Expand All @@ -155,20 +85,21 @@ public void update() {
drivetrainSimulator.setInputs(
leftMotorCmd * RobotController.getBatteryVoltage(),
-rightMotorCmd * RobotController.getBatteryVoltage());
drivetrainSimulator.update(0.02);

// Update PhotonVision based on our new robot position.
visionSim.update(drivetrainSimulator.getPose());
drivetrainSimulator.update(Robot.kDefaultPeriod);
}

public Pose2d getPose() {
return 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).
* 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 (e.g. at the start of each match).
*
* @param pose
*/
public void resetPose(Pose2d pose) {
drivetrainSimulator.setPose(pose);
visionSim.resetRobotPose(pose);
}
}
Loading

0 comments on commit 5cb9e85

Please sign in to comment.