Valor 6800 1.0
Loading...
Searching...
No Matches
valor::Swerve Class Reference
Inheritance diagram for valor::Swerve:
valor::BaseSubsystem valor::Loggable

Public Member Functions

 Swerve (std::string name, std::vector< std::pair< BaseController *, BaseController * > > modules, units::meter_t module_radius, units::meter_t _wheelDiameter, std::vector< int > moduleCoordsX, std::vector< int > moduleCoordsY, units::meter_t driveBaseRadius)
 
void analyzeDashboard () override
 Synchronize dashboard data (both read and write)
 
void assignOutputs () override
 Read subsystem state and send motor commands.
 
void LoggablePeriodic () override
 Periodic callback for logging updates.
 
void reset () override
 Reset all subsystem state.
 
void SimulationPeriodic () override
 
frc::Pose2d getRawPose ()
 
frc::Pose2d getCalculatedPose ()
 
void setupGyro (int, const char *, units::degree_t, units::degree_t, units::degree_t)
 
void resetGyro ()
 
frc::Rotation2d getGyro ()
 
void resetOdometry (frc::Pose2d pose)
 
void resetEncoders ()
 
wpi::array< frc::SwerveModulePosition, MODULE_COUNT > getModulePositions ()
 Retrieves the current position of each swerve module.
 
wpi::array< frc::SwerveModuleState, MODULE_COUNT > getModuleStates ()
 Retrieves the current state (speed and angle) of each swerve module.
 
wpi::array< frc::SwerveModuleState, MODULE_COUNT > calculateModuleStates (frc::ChassisSpeeds chassisSpeeds, bool isFOC)
 Calculates the desired swerve module states based on chassis speeds.
 
void updateAngularPosition ()
 
void drive (units::velocity::meters_per_second_t vx_mps, units::velocity::meters_per_second_t vy_mps, units::angular_velocity::radians_per_second_t omega_radps, bool isFOC)
 Drives the robot with given x, y, and rotational velocities. This function internally converts the individual speed components into a frc::ChassisSpeeds object and calls the overloaded drive(frc::ChassisSpeeds, bool) method.
 
void drive (frc::ChassisSpeeds _chassisSpeeds, bool isFOC)
 Drives the robot using specified chassis speeds. This is the primary drive method that calculates and sets the desired states for each swerve module.
 
void driveRobotRelative (frc::ChassisSpeeds speeds)
 Drives the robot relative to its own orientation. This method calculates swerve module states based on the provided chassis speeds, assuming robot-oriented control.
 
double getSkiddingRatio ()
 
bool isRobotSkidding ()
 
void updateAngularAcceleration ()
 Updates the calculated angular acceleration of the robot. This function uses the current and previous yaw rates from the gyroscope to compute the angular acceleration.
 
units::angular_acceleration::radians_per_second_squared_t getSmoothedAngularAcceleration ()
 
double rotationLerping (double)
 
void followTrajectory (const choreo::SwerveSample &sample)
 
units::meter_t getDriveBaseRadius ()
 
- Public Member Functions inherited from valor::BaseSubsystem
 BaseSubsystem (std::string name)
 Construct a new Valor Subsystem object.
 
virtual void assessInputs ()
 Read controller logic and set subsystem state.
 
void setGamepads (valor::Gamepad *_operatorGamepad, valor::Gamepad *_driverGamepad)
 
- Public Member Functions inherited from valor::Loggable
void LogChild (std::string_view name, Loggable *child)
 Register a child Loggable under a named subtree.
 
void LogChild (std::string_view name, wpi::Sendable *child)
 Register a child Sendable under a named subtree.
 
void setLoggingPeriod (units::millisecond_t period)
 Set the minimum period between LoggablePeriodic() invocations.
 
template<>
decltype(Loggable::subscribers) ::iterator addSubscriber (std::string_view field, const std::vector< bool > &defaultValue)
 
template<>
void WriteLogImpl (nt::Publisher *pub, const std::vector< bool > &data)
 
template<>
std::vector< boolReadLogImpl (nt::Subscriber *sub)
 

Public Attributes

CharMode::MODE_OPTIONS selectedTest
 
frc::Pose2d cachedRawPose
 
