[robotpy][examples] Split examples and snippets (#8944)

This also updates the bazel scripts to behave more like the C++ and Java
examples, and updates the copybara scripts to be able to sync up
`mostrobotpy`

robotpyExamples/examples/SwerveDrivePoseEstimator/drivetrain.py

@@ -0,0 +1,113 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import wpilib
+import wpimath
+import wpimath.units
+
+import swervemodule
+from exampleglobalmeasurementsensor import ExampleGlobalMeasurementSensor
+
+
+class Drivetrain:
+    """Represents a swerve drive style drivetrain."""
+
+    kMaxVelocity = 3.0  # 3 meters per second
+    kMaxAngularVelocity = math.pi  # 1/2 rotation per second
+
+    def __init__(self) -> None:
+        self.frontLeftLocation = wpimath.Translation2d(0.381, 0.381)
+        self.frontRightLocation = wpimath.Translation2d(0.381, -0.381)
+        self.backLeftLocation = wpimath.Translation2d(-0.381, 0.381)
+        self.backRightLocation = wpimath.Translation2d(-0.381, -0.381)
+
+        self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3)
+        self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7)
+        self.backLeft = swervemodule.SwerveModule(5, 6, 8, 9, 10, 11)
+        self.backRight = swervemodule.SwerveModule(7, 8, 12, 13, 14, 15)
+
+        self.imu = wpilib.OnboardIMU(wpilib.OnboardIMU.MountOrientation.FLAT)
+
+        self.kinematics = wpimath.SwerveDrive4Kinematics(
+            self.frontLeftLocation,
+            self.frontRightLocation,
+            self.backLeftLocation,
+            self.backRightLocation,
+        )
+
+        # Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers
+        # used below are robot specific, and should be tuned.
+        self.poseEstimator = wpimath.SwerveDrive4PoseEstimator(
+            self.kinematics,
+            self.imu.getRotation2d(),
+            (
+                self.frontLeft.getPosition(),
+                self.frontRight.getPosition(),
+                self.backLeft.getPosition(),
+                self.backRight.getPosition(),
+            ),
+            wpimath.Pose2d(),
+            (0.05, 0.05, wpimath.units.degreesToRadians(5)),
+            (0.5, 0.5, wpimath.units.degreesToRadians(30)),
+        )
+
+        self.imu.resetYaw()
+
+    def drive(
+        self,
+        xVelocity: float,
+        yVelocity: float,
+        rot: float,
+        fieldRelative: bool,
+        period: float,
+    ) -> None:
+        """Method to drive the robot using joystick info.
+
+        :param xVelocity: Velocity of the robot in the x direction (forward).
+        :param yVelocity: Velocity of the robot in the y direction (sideways).
+        :param rot: Angular rate of the robot.
+        :param fieldRelative: Whether the provided x and y velocities are relative to the field.
+        """
+        chassisVelocities = wpimath.ChassisVelocities(xVelocity, yVelocity, rot)
+        if fieldRelative:
+            chassisVelocities = chassisVelocities.toRobotRelative(
+                self.poseEstimator.getEstimatedPosition().rotation()
+            )
+
+        chassisVelocities = chassisVelocities.discretize(period)
+
+        velocities = wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(
+            self.kinematics.toSwerveModuleVelocities(chassisVelocities),
+            self.kMaxVelocity,
+        )
+
+        self.frontLeft.setDesiredVelocity(velocities[0])
+        self.frontRight.setDesiredVelocity(velocities[1])
+        self.backLeft.setDesiredVelocity(velocities[2])
+        self.backRight.setDesiredVelocity(velocities[3])
+
+    def updateOdometry(self) -> None:
+        """Updates the field relative position of the robot."""
+        self.poseEstimator.update(
+            self.imu.getRotation2d(),
+            (
+                self.frontLeft.getPosition(),
+                self.frontRight.getPosition(),
+                self.backLeft.getPosition(),
+                self.backRight.getPosition(),
+            ),
+        )
+
+        # Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
+        # a real robot, this must be calculated based either on latency or timestamps.
+        self.poseEstimator.addVisionMeasurement(
+            ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
+                self.poseEstimator.getEstimatedPosition()
+            ),
+            wpilib.Timer.getTimestamp() - 0.3,
+        )

