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

Added setVoltage function to swerve motors, made SwervePoseEstimator public, and changed setting targetVelocity to inside setState

Browse Source
This commit is contained in:
thenetworkgrinch
2023-01-31 10:42:02 -06:00
parent b1a4d0e370
commit c3d6df94d0
5 changed files with 66 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
swervelib/SwerveDrive.java
View File

@@ -62,18 +62,18 @@ public class SwerveDrive extends RobotDriveBase implements Sendable, AutoCloseab
* Maximum speed in meters per second.
*/
public final double m_driverMaxSpeedMPS, m_driverMaxAngularVelocity, m_physicalMaxSpeedMPS;
/**
* Swerve drive kinematics.
*/
private final SwerveDriveKinematics2 m_swerveKinematics;
/**
* Swerve drive pose estimator for attempting to figure out our current position.
*/
private final SwerveDrivePoseEstimator m_swervePoseEstimator;
public final SwerveDrivePoseEstimator m_swervePoseEstimator;
/**
* Pigeon 2.0 centered on the robot.
*/
private final WPI_Pigeon2 m_pigeonIMU;
public final WPI_Pigeon2 m_pigeonIMU;
/**
* Swerve drive kinematics.
*/
private final SwerveDriveKinematics2 m_swerveKinematics;
/**
* Field2d displayed on shuffleboard with current position.
*/
@@ -87,8 +87,17 @@ public class SwerveDrive extends RobotDriveBase implements Sendable, AutoCloseab
* Invert the gyro reading.
*/
private boolean m_gyroInverted;
/**
* Robot desired angle in degrees.
*/
private double m_angle;
/**
* Previous chassis speeds for state-space modeling.
*/
private ChassisSpeeds m_prevChassisSpeed = new ChassisSpeeds(0, 0, 0);
/**
* Previous timer value for state-space modeling.
*/
private double m_timerPrev;
/**
@@ -167,19 +176,6 @@ public class SwerveDrive extends RobotDriveBase implements Sendable, AutoCloseab
zeroModules(); // Set all modules to 0.
}
/**
* Enable voltage compensation to the current battery voltage on all modules.
*/
public void setVoltageCompensation()
{
double currentVoltage = RobotController.getBatteryVoltage();
m_frontLeft.setVoltageCompensation(currentVoltage);
m_frontRight.setVoltageCompensation(currentVoltage);
m_backLeft.setVoltageCompensation(currentVoltage);
m_backRight.setVoltageCompensation(currentVoltage);
}
/**
* Create swerve drive modules
*
@@ -233,6 +229,18 @@ public class SwerveDrive extends RobotDriveBase implements Sendable, AutoCloseab
return modules;
}
/**
* Enable voltage compensation to the current battery voltage on all modules.
*/
public void setVoltageCompensation()
{
double currentVoltage = RobotController.getBatteryVoltage();
m_frontLeft.setVoltageCompensation(currentVoltage);
m_frontRight.setVoltageCompensation(currentVoltage);
m_backLeft.setVoltageCompensation(currentVoltage);
m_backRight.setVoltageCompensation(currentVoltage);
}
/**
* Sets the speed to 0 and angle to 0 for all the swerve drive modules.
*/