Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 08:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

wpilib example swerve

Browse Source
This commit is contained in:
Abhi
2023-01-24 17:57:48 -07:00
parent b24bc6c6ab
commit 2376d2636f
3 changed files with 42 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+14 -1
View File
@@ -4,6 +4,7 @@
package frc4388.robot.subsystems;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
@@ -28,7 +29,9 @@ public class SwerveDrive extends SubsystemBase {
private SwerveModule[] modules;
// private RobotGyro gyro;
// private RobotGyro gyro3
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
@@ -98,6 +101,16 @@ public class SwerveDrive extends SubsystemBase {
setModuleStates(states);
}
// ! experimental WPILib swerve drive example
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
// SwerveModuleState[] states = kinematics.toSwerveModuleStates(
// fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d())
// : new ChassisSpeeds(xSpeed, ySpeed, rot)
// );
SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED));
setModuleStates(states);
}
/**
* Set each module of the swerve drive to the corresponding desired state.
* @param desiredStates Array of module states to set.