Package frc.team_8840_lib.controllers
Class SwerveDrive
java.lang.Object
frc.team_8840_lib.utils.IO.IOLayer
frc.team_8840_lib.replay.Replayable
frc.team_8840_lib.controllers.SwerveDrive
- All Implemented Interfaces:
Loggable
-
Constructor Summary
ConstructorsConstructorDescriptionSwerveDrive(ModuleConfig frontLeft, ModuleConfig frontRight, ModuleConfig backLeft, ModuleConfig backRight, Pigeon pigeon, SwerveSettings settings) -
Method Summary
Modifier and TypeMethodDescriptionvoiddrive(edu.wpi.first.math.geometry.Translation2d translation, edu.wpi.first.math.geometry.Rotation2d rotationSpeed, boolean fieldRelative, boolean openLoop) Sets the speed and rotation of each module using a translation and rotation.voidedu.wpi.first.math.geometry.Rotation2dgetAngle()Returns the angle of the gyroscope on the robot.double[]double[]edu.wpi.first.math.geometry.Pose2dgetPose()double[]edu.wpi.first.math.kinematics.SwerveModulePosition[]booleanbooleanisReady()voidreplayAngles(double[] angles) voidreplayGyroscope(double[] pointing) voidvoidreplayOpenLoop(boolean openLoop) voidreplaySpeeds(double[] speeds) voidresetOdometry(edu.wpi.first.math.geometry.Pose2d pose) voidsetModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState frontRight, edu.wpi.first.math.kinematics.SwerveModuleState backRight, edu.wpi.first.math.kinematics.SwerveModuleState frontLeft, edu.wpi.first.math.kinematics.SwerveModuleState backLeft, boolean openLoop, boolean runOptimization) voidspin(edu.wpi.first.math.geometry.Rotation2d rotationPerSecond, boolean openLoop) voidstop()voidUpdates the odometry of the robot using the positions of the encoders.Methods inherited from class frc.team_8840_lib.replay.Replayable
feedThread, getField, getReplaySaveMethod, replay, replayable, replaying
-
Constructor Details
-
SwerveDrive
public SwerveDrive(ModuleConfig frontLeft, ModuleConfig frontRight, ModuleConfig backLeft, ModuleConfig backRight, Pigeon pigeon, SwerveSettings settings)
-
-
Method Details
-
drive
public void drive(edu.wpi.first.math.geometry.Translation2d translation, edu.wpi.first.math.geometry.Rotation2d rotationSpeed, boolean fieldRelative, boolean openLoop) Sets the speed and rotation of each module using a translation and rotation.- Parameters:
translation- Translation for the robot (in meters).rotationSpeed- Rotation speed of the robot.fieldRelative- Whether the translation is field relative or notopenLoop- Whether to use open loop or closed loop
-
stop
public void stop() -
spin
public void spin(edu.wpi.first.math.geometry.Rotation2d rotationPerSecond, boolean openLoop) -
setModuleStates
public void setModuleStates(edu.wpi.first.math.kinematics.SwerveModuleState frontRight, edu.wpi.first.math.kinematics.SwerveModuleState backRight, edu.wpi.first.math.kinematics.SwerveModuleState frontLeft, edu.wpi.first.math.kinematics.SwerveModuleState backLeft, boolean openLoop, boolean runOptimization) -
getSwervePositions
public edu.wpi.first.math.kinematics.SwerveModulePosition[] getSwervePositions() -
updateOdometry
public void updateOdometry()Updates the odometry of the robot using the positions of the encoders. -
resetOdometry
public void resetOdometry(edu.wpi.first.math.geometry.Pose2d pose) -
getPose
public edu.wpi.first.math.geometry.Pose2d getPose() -
getSettings
-
getModules
-
getAngle
public edu.wpi.first.math.geometry.Rotation2d getAngle()Returns the angle of the gyroscope on the robot.- Returns:
- The angle of the gyroscope on the robot
-
isReady
public boolean isReady() -
getBaseName
- Specified by:
getBaseNamein interfaceLoggable- Specified by:
getBaseNamein classReplayable
-
getPointing
public double[] getPointing() -
replayGyroscope
public void replayGyroscope(double[] pointing) -
isOpenLoop
public boolean isOpenLoop() -
replayOpenLoop
public void replayOpenLoop(boolean openLoop) -
getAngles
public double[] getAngles() -
replayAngles
public void replayAngles(double[] angles) -
getSpeeds
public double[] getSpeeds() -
replaySpeeds
public void replaySpeeds(double[] speeds) -
replayInit
public void replayInit()- Specified by:
replayInitin classReplayable
-
exitReplay
public void exitReplay()- Specified by:
exitReplayin classReplayable
-