Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Updated to include gyro and accelerometer, and Rotation3d offset.

Browse Source
This commit is contained in:
thenetworkgrinch
2023-03-06 20:45:54 -06:00
parent 9b78c6363f
commit 5fd00878d2
120 changed files with 994 additions and 312 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
swervelib/SwerveDrive.java
View File

@@ -9,6 +9,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N1;
@@ -20,6 +21,7 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import swervelib.imu.SwerveIMU;
import swervelib.math.SwerveKinematics2;
import swervelib.math.SwerveMath;
@@ -407,6 +409,7 @@ public class SwerveDrive
simIMU.setAngle(0);
}
swerveController.lastAngleScalar = 0;
lastHeadingRadians = 0;
resetOdometry(new Pose2d(getPose().getTranslation(), new Rotation2d()));
}
@@ -420,9 +423,9 @@ public class SwerveDrive
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!SwerveDriveTelemetry.isSimulation)
{
double[] ypr = new double[3];
imu.getYawPitchRoll(ypr);
return Rotation2d.fromDegrees(swerveDriveConfiguration.invertedIMU ? 360 - ypr[0] : ypr[0]);
return swerveDriveConfiguration.invertedIMU
? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getZ())
: Rotation2d.fromRadians(imu.getRotation3d().getZ());
} else
{
return simIMU.getYaw();
@@ -439,9 +442,9 @@ public class SwerveDrive
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!SwerveDriveTelemetry.isSimulation)
{
double[] ypr = new double[3];
imu.getYawPitchRoll(ypr);
return Rotation2d.fromDegrees(swerveDriveConfiguration.invertedIMU ? 360 - ypr[1] : ypr[1]);
return swerveDriveConfiguration.invertedIMU
? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getY())
: Rotation2d.fromRadians(imu.getRotation3d().getY());
} else
{
return simIMU.getPitch();
@@ -458,9 +461,9 @@ public class SwerveDrive
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!SwerveDriveTelemetry.isSimulation)
{
double[] ypr = new double[3];
imu.getYawPitchRoll(ypr);
return Rotation2d.fromDegrees(swerveDriveConfiguration.invertedIMU ? 360 - ypr[2] : ypr[2]);
return swerveDriveConfiguration.invertedIMU
? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getX())
: Rotation2d.fromRadians(imu.getRotation3d().getX());
} else
{
return simIMU.getRoll();
@@ -477,18 +480,31 @@ public class SwerveDrive
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!SwerveDriveTelemetry.isSimulation)
{
double[] ypr = new double[3];
imu.getYawPitchRoll(ypr);
return new Rotation3d(
Math.toRadians(swerveDriveConfiguration.invertedIMU ? 360 - ypr[2] : ypr[2]),
Math.toRadians(swerveDriveConfiguration.invertedIMU ? 360 - ypr[1] : ypr[1]),
Math.toRadians(swerveDriveConfiguration.invertedIMU ? 360 - ypr[0] : ypr[0]));
return swerveDriveConfiguration.invertedIMU
? imu.getRotation3d().unaryMinus()
: imu.getRotation3d();
} else
{
return simIMU.getGyroRotation3d();
}
}
/**
* Gets current acceleration of the robot in m/s/s. If gyro unsupported returns empty.
*
* @return acceleration of the robot as a {@link Translation3d}
*/
public Optional<Translation3d> getAccel()
{
if (!SwerveDriveTelemetry.isSimulation)
{
return imu.getAccel();
} else
{
return simIMU.getAccel();
}
}
/**
* Sets the drive motors to brake/coast mode.
*
@@ -673,4 +689,15 @@ public class SwerveDrive
simIMU.setAngle(swerveDrivePoseEstimator.getEstimatedPosition().getRotation().getRadians());
}
}
/**
* Set the Gyroscope offset using a {@link Rotation3d} object. (Only yaw works currently.)
*
* @param gyro Gyroscope offset.
*/
public void setGyro(Rotation3d gyro)
{
imu.setYaw(gyro.getZ());
}
}