robotpyExamples/examples/SwerveDrivePoseEstimator/exampleglobalmeasurementsensor.py

@@ -0,0 +1,33 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import random
+
+import wpimath
+
+
+class ExampleGlobalMeasurementSensor:
+    """This dummy class represents a global measurement sensor, such as a computer vision
+    solution.
+    """
+
+    def __init__(self) -> None:
+        raise RuntimeError("Utility class")
+
+    @staticmethod
+    def getEstimatedGlobalPose(estimatedRobotPose: wpimath.Pose2d) -> wpimath.Pose2d:
+        """Get a "noisy" fake global pose reading.
+
+        :param estimatedRobotPose: The robot pose.
+        """
+        rand_x = random.gauss(0.0, 0.5)
+        rand_y = random.gauss(0.0, 0.5)
+        rand_rot = random.gauss(0.0, wpimath.units.degreesToRadians(30))
+        return wpimath.Pose2d(
+            estimatedRobotPose.x + rand_x,
+            estimatedRobotPose.y + rand_y,
+            estimatedRobotPose.rotation().rotateBy(wpimath.Rotation2d(rand_rot)),
+        )

robotpyExamples/examples/SwerveDrivePoseEstimator/robot.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import wpilib
+import wpimath
+
+from drivetrain import Drivetrain
+
+
+class MyRobot(wpilib.TimedRobot):
+    def __init__(self) -> None:
+        super().__init__()
+
+        self.controller = wpilib.NiDsXboxController(0)
+        self.swerve = Drivetrain()
+
+        # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+        self.xvelocityLimiter = wpimath.SlewRateLimiter(3)
+        self.yvelocityLimiter = wpimath.SlewRateLimiter(3)
+        self.rotLimiter = wpimath.SlewRateLimiter(3)
+
+    def autonomousPeriodic(self) -> None:
+        self.driveWithJoystick(False)
+        self.swerve.updateOdometry()
+
+    def teleopPeriodic(self) -> None:
+        self.driveWithJoystick(True)
+
+    def driveWithJoystick(self, fieldRelative: bool) -> None:
+        # Get the x velocity. We are inverting this because Xbox controllers return
+        # negative values when we push forward.
+        xVelocity = -self.xvelocityLimiter.calculate(self.controller.getLeftY())
+        xVelocity *= Drivetrain.kMaxVelocity
+
+        # Get the y velocity or sideways/strafe velocity. We are inverting this because
+        # we want a positive value when we pull to the left. Xbox controllers
+        # return positive values when you pull to the right by default.
+        yVelocity = -self.yvelocityLimiter.calculate(self.controller.getLeftX())
+        yVelocity *= Drivetrain.kMaxVelocity
+
+        # 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.
+        rot = -self.rotLimiter.calculate(self.controller.getRightX())
+        rot *= Drivetrain.kMaxAngularVelocity
+
+        self.swerve.drive(xVelocity, yVelocity, rot, fieldRelative, self.getPeriod())

robotpyExamples/examples/SwerveDrivePoseEstimator/swervemodule.py

