Package com.lightningrobotics.common.controller

Class DiffDriveController

  • public class DiffDriveController
extends java.lang.Object
    Ramsete is a nonlinear time-varying feedback controller for unicycle models that drives the model to a desired pose along a two-dimensional trajectory. Why would we need a nonlinear control law in addition to the linear ones we have used so far like PID? If we use the original approach with PID controllers for left and right position and velocity states, the controllers only deal with the local pose. If the robot deviates from the path, there is no way for the controllers to correct and the robot may not reach the desired global pose. This is due to multiple endpoints existing for the robot which have the same encoder path arc lengths.

    Instead of using wheel path arc lengths (which are in the robot's local coordinate frame), nonlinear controllers like pure pursuit and Ramsete use global pose. The controller uses this extra information to guide a linear reference tracker like the PID controllers back in by adjusting the references of the PID controllers.

    The paper "Control of Wheeled Mobile Robots: An Experimental Overview" describes a nonlinear controller for a wheeled vehicle with unicycle-like kinematics; a global pose consisting of x, y, and theta; and a desired pose consisting of x_d, y_d, and theta_d. We call it Ramsete because that's the acronym for the title of the book it came from in Italian ("Robotica Articolata e Mobile per i SErvizi e le TEcnologie").

    See Controls Engineering in the FIRST Robotics Competition section on Ramsete unicycle controller for a derivation and analysis.

    • Constructor Summary

      Constructors 
      Constructor Description
      DiffDriveController()
      Construct a Ramsete unicycle controller.
      DiffDriveController​(double b, double zeta)
      Construct a Ramsete unicycle controller.

    • Method Summary

      All Methods Instance Methods Concrete Methods 
      Modifier and Type Method Description
      boolean atReference()
      Returns true if the pose error is within tolerance of the reference.
      DrivetrainSpeed calculate​(edu.wpi.first.math.geometry.Pose2d currentPose, TrajectoryState desiredState)
      Returns the next output of the Ramsete controller.
      DrivetrainSpeed calculate​(edu.wpi.first.math.geometry.Pose2d currentPose, edu.wpi.first.math.geometry.Pose2d poseRef, double linearVelocityRefMeters, double angularVelocityRefRadiansPerSecond)
      Returns the next output of the Ramsete controller.
      void setEnabled​(boolean enabled)
      Enables and disables the controller for troubleshooting purposes.
      void setTolerance​(edu.wpi.first.math.geometry.Pose2d poseTolerance)
      Sets the pose error which is considered tolerable for use with atReference().

      • Methods inherited from class java.lang.Object

        clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

    • Constructor Detail

      • DiffDriveController

        public DiffDriveController​(double b,
                           double zeta)
        Construct a Ramsete unicycle controller.
        Parameters:
        b - Tuning parameter (b > 0 rad²/m²) for which larger values make convergence more aggressive like a proportional term.
        zeta - Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger values provide more damping in response.

      • DiffDriveController

        public DiffDriveController()
        Construct a Ramsete unicycle controller. The default arguments for b and zeta of 2.0 rad²/m² and 0.7 rad⁻¹ have been well-tested to produce desirable results.

    • Method Detail

      • atReference

        public boolean atReference()
        Returns true if the pose error is within tolerance of the reference.
        Returns:
        True if the pose error is within tolerance of the reference.

      • setTolerance

        public void setTolerance​(edu.wpi.first.math.geometry.Pose2d poseTolerance)
        Sets the pose error which is considered tolerable for use with atReference().
        Parameters:
        poseTolerance - Pose error which is tolerable.

      • calculate

        public DrivetrainSpeed calculate​(edu.wpi.first.math.geometry.Pose2d currentPose,
                                 edu.wpi.first.math.geometry.Pose2d poseRef,
                                 double linearVelocityRefMeters,
                                 double angularVelocityRefRadiansPerSecond)
        Returns the next output of the Ramsete controller.

        The reference pose, linear velocity, and angular velocity should come from a drivetrain trajectory.

        Parameters:
        currentPose - The current pose.
        poseRef - The desired pose.
        linearVelocityRefMeters - The desired linear velocity in meters per second.
        angularVelocityRefRadiansPerSecond - The desired angular velocity in radians per second.
        Returns:
        The next controller output.

      • calculate

        public DrivetrainSpeed calculate​(edu.wpi.first.math.geometry.Pose2d currentPose,
                                 TrajectoryState desiredState)
        Returns the next output of the Ramsete controller.

        The reference pose, linear velocity, and angular velocity should come from a drivetrain trajectory.

        Parameters:
        currentPose - The current pose.
        desiredState - The desired pose, linear velocity, and angular velocity from a trajectory.
        Returns:
        The next controller output.

      • setEnabled

        public void setEnabled​(boolean enabled)
        Enables and disables the controller for troubleshooting purposes.
        Parameters:
        enabled - If the controller is enabled or not.