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

Update YAGSL to handle controls better

Browse Source
This commit is contained in:
thenetworkgrinch
2023-04-08 12:31:07 -05:00
parent d37a38bb7b
commit 5b38929c48
123 changed files with 871 additions and 805 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
swervelib/math/SwerveModuleState2.java
View File

@@ -2,6 +2,7 @@ package swervelib.math;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
/**
* Second order kinematics swerve module state.
@@ -9,24 +10,17 @@ import edu.wpi.first.math.kinematics.SwerveModuleState;
public class SwerveModuleState2 extends SwerveModuleState
{
/**
* Swerve module speed in meters per second.
*/
public double speedMetersPerSecond;
/**
* Rad per sec
*/
public double omegaRadPerSecond = 0;
/**
* Swerve module angle as a {@link Rotation2d}.
*/
public Rotation2d angle = Rotation2d.fromDegrees(0);
public double omegaRadPerSecond = 0;
/**
* Constructs a SwerveModuleState with zeros for speed and angle.
*/
public SwerveModuleState2()
{
super();
}
/**
@@ -39,8 +33,75 @@ public class SwerveModuleState2 extends SwerveModuleState
public SwerveModuleState2(
double speedMetersPerSecond, Rotation2d angle, double omegaRadPerSecond)
{
this.speedMetersPerSecond = speedMetersPerSecond;
this.angle = angle;
super(speedMetersPerSecond, angle);
this.omegaRadPerSecond = omegaRadPerSecond;
}
/**
* Create a {@link SwerveModuleState2} based on the {@link SwerveModuleState} with the radians per second defined.
*
* @param state First order kinematic module state.
* @param omegaRadPerSecond Module wheel angular rotation in radians per second.
*/
public SwerveModuleState2(SwerveModuleState state, double omegaRadPerSecond)
{
super(state.speedMetersPerSecond, state.angle);
this.omegaRadPerSecond = omegaRadPerSecond;
}
/**
* Minimize the change in heading the desired swerve module state would require by potentially reversing the direction
* the wheel spins. If this is used with the PIDController class's continuous input functionality, the furthest a
* wheel will ever rotate is 90 degrees.
*
* @param desiredState The desired state.
* @param currentAngle The current module angle.
* @param lastState The last state of the module.
* @param moduleSteerFeedForwardClosedLoop The module feed forward closed loop for the angle motor.
* @return Optimized swerve module state.
*/
public static SwerveModuleState2 optimize(SwerveModuleState2 desiredState, Rotation2d currentAngle,
SwerveModuleState2 lastState, double moduleSteerFeedForwardClosedLoop)
{
if (moduleSteerFeedForwardClosedLoop == 0)
{
// desiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(
// lastState.omegaRadPerSecond * moduleSteerFeedForwardClosedLoop * 0.065));
// return new SwerveModuleState2(SwerveModuleState.optimize(desiredState, currentAngle),
// desiredState.omegaRadPerSecond);
return new SwerveModuleState2(SwerveModuleState.optimize(desiredState, currentAngle), 0);
// return desiredState;
} else
{
double targetAngle = SwerveMath.placeInAppropriate0To360Scope(currentAngle.getDegrees(),
desiredState.angle.getDegrees() +
Units.radiansToDegrees(lastState.omegaRadPerSecond *
moduleSteerFeedForwardClosedLoop *
0.065));
double targetSpeed = desiredState.speedMetersPerSecond;
double delta = targetAngle - currentAngle.getDegrees();
if (Math.abs(delta) > 90)
{
targetSpeed = -targetSpeed;
if (delta > 90)
{
targetAngle -= 180;
} else
{
targetAngle += 180;
}
}
return new SwerveModuleState2(targetSpeed, Rotation2d.fromDegrees(targetAngle), desiredState.omegaRadPerSecond);
}
}
/**
* Convert to a {@link SwerveModuleState}.
*
* @return {@link SwerveModuleState} with the same angle and speed.
*/
public SwerveModuleState toSwerveModuleState()
{
return new SwerveModuleState(this.speedMetersPerSecond, this.angle);
}
}