frc::Pose2d cachedCalculatedPose
 
frc::Pose2d cachedPredictedPose
 
frc::ChassisSpeeds cachedChassisFieldSpeed
 
frc::ChassisSpeeds cachedRobotRelativeSpeed
 

Protected Member Functions

void enableCarpetGrain (double grainMultiplier, bool roughTowardsRed)
 
frc::ChassisSpeeds vectorLockedSpeeds (const frc::ChassisSpeeds speeds, const frc::Rotation2d heading)
 
frc::ChassisSpeeds getRobotRelativeSpeeds ()
 
frc::ChassisSpeeds getFieldRelativeSpeeds ()
 
void setSwerveDesiredState (wpi::array< frc::SwerveModuleState, MODULE_COUNT > desiredStates, bool isDriveOpenLoop)
 
void startOdomThread (units::hertz_t freq)
 
void updateOdometry ()
 
- Protected Member Functions inherited from valor::Loggable
 Loggable (std::string_view name)
 Construct a Loggable that registers a top-level table name.
 
 Loggable ()
 Default construct an uninitialized Loggable.
 
virtual ~Loggable ()=default
 Virtual destructor.
 
virtual void OnLoggingStart ()
 Hook invoked when logging is started for this object.
 
template<typename T >
T WriteLog (std::string_view field, const T &data)
 Publish a value to NetworkTables under the given field.
 
template<typename T >
ReadLog (std::string_view field, const T &defaultValue={})
 Read a value from NetworkTables for the given field.
 

Static Protected Member Functions

static frc::ChassisSpeeds reflectDriveSpeeds (const frc::ChassisSpeeds unreflectedSpeeds)
 

Protected Attributes

double xSpeed
 
double ySpeed
 
double rotSpeed
 
frc::ChassisSpeeds commandedSpeeds
 
units::meters_per_second_t maxDriveSpeed
 
units::radians_per_second_t maxRotationSpeed
 
std::unique_ptr< ctre::phoenix6::hardware::Pigeon2 > pigeon
 
std::unique_ptr< frc::SwerveDriveKinematics< MODULE_COUNT > > kinematics
 
util::Locked< std::unique_ptr< frc::SwerveDrivePoseEstimator< MODULE_COUNT > > > rawEstimator
 
util::Locked< std::unique_ptr< frc::SwerveDrivePoseEstimator< MODULE_COUNT > > > calcEstimator
 
bool toast
 
units::radian_t angularPosition = 0_rad
 
wpi::array< frc::SwerveModuleState, MODULE_COUNT > testModeDesiredStates {wpi::empty_array}
 
std::vector< valor::SwerveModule * > swerveModules
 
frc::Timer resetOdom
 
- Protected Attributes inherited from valor::BaseSubsystem
valor::GamepadoperatorGamepad
 
valor::GamepaddriverGamepad
 

Additional Inherited Members

- Static Public Member Functions inherited from valor::Loggable
static units::millisecond_t GetLoggingTime ()
 

Member Function Documentation

◆ analyzeDashboard()

void valor::Swerve::analyzeDashboard ( )
overridevirtual

Synchronize dashboard data (both read and write)

The analyzeDashboard function runs at all times (disabled, auto, and teleop). The intent of the function is to send sensor and state information from the robot to the Driver Station for debugging. Additionally, driver commands and data can be sent from the Driver Station to the robot and collected in this function.

ALL network table and sensor logic should be in this function as both network table and sensor information should be read at all times, not just during teleop since auto will never get that information.

This function is a virtual function and should be implemented by the subsystem.

Reimplemented from valor::BaseSubsystem.

◆ assignOutputs()

void valor::Swerve::assignOutputs ( )
overridevirtual

Read subsystem state and send motor commands.

This function runs during teleop and auto, but not disabled. This is because the function is intended to control motors and should not send motor outputs when the robot is disabled. assignOutputs should read the subsystem state set in assessInputs and analyzeDashboard and create motor commands.

Note that state should be READ and not be written to (the opposite of assessInputs). The big rule to obey with this function is that other subsystem's state should never be written to. For example, if sensors in subsystem A determine outputs for subsystem A AND subsystem B, then the sensor should live in both subsystems instead of subsystem A trying to write results in subsystem B. This will make logic easier to understand for all developers.