@@ -0,0 +1,128 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+
+import wpilib
+import wpimath
+
+kWheelRadius = 0.0508
+kEncoderResolution = 4096
+
+kModuleMaxAngularVelocity = math.pi
+kModuleMaxAngularAcceleration = math.tau  # radians per second squared
+
+
+class SwerveModule:
+    def __init__(
+        self,
+        driveMotorChannel: int,
+        turningMotorChannel: int,
+        driveEncoderChannelA: int,
+        driveEncoderChannelB: int,
+        turningEncoderChannelA: int,
+        turningEncoderChannelB: int,
+    ) -> None:
+        """Constructs a SwerveModule with a drive motor, turning motor, drive encoder and
+        turning encoder.
+
+        :param driveMotorChannel: PWM output for the drive motor.
+        :param turningMotorChannel: PWM output for the turning motor.
+        :param driveEncoderChannelA: DIO input for the drive encoder channel A
+        :param driveEncoderChannelB: DIO input for the drive encoder channel B
+        :param turningEncoderChannelA: DIO input for the turning encoder channel A
+        :param turningEncoderChannelB: DIO input for the turning encoder channel B
+        """
+        self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel)
+        self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel)
+
+        self.driveEncoder = wpilib.Encoder(driveEncoderChannelA, driveEncoderChannelB)
+        self.turningEncoder = wpilib.Encoder(
+            turningEncoderChannelA, turningEncoderChannelB
+        )
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.drivePIDController = wpimath.PIDController(1, 0, 0)
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.turningPIDController = wpimath.ProfiledPIDController(
+            1,
+            0,
+            0,
+            wpimath.TrapezoidProfile.Constraints(
+                kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration
+            ),
+        )
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.driveFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3)
+        self.turnFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 0.5)
+
+        # Set the distance per pulse for the drive encoder. We can simply use the
+        # distance traveled for one rotation of the wheel divided by the encoder
+        # resolution.
+        self.driveEncoder.setDistancePerPulse(
+            math.tau * kWheelRadius / kEncoderResolution
+        )
+
+        # Set the distance (in this case, angle) in radians per pulse for the turning encoder.
+        # This is the the angle through an entire rotation (2 * pi) divided by the
+        # encoder resolution.
+        self.turningEncoder.setDistancePerPulse(math.tau / kEncoderResolution)
+
+        # Limit the PID Controller's input range between -pi and pi and set the input
+        # to be continuous.
+        self.turningPIDController.enableContinuousInput(-math.pi, math.pi)
+
+    def getState(self) -> wpimath.SwerveModuleVelocity:
+        """Returns the current state of the module.
+
+        :returns: The current state of the module.
+        """
+        return wpimath.SwerveModuleVelocity(
+            self.driveEncoder.getRate(),
+            wpimath.Rotation2d(self.turningEncoder.getDistance()),
+        )
+
+    def getPosition(self) -> wpimath.SwerveModulePosition:
+        """Returns the current position of the module.
+
+        :returns: The current position of the module.
+        """
+        return wpimath.SwerveModulePosition(
+            self.driveEncoder.getDistance(),
+            wpimath.Rotation2d(self.turningEncoder.getDistance()),
+        )
+
+    def setDesiredVelocity(self, desiredVelocity: wpimath.SwerveModuleVelocity) -> None:
+        """Sets the desired state for the module.
+
+        :param desiredVelocity: Desired state with velocity and angle.
+        """
+        encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())
+
+        # Optimize the desired velocity to avoid spinning further than 90 degrees, then scale
+        # velocity by cosine of angle error. This scales down movement perpendicular to the desired
+        # direction of travel that can occur when modules change directions. This results in
+        # smoother driving.
+        velocity = desiredVelocity.optimize(encoderRotation).cosineScale(
+            encoderRotation
+        )
+
+        # Calculate the drive output from the drive PID controller and feedforward.
+        driveOutput = self.drivePIDController.calculate(
+            self.driveEncoder.getRate(), velocity.velocity
+        ) + self.driveFeedforward.calculate(velocity.velocity)
+
+        # Calculate the turning motor output from the turning PID controller and feedforward.
+        turnOutput = self.turningPIDController.calculate(
+            self.turningEncoder.getDistance(), velocity.angle.radians()
+        ) + self.turnFeedforward.calculate(
+            self.turningPIDController.getSetpoint().velocity
+        )
+
+        self.driveMotor.setVoltage(driveOutput)
+        self.turningMotor.setVoltage(turnOutput)