Class SwerveController


  • public class SwerveController
    extends java.lang.Object
    The high-level API for controlling a swerve drive with this library.
    • Constructor Summary

      Constructors 
      Constructor Description
      SwerveController​(java.lang.String configPath)
      Constructs the SwerveController using the given toml config.
      SwerveController​(java.lang.String configPath, SwerveGyro gyro)
      Constructs the SwerveController using the given toml config.
    • Method Summary

      All Methods Instance Methods Concrete Methods 
      Modifier and Type Method Description
      void disableNetworkTablesPublishing()
      Disables pushing swerve data to NetworkTables.
      void enableNetworkTablesPublishing()
      Enables publishing swerve data to NetworkTables.
      java.util.Map<java.lang.Integer,​com.ctre.phoenix.CANifier> getCanifiers()
      Gets the CANifiers used by this swerve for sensing, if any.
      VelocityState getConstrainedVelocity()
      Gets the velocity state that is being commanded at this moment based on the various constraints around how quickly the robot can change it's motion.
      SwerveGyro getGyro()
      Gets the gyro being used by this swerve controller.
      double getMaxRotationSpeed()
      Gets the maximum rotation speed if the drive is doing nothing else.
      double getMaxTranslationSpeed()
      Gets the maximum translation speed if the drive is doing nothing else.
      OdomState getOdometry()
      Gets the odometry readings, which estimate the robot's position and velocity based on measured wheel speed and angle.
      VelocityState getTargetVelocity()
      Gets the velocity state that this controller is trying to achieve.
      void holdDirection()
      Sets the robot to stop moving and hold it's current swerve module positions.
      void setBrake()
      Sets all motors on the swerve drive to brake.
      void setCenterOfRotation​(double x, double y)
      Sets the point that the robot will rotate around, relative to the origin used to define the location of the swerve modules in the config.
      void setCoast()
      Sets all motors on the swerve drive to coast.
      void setGyroYaw​(double yaw)
      Offsets the yaw readings from the gyro so that the robot's current heading will be set to the given yaw.
      void setTranslationDirection​(double translationDirection, boolean fieldCentric)
      Sets the translation direction of the robot.
      void setVelocity​(double translationSpeed, double rotationVelocity)
      Sets the translation speed and rotation velocity of the robot.
      void setVelocity​(double translationDirection, double translationSpeed, double rotationVelocity, boolean fieldCentric)
      Sets the translation and rotation velocity of the robot.
      void update()
      Updates all controllers, data loggers, and anything else associated with this SwerveController.
      • Methods inherited from class java.lang.Object

        clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
    • Constructor Detail

      • SwerveController

        public SwerveController​(java.lang.String configPath)
        Constructs the SwerveController using the given toml config.
        Parameters:
        configPath - The file path of the toml config. It can be a relative path inside of the deploy directory or an absolute path.
      • SwerveController

        public SwerveController​(java.lang.String configPath,
                                SwerveGyro gyro)
        Constructs the SwerveController using the given toml config.
        Parameters:
        configPath - The file path of the toml config. It can be a relative path inside of the deploy directory or an absolute path.
        gyro - The gyro to use instead of instantiating one from the config.
    • Method Detail

      • update

        public void update()
        Updates all controllers, data loggers, and anything else associated with this SwerveController.
      • setVelocity

        public void setVelocity​(double translationDirection,
                                double translationSpeed,
                                double rotationVelocity,
                                boolean fieldCentric)
        Sets the translation and rotation velocity of the robot.
        Parameters:
        translationDirection - The direction for the robot to move, in degrees counterclockwise from forwards.
        translationSpeed - The speed for the robot to move, in feet per second.
        rotationVelocity - The velocity for the robot to spin about the the center of rotation, independent of translation, in degrees per second.
        fieldCentric - If true, the given translation direction will be interpretted relative to the gyro's zero point, which should be consistently facing away from the driver. If false, it will be interpretted relative to the front of the robot.
      • setVelocity

        public void setVelocity​(double translationSpeed,
                                double rotationVelocity)
        Sets the translation speed and rotation velocity of the robot. Does not modify translation direction or center of rotation.
        Parameters:
        translationSpeed - The speed for the robot to move, in feet per second.
        rotationVelocity - The velocity for the robot to spin about the the center of rotation, independent of translation, in degrees per second.
      • setTranslationDirection

        public void setTranslationDirection​(double translationDirection,
                                            boolean fieldCentric)
        Sets the translation direction of the robot.
        Parameters:
        translationDirection - The direction for the robot to move, in degrees counterclockwise from forwards.
        fieldCentric - If true, the given translation direction will be interpretted relative to the gyro's zero point, which should be consistently facing away from the driver. If false, it will be interpretted relative to the front of the robot.
      • setCenterOfRotation

        public void setCenterOfRotation​(double x,
                                        double y)
        Sets the point that the robot will rotate around, relative to the origin used to define the location of the swerve modules in the config. It does not need to be within the robot's frame perimeter. Positive x corresponds to robot forwards, positive y corresponds to robot left.
        Parameters:
        x - The x component, in feet.
        y - The y component, in feet.
      • holdDirection

        public void holdDirection()
        Sets the robot to stop moving and hold it's current swerve module positions.
      • setGyroYaw

        public void setGyroYaw​(double yaw)
        Offsets the yaw readings from the gyro so that the robot's current heading will be set to the given yaw.
        Parameters:
        yaw - The yaw to offset the gyro to.
      • getMaxTranslationSpeed

        public double getMaxTranslationSpeed()
        Gets the maximum translation speed if the drive is doing nothing else.
        Returns:
        The maximum translation speed, in feet per second.
      • getMaxRotationSpeed

        public double getMaxRotationSpeed()
        Gets the maximum rotation speed if the drive is doing nothing else.
        Returns:
        The maximum rotation speed, in feet per second.
      • setCoast

        public void setCoast()
        Sets all motors on the swerve drive to coast. This is not recommended during driving, and is only provided the for ease of moving the modules while disabled.
      • setBrake

        public void setBrake()
        Sets all motors on the swerve drive to brake. This is the default behavior.
      • getTargetVelocity

        public VelocityState getTargetVelocity()
        Gets the velocity state that this controller is trying to achieve.
        Returns:
        The target velocity state.
      • getConstrainedVelocity

        public VelocityState getConstrainedVelocity()
        Gets the velocity state that is being commanded at this moment based on the various constraints around how quickly the robot can change it's motion.
        Returns:
        The commanded velocity state after constraints have been applied.
      • getOdometry

        public OdomState getOdometry()
        Gets the odometry readings, which estimate the robot's position and velocity based on measured wheel speed and angle.
        Returns:
        The current odometry state.
      • getGyro

        public SwerveGyro getGyro()
        Gets the gyro being used by this swerve controller.
        Returns:
        The gyro object.
      • getCanifiers

        public java.util.Map<java.lang.Integer,​com.ctre.phoenix.CANifier> getCanifiers()
        Gets the CANifiers used by this swerve for sensing, if any.
        Returns:
        A mapping from can IDs to Canifiers with that ID.
      • enableNetworkTablesPublishing

        public void enableNetworkTablesPublishing()
        Enables publishing swerve data to NetworkTables.
      • disableNetworkTablesPublishing

        public void disableNetworkTablesPublishing()
        Disables pushing swerve data to NetworkTables.