Add quick-vision and ramsete-controller examples

Author: fletch3555

potentiometer-pid/robot.py

@@ -11,9 +11,8 @@
 
 class MyRobot(wpilib.TimedRobot):
     """
-    This is a sample program to demonstrate how to use a soft potentiometer and
-    a PID controller to reach and maintain position setpoints on an elevator
-    mechanism.
+    This is a sample program to demonstrate how to use a soft potentiometer and a PID controller to
+    reach and maintain position setpoints on an elevator mechanism.
     """
 
     kPotChannel = 1

quick-vision/robot.py

@@ -0,0 +1,24 @@
+#!/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
+from wpilib.cameraserver import CameraServer
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    Uses the CameraServer class to automatically capture video from a USB webcam and send it to the
+    FRC dashboard without doing any vision processing. This is the easiest way to get camera images
+    to the dashboard. Just add this to the robotInit() method in your program.
+    """
+
+    def robotInit(self):
+        CameraServer().launch()
+
+
+if __name__ == "__main__":
+    wpilib.run(MyRobot)

ramsete-controller/drivetrain.py

@@ -0,0 +1,118 @@
+#!/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.drive
+import wpimath.controller
+import wpimath.geometry
+import wpimath.kinematics
+import wpimath.units
+
+import math
+
+
+class Drivetrain:
+    """Represents a differential drive style drivetrain."""
+
+    kMaxSpeed = 3.0  # meters per second
+    kMaxAngularSpeed = 2 * math.pi  # one rotation per second
+
+    kTrackWidth = 0.381 * 2  # meters
+    kWheelRadius = 0.0508  # meters
+    kEncoderResolution = 4096  # counts per revolution
+
+    def __init__(self):
+        leftLeader = wpilib.PWMSparkMax(1)
+        leftFollower = wpilib.PWMSparkMax(2)
+        rightLeader = wpilib.PWMSparkMax(3)
+        rightFollower = wpilib.PWMSparkMax(4)
+
+        self.leftEncoder = wpilib.Encoder(0, 1)
+        self.rightEncoder = wpilib.Encoder(2, 3)
+
+        self.leftGroup = wpilib.MotorControllerGroup(leftLeader, leftFollower)
+        self.rightGroup = wpilib.MotorControllerGroup(rightLeader, rightFollower)
+
+        self.gyro = wpilib.AnalogGyro(0)
+
+        self.leftPIDController = wpimath.controller.PIDController(1.0, 0.0, 0.0)
+        self.rightPIDController = wpimath.controller.PIDController(1.0, 0.0, 0.0)
+
+        self.kinematics = wpimath.kinematics.DifferentialDriveKinematics(
+            self.kTrackWidth
+        )
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.feedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3)
+
+        self.gyro.reset()
+
+        # We need to invert one side of the drivetrain so that positive voltages
+        # result in both sides moving forward. Depending on how your robot's
+        # gearbox is constructed, you might have to invert the left side instead.
+        self.rightGroup.setInverted(True)
+
+        # 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.
+        self.leftEncoder.setDistancePerPulse(
+            2 * math.pi * self.kWheelRadius / self.kEncoderResolution
+        )
+        self.rightEncoder.setDistancePerPulse(
+            2 * math.pi * self.kWheelRadius / self.kEncoderResolution
+        )
+
+        self.leftEncoder.reset()
+        self.rightEncoder.reset()
+
+        self.odometry = wpimath.kinematics.DifferentialDriveOdometry(
+            self.gyro.getRotation2d(),
+            self.leftEncoder.getDistance(),
+            self.rightEncoder.getDistance(),
+        )
+
+    def setSpeeds(self, speeds: wpimath.kinematics.DifferentialDriveWheelSpeeds):
+        """Sets the desired wheel speeds."""
+        leftFeedforward = self.feedforward.calculate(speeds.left)
+        rightFeedforward = self.feedforward.calculate(speeds.right)
+
+        leftOutput = self.leftPIDController.calculate(
+            self.leftEncoder.getRate(), speeds.left
+        )
+        rightOutput = self.rightPIDController.calculate(
+            self.rightEncoder.getRate(), speeds.right
+        )
+
+        self.leftGroup.setVoltage(leftOutput + leftFeedforward)
+        self.rightGroup.setVoltage(rightOutput + rightFeedforward)
+
+    def drive(self, xSpeed, rot):
+        """Drives the robot with the given linear velocity and angular velocity."""
+        wheelSpeeds = self.kinematics.toWheelSpeeds(
+            wpimath.kinematics.ChassisSpeeds(xSpeed, 0, rot)
+        )
+        self.setSpeeds(wheelSpeeds)
+
+    def updateOdometry(self):
+        """Updates the field-relative position."""
+        self.odometry.update(
+            self.gyro.getRotation2d(),
+            self.leftEncoder.getDistance(),
+            self.rightEncoder.getDistance(),
+        )
+
+    def resetOdometry(self, pose: wpimath.geometry.Pose2d):
+        """Resets the field-relative position to a specific location."""
+        self.odometry.resetPosition(
+            self.gyro.getRotation2d(),
+            self.leftEncoder.getDistance(),
+            self.rightEncoder.getDistance(),
+            pose
+        )
+
+    def getPose(self) -> wpimath.geometry.Pose2d:
+        """Returns the pose of the robot."""
+        return self.odometry.getPose()

ramsete-controller/robot.py

@@ -0,0 +1,103 @@
+#!/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.controller
+import wpimath.filter
+import wpimath.geometry
+import wpimath.trajectory
+import wpimath.units
+
+from drivetrain import Drivetrain
+
+
+class MyRobot(wpilib.TimedRobot):
+    """
+    Uses the CameraServer class to automatically capture video from a USB webcam and send it to the
+    FRC dashboard without doing any vision processing. This is the easiest way to get camera images
+    to the dashboard. Just add this to the robotInit() method in your program.
+    """
+
+    def robotInit(self):
+        self.controller = wpilib.XboxController(0)
+        self.drive = Drivetrain()
+
+        # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+        self.speedLimiter = wpimath.filter.SlewRateLimiter(3)
+        self.rotLimiter = wpimath.filter.SlewRateLimiter(3)
+
+        # An example trajectory to follow during the autonomous period.
+        self.trajectory = wpimath.trajectory.TrajectoryGenerator.generateTrajectory(
+            wpimath.geometry.Pose2d(0, 0, wpimath.geometry.Rotation2d.fromDegrees(0)),
+            [
+                wpimath.geometry.Translation2d(1, 1),
+                wpimath.geometry.Translation2d(2, -1),
+            ],
+            wpimath.geometry.Pose2d(3, 0, wpimath.geometry.Rotation2d.fromDegrees(0)),
+            wpimath.trajectory.TrajectoryConfig(
+                wpimath.units.feetToMeters(3.0), wpimath.units.feetToMeters(3.0)
+            )
+        )
+
+        # The Ramsete Controller to follow the trajectory.
+        self.ramseteController = wpimath.controller.RamseteController()
+
+        # The timer to use during the autonomous period.
+        self.timer = wpilib.Timer()
+
+        # Create Field2d for robot and trajectory visualizations.
+        self.field = wpilib.Field2d()
+
+        # Create and push Field2d to SmartDashboard.
+        wpilib.SmartDashboard.putData(self.field)
+
+        # Push the trajectory to Field2d.
+        self.field.getObject("traj").setTrajectory(self.trajectory)
+
+    def autonomousInit(self):
+        # Initialize the timer.
+        self.timer = wpilib.Timer()
+        self.timer.start()
+
+        # Reset the drivetrain's odometry to the starting pose of the trajectory.
+        self.drive.resetOdometry(self.trajectory.initialPose())
+
+    def autonomousPeriodic(self):
+        # Update odometry.
+        self.drive.updateOdometry()
+
+        # Update robot position on Field2d.
+        self.field.setRobotPose(self.drive.getPose())
+
+        if self.timer.get() < self.trajectory.totalTime():
+            # Get the desired pose from the trajectory.
+            desiredPose = self.trajectory.sample(self.timer.get())
+
+            # Get the reference chassis speeds from the Ramsete controller.
+            refChassisSpeeds = self.ramseteController.calculate(self.drive.getPose(), desiredPose)
+
+            # Set the linear and angular speeds.
+            self.drive.drive(refChassisSpeeds.vx, refChassisSpeeds.omega)
+        else:
+            self.drive.drive(0, 0)
+
+    def teleopPeriodic(self):
+        # Get the x speed. We are inverting this because Xbox controllers return
+        # negative values when we push forward.
+        xSpeed = -self.speedLimiter.calculate(self.controller.getLeftY()) * Drivetrain.kMaxSpeed
+
+        # 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()) * Drivetrain.kMaxAngularSpeed
+
+        self.drive.drive(xSpeed, rot)
+
+
+if __name__ == "__main__":
+    wpilib.run(MyRobot)

relay/robot.py

@@ -10,30 +10,28 @@
 
 class MyRobot(wpilib.TimedRobot):
     """