Suggested flow:

  • Read subsystem state
  • Set motor output based on those states in if statements
    • The higher up in the if statement, the higher the priority of the motor output

This function is a virtual function and should be implemented by the subsystem.

Reimplemented from valor::BaseSubsystem.

◆ calculateModuleStates()

wpi::array< frc::SwerveModuleState, MODULE_COUNT > valor::Swerve::calculateModuleStates ( frc::ChassisSpeeds chassisSpeeds,
bool isFOC )

Calculates the desired swerve module states based on chassis speeds.

Parameters
chassisSpeedsAn frc::ChassisSpeeds object containing the desired robot velocities (vx, vy, omega).
isFOCTrue if the chassisSpeeds are field-oriented, false if robot-oriented.
Returns
An array of frc::SwerveModuleState objects, representing the calculated state for each module.

◆ drive() [1/2]

void valor::Swerve::drive ( frc::ChassisSpeeds _chassisSpeeds,
bool isFOC )

Drives the robot using specified chassis speeds. This is the primary drive method that calculates and sets the desired states for each swerve module.

Parameters
_chassisSpeedsAn frc::ChassisSpeeds object containing the desired field-relative or robot-relative velocities (vx, vy, omega).
isFOCTrue if the _chassisSpeeds are field-oriented, false if robot-oriented.

◆ drive() [2/2]

void valor::Swerve::drive ( units::velocity::meters_per_second_t vx_mps,
units::velocity::meters_per_second_t vy_mps,
units::angular_velocity::radians_per_second_t omega_radps,
bool isFOC )

Drives the robot with given x, y, and rotational velocities. This function internally converts the individual speed components into a frc::ChassisSpeeds object and calls the overloaded drive(frc::ChassisSpeeds, bool) method.

Parameters
vx_mpsThe desired x velocity component in meters per second.
vy_mpsThe desired y velocity component in meters per second.
omega_radpsThe desired rotational velocity component in radians per second.
isFOCTrue if driving field-oriented, false for robot-oriented.

◆ driveRobotRelative()

void valor::Swerve::driveRobotRelative ( frc::ChassisSpeeds speeds)

Drives the robot relative to its own orientation. This method calculates swerve module states based on the provided chassis speeds, assuming robot-oriented control.

Parameters
speedsAn frc::ChassisSpeeds object containing the desired robot-relative velocities (vx, vy, omega).

◆ getModulePositions()

wpi::array< frc::SwerveModulePosition, MODULE_COUNT > valor::Swerve::getModulePositions ( )

Retrieves the current position of each swerve module.

Returns
An array of frc::SwerveModulePosition objects, representing the position of each module.

◆ getModuleStates()

wpi::array< frc::SwerveModuleState, MODULE_COUNT > valor::Swerve::getModuleStates ( )

Retrieves the current state (speed and angle) of each swerve module.

Returns
An array of frc::SwerveModuleState objects, representing the state of each module.

◆ getRawPose()

frc::Pose2d valor::Swerve::getRawPose ( )

Returns the position of the robot on the field in meters

Returns
the pose of the robot

◆ LoggablePeriodic()

void valor::Swerve::LoggablePeriodic ( )
overridevirtual

Periodic callback for logging updates.

If a derived class needs to perform periodic updates to published values, override this method. It is intended to be called by the logging framework; this base class provides an empty implementation.

Reimplemented from valor::Loggable.

◆ reset()

void valor::Swerve::reset ( )
overridevirtual

Reset all subsystem state.

Use this function to set the subsystem state to default values, as well as set the motor outputs to 0 so the robot doesn't "lurch" when booting up due to phantom state (aka. state that was previously set but hasn't been reset yet).

This function is a virtual function and should be implemented by the subsystem.

Reimplemented from valor::BaseSubsystem.

◆ resetOdometry()

void valor::Swerve::resetOdometry ( frc::Pose2d pose)

Reset the robot's position on the field. Any accumulted gyro drift will be noted and accounted for in subsequent calls to getPoseMeters()

Parameters
poseThe robot's actual position on the field

The documentation for this class was generated from the following file: