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 YAGSL to support CANandEncoders.

Browse Source
This commit is contained in:
thenetworkgrinch
2023-08-09 13:15:02 -05:00
parent b18491fa9c
commit d356dec4d0
10 changed files with 277 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
swervelib/math/SwerveModuleState2.java
View File

@@ -2,7 +2,6 @@ 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.
@@ -63,36 +62,87 @@ public class SwerveModuleState2 extends SwerveModuleState
public static SwerveModuleState2 optimize(SwerveModuleState2 desiredState, Rotation2d currentAngle,
SwerveModuleState2 lastState, double moduleSteerFeedForwardClosedLoop)
{
SwerveModuleState2 optimized = new SwerveModuleState2(SwerveModuleState.optimize(desiredState, currentAngle),
desiredState.omegaRadPerSecond);
if (desiredState.angle.equals(currentAngle) || desiredState.angle.equals(
optimized.angle.rotateBy(Rotation2d.fromDegrees(180))) || moduleSteerFeedForwardClosedLoop == 0)
{
optimized.omegaRadPerSecond = 0;
}
if (desiredState.angle.equals(optimized.angle.rotateBy(Rotation2d.fromDegrees(180))))
{
desiredState.omegaRadPerSecond = 0;
return desiredState;
}
return optimized;
/*
// NEVER optimize if it's the same angle, it just doesn't make sense...
if (currentAngle.equals(desiredState.angle.rotateBy(Rotation2d.fromDegrees(180))))
{
desiredState.invert();
desiredState.omegaRadPerSecond = 0;
return desiredState;
} else if (currentAngle.equals(desiredState.angle))
{
desiredState.omegaRadPerSecond = 0;
return desiredState;
}
SwerveModuleState2 optimized;
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);
optimized = 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)
Rotation2d delta = desiredState.angle.plus(Rotation2d.fromRadians(lastState.omegaRadPerSecond *
moduleSteerFeedForwardClosedLoop *
0.065))
.minus(currentAngle);
if (Double.compare(Math.abs(delta.getDegrees()), 90.0) < 0)
{
targetSpeed = -targetSpeed;
if (delta > 90)
{
targetAngle -= 180;
} else
{
targetAngle += 180;
}
optimized = desiredState.invert();
optimized.angle = optimized.angle.plus(Rotation2d.fromRadians(lastState.omegaRadPerSecond *
moduleSteerFeedForwardClosedLoop *
0.065));
} else
{
optimized = desiredState;
}
return new SwerveModuleState2(targetSpeed, Rotation2d.fromDegrees(targetAngle), desiredState.omegaRadPerSecond);
}
if (Double.compare(Math.abs(currentAngle.minus(optimized.angle).getDegrees()),
Math.abs(currentAngle.minus(desiredState.angle)
.getDegrees())) > 0)
{
desiredState.omegaRadPerSecond = 0;
return desiredState;
}
// Maybe the omega rad per second will always be off when optimized?
// optimized.omegaRadPerSecond = 0;
return optimized;
*/
}
/**
* Invert the swerve module state.
*
* @return Current inverted {@link SwerveModuleState2}
*/
public SwerveModuleState2 invert()
{
// omegaRadPerSecond *= -1;
// speedMetersPerSecond *= -1;
// angle = angle.rotateBy(Rotation2d.fromDegrees(180));
// return this;
return new SwerveModuleState2(-speedMetersPerSecond,
angle.rotateBy(Rotation2d.fromDegrees(180)),
-omegaRadPerSecond);
}
/**