Examples Clean-Up (#1408)

photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java

@@ -24,87 +24,121 @@
 
 package frc.robot;
 
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.util.Units;
+import static frc.robot.Constants.Vision.*;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.subsystems.drivetrain.SwerveDrive;
 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.
- */
 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);
-    // Angle between horizontal and the camera.
-    final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0);
+    private SwerveDrive drivetrain;
+    private VisionSim visionSim;
+    private PhotonCamera camera;
 
-    // How far from the target we want to be
-    final double GOAL_RANGE_METERS = Units.feetToMeters(3);
+    private final double VISION_TURN_kP = 0.01;
 
-    // Change this to match the name of your camera as shown in the web UI
-    PhotonCamera camera = new PhotonCamera("Microsoft_LifeCam_HD-3000");
+    private XboxController controller;
 
-    // PID constants should be tuned per robot
-    final double LINEAR_P = 0.1;
-    final double LINEAR_D = 0.0;
-    PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);
+    @Override
+    public void robotInit() {
+        drivetrain = new SwerveDrive();
+        camera = new PhotonCamera(kCameraName);
 
-    final double ANGULAR_P = 0.1;
-    final double ANGULAR_D = 0.0;
-    PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D);
+        visionSim = new VisionSim(camera);
 
-    XboxController xboxController = new XboxController(0);
-
-    // Drive motors
-    PWMVictorSPX leftMotor = new PWMVictorSPX(0);
-    PWMVictorSPX rightMotor = new PWMVictorSPX(1);
-    DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);
+        controller = new XboxController(0);
+    }
 
     @Override
     public void robotPeriodic() {
-        var start = Timer.getFPGATimestamp();
-        var res = camera.getLatestResult();
-        var end = Timer.getFPGATimestamp();
-        System.out.println(
-                "dt: " + (int) ((end - start) * 1e6) + "uS for targets: " + res.getTargets().size());
-        SmartDashboard.putNumber("decodeTime", (int) ((end - start) * 1e6));
+        // Update drivetrain subsystem
+        drivetrain.periodic();
+
+        // Log values to the dashboard
+        drivetrain.log();
+    }
+
+    @Override
+    public void disabledPeriodic() {
+        drivetrain.stop();
+    }
+
+    @Override
+    public void teleopInit() {
+        resetPose();
     }
 
     @Override
     public void teleopPeriodic() {
-        double forwardSpeed;
-        double rotationSpeed;
-
-        forwardSpeed = -xboxController.getRightY();
-
-        if (xboxController.getAButton()) {
-            // Vision-alignment mode
-            // Query the latest result from PhotonVision
-            var result = camera.getLatestResult();
+        // Calculate drivetrain commands from Joystick values
+        double forward = -controller.getLeftY() * Constants.Swerve.kMaxLinearSpeed;
+        double strafe = -controller.getLeftX() * Constants.Swerve.kMaxLinearSpeed;
+        double turn = -controller.getRightX() * Constants.Swerve.kMaxAngularSpeed;
 
+        // Read in relevant data from the Camera
+        boolean targetVisible = false;
+        double targetYaw = 0.0;
+        var results = camera.getAllUnreadResults();
+        if (!results.isEmpty()) {
+            // Camera processed a new frame since last
+            // Get the last one in the list.
+            var result = results.get(results.size() - 1);
             if (result.hasTargets()) {
-                // Calculate angular turn power
-                // -1.0 required to ensure positive PID controller effort _increases_ yaw
-                rotationSpeed = -turnController.calculate(result.getBestTarget().getYaw(), 0);
-            } else {
-                // If we have no targets, stay still.
-                rotationSpeed = 0;
+                // At least one AprilTag was seen by the camera
+                for (var target : result.getTargets()) {
+                    if (target.getFiducialId() == 7) {
+                        // Found Tag 7, record its information
+                        targetYaw = target.getYaw();
+                        targetVisible = true;
+                    }
+                }
             }
-        } else {
-            // Manual Driver Mode
-            rotationSpeed = xboxController.getLeftX();
         }
 
-        // Use our forward/turn speeds to control the drivetrain
-        drive.arcadeDrive(forwardSpeed, rotationSpeed);
+        // Auto-align when requested
+        if (controller.getAButton() && targetVisible) {
+            // Driver wants auto-alignment to tag 7
+            // And, tag 7 is in sight, so we can turn toward it.
+            // Override the driver's turn command with an automatic one that turns toward the tag.
+            turn = -1.0 * targetYaw * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed;
+        }
+
+        // Command drivetrain motors based on target speeds
+        drivetrain.drive(forward, strafe, turn);
+
+        // Put debug information to the dashboard
+        SmartDashboard.putBoolean("Vision Target Visible", targetVisible);
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        // Update drivetrain simulation
+        drivetrain.simulationPeriodic();
+
+        // Update camera simulation
+        visionSim.simulationPeriodic(drivetrain.getSimPose());
+
+        var debugField = visionSim.getSimDebugField();
+        debugField.getObject("EstimatedRobot").setPose(drivetrain.getPose());
+        debugField.getObject("EstimatedRobotModules").setPoses(drivetrain.getModulePoses());
+
+        // Calculate battery voltage sag due to current draw
+        RoboRioSim.setVInVoltage(
+                BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw()));
+    }
+
+    public void resetPose() {
+        // Example Only - startPose should be derived from some assumption
+        // of where your robot was placed on the field.
+        // The first pose in an autonomous path is often a good choice.
+        var startPose = new Pose2d(1, 1, new Rotation2d());
+        drivetrain.resetPose(startPose, true);
+        visionSim.resetSimPose(startPose);
     }
 }