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

Add partial field relative chassis rotation

Browse Source
This commit is contained in:
nathanrsxtn
2022-01-30 21:27:50 -07:00
parent bb1c58764e
commit c6975fa81e
2 changed files with 20 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+18 -8
View File
@@ -110,14 +110,6 @@ public class SwerveDrive extends SubsystemBase {
*/
public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative)
{
// Mechanism2d controllerMechanism = new Mechanism2d(2, 2);
// controllerMechanism.getRoot("Left Axes", speedX, speedY);
// double[] speedsClamped = clampJoystickAxes(speedX, speedY);
// double speedXClamped = speedsClamped[0];
// double speedYClamped = speedsClamped[1];
// controllerMechanism.getRoot("Left Axes (Clamped)", speedXClamped, speedYClamped);
// SmartDashboard.putData("Driver Controller", controllerMechanism);
// SmartDashboard.putNumberArray("Left Axes -> Left Axes (Clamped)", new double[] {speedX, speedY, speedXClamped, speedYClamped});
if (speedX == 0 && speedY == 0 && rot == 0) ignoreAngles = true;
else ignoreAngles = false;
Translation2d speed = new Translation2d(speedX, speedY);
@@ -133,6 +125,24 @@ public class SwerveDrive extends SubsystemBase {
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
setModuleStates(states);
}
private Rotation2d rotTarget = new Rotation2d();
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
{
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
Translation2d speed = new Translation2d(leftX, leftY);
speed = speed.times(speed.getNorm() * speedAdjust);
if (rightX > OIConstants.AXIS_DEADBAND || rightY > OIConstants.AXIS_DEADBAND)
rotTarget = new Rotation2d(rightX, -rightY);
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
double xSpeedMetersPerSecond = -speed.getX();
double ySpeedMetersPerSecond = speed.getY();
SwerveModuleState[] states =
m_kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED));
setModuleStates(states);
}
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));