-    This is a sample program which uses joystick buttons to control a relay.
-    A Relay (generally a spike) has two outputs, each of which can be at either
-    0V or 12V and so can be used for actions such as turning a motor off, full
-    forwards, or full reverse, and is generally used on the compressor. This
-    program uses two buttons on a joystick and each button corresponds to one
+    This is a sample program which uses joystick buttons to control a relay. A Relay (generally a
+    spike) has two outputs, each of which can be at either 0V or 12V and so can be used for actions
+    such as turning a motor off, full forwards, or full reverse, and is generally used on the
+    compressor. This program uses two buttons on a joystick and each button corresponds to one
     output; pressing the button sets the output to 12V and releasing sets it to 0V.
     """
 
+    kRelayForwardButton = 1
+    kRelayReverseButton = 2
+
     def robotInit(self):
         """Robot initialization function"""
 
+        self.stick = wpilib.Joystick(0)
         self.relay = wpilib.Relay(0)
 
-        self.joystick = wpilib.Joystick(0)
-
-        self.relayForwardButton = 1
-        self.relayReverseButton = 2
-
     def teleopPeriodic(self):
         # Retrieve the button values. GetRawButton will
         # return true if the button is pressed and false if not.
 
-        forward = self.joystick.getRawButton(self.relayForwardButton)
-        reverse = self.joystick.getRawButton(self.relayReverseButton)
+        forward = self.stick.getRawButton(self.kRelayForwardButton)
+        reverse = self.stick.getRawButton(self.kRelayReverseButton)
 
         ##
         # Depending on the button values, we want to use one of

run_tests.sh

@@ -38,6 +38,8 @@ BASE_TESTS="
   physics-mecanum/src
   physics-spi/src
   potentiometer-pid
+  quick-vision
+  ramsete-controller
   relay
   shuffleboard
   solenoid