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

mondays changes

Browse Source
This commit is contained in:
Abhishrek05
2024-02-19 15:04:55 -07:00
parent 2660adcd68
commit 83a4da3060
5 changed files with 133 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+21
View File
@@ -4,6 +4,7 @@
package frc4388.robot.subsystems;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -35,6 +36,7 @@ public class SwerveDrive extends SubsystemBase {
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
public double rotTarget = 0.0;
public Rotation2d orientRotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */
@@ -55,6 +57,7 @@ public class SwerveDrive extends SubsystemBase {
double rot = 0;
// ! drift correction
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
@@ -84,6 +87,24 @@ public class SwerveDrive extends SubsystemBase {
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if(fieldRelative) {
double rot = 0;
if(rightStick.getNorm() > 0.5) {
orientRotTarget = rightStick.getAngle().minus(new Rotation2d(0, 1));
rot = orientRotTarget.minus(gyro.getRotation2d()).getRadians();
}
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
} else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
/**
* Set each module of the swerve drive to the corresponding desired state.
* @param desiredStates Array of module states to set.