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

Added matter calculator

Browse Source
This commit is contained in:
thenetworkgrinch
2023-02-24 22:10:33 -06:00
parent 69edd17103
commit 4651cccb8e
112 changed files with 535 additions and 257 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
swervelib/math/Matter.java Normal file
View File

@@ -0,0 +1,41 @@
package swervelib.math;
import edu.wpi.first.math.geometry.Translation3d;
/**
* Object with significant mass that needs to be taken into account.
*/
public class Matter
{
/**
* Position in meters from robot center in 3d space.
*/
public Translation3d position;
/**
* Mass in kg of object.
*/
public double mass;
/**
* Construct an object representing some significant matter on the robot.
*
* @param position Position of the matter in meters.
* @param mass Mass in kg.
*/
public Matter(Translation3d position, double mass)
{
this.mass = mass;
this.position = position;
}
/**
* Get the center mass of the object.
*
* @return center mass = position * mass
*/
public Translation3d massMoment()
{
return position.times(mass);
}
}

56
swervelib/math/SwerveMath.java
View File

@@ -6,6 +6,7 @@ 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.wpilibj.smartdashboard.SmartDashboard;
import java.util.List;
import swervelib.SwerveController;
import swervelib.SwerveModule;
import swervelib.parser.SwerveDriveConfiguration;
@@ -139,28 +140,27 @@ public class SwerveMath
* Calculates the maximum acceleration allowed in a direction without tipping the robot. Reads arm position from
* NetworkTables and is passed the direction in question.
*
* @param angle The direction in which to calculate max acceleration, as a Rotation2d. Note that this
* is robot-relative.
* @param chassisMass Chassis mass in kg. (The weight of just the chassis not anything else)
* @param robotMass The weight of the robot in kg. (Including manipulators, etc).
* @param chassisCenterOfGravity Chassis center of gravity.
* @param config The swerve drive configuration.
* @param angle The direction in which to calculate max acceleration, as a Rotation2d. Note that this is
* robot-relative.
* @param matter Matter that the robot is composed of in kg. (Includes chassis)
* @param robotMass The weight of the robot in kg. (Including manipulators, etc).
* @param config The swerve drive configuration.
* @return Maximum acceleration allowed in the robot direction.
*/
private static double calcMaxAccel(
Rotation2d angle,
double chassisMass,
List<Matter> matter,
double robotMass,
Translation3d chassisCenterOfGravity,
SwerveDriveConfiguration config)
{
double xMoment = (chassisCenterOfGravity.getX() * chassisMass);
double yMoment = (chassisCenterOfGravity.getY() * chassisMass);
// Calculate the vertical mass moment using the floor as the datum. This will be used later to
// calculate max
// acceleration
double zMoment = (chassisCenterOfGravity.getZ() * (chassisMass));
Translation3d robotCG = new Translation3d(xMoment, yMoment, zMoment).div(robotMass);
// calculate max acceleration
Translation3d centerMass = new Translation3d();
for (Matter object : matter)
{
centerMass = centerMass.plus(object.massMoment());
}
Translation3d robotCG = centerMass.div(robotMass);
Translation2d horizontalCG = robotCG.toTranslation2d();
Translation2d projectedHorizontalCg =
@@ -211,19 +211,17 @@ public class SwerveMath
}
/**
* Limits a commanded velocity to prevent exceeding the maximum acceleration given by
* {@link SwerveMath#calcMaxAccel(Rotation2d, double, double, Translation3d, SwerveDriveConfiguration)}. Note that
* this takes and returns field-relative velocities.
* Limits a commanded velocity to prevent exceeding the maximum acceleration given by {@link SwerveMath#calcMaxAccel}.
* Note that this takes and returns field-relative velocities.
*
* @param commandedVelocity The desired velocity
* @param fieldVelocity The velocity of the robot within a field relative state.
* @param robotPose The current pose of the robot.
* @param loopTime The time it takes to update the velocity in seconds. <b>Note: this should include the
* 100ms that it takes for a SparkMax velocity to update.</b>
* @param chassisMass Chassis mass in kg. (The weight of just the chassis not anything else)
* @param robotMass The weight of the robot in kg. (Including manipulators, etc).
* @param chassisCenterOfGravity Chassis center of gravity.
* @param config The swerve drive configuration.
* @param commandedVelocity The desired velocity
* @param fieldVelocity The velocity of the robot within a field relative state.
* @param robotPose The current pose of the robot.
* @param loopTime The time it takes to update the velocity in seconds. <b>Note: this should include the
* 100ms that it takes for a SparkMax velocity to update.</b>
* @param matter Matter that the robot is composed of with position in meters and mass in kg.
* @param robotMass The weight of the robot in kg. (Including manipulators, etc).
* @param config The swerve drive configuration.
* @return The limited velocity. This is either the commanded velocity, if attainable, or the closest attainable
* velocity.
*/
@@ -232,9 +230,8 @@ public class SwerveMath
ChassisSpeeds fieldVelocity,
Pose2d robotPose,
double loopTime,
double chassisMass,
double robotMass,
Translation3d chassisCenterOfGravity,
List<Matter> matter,
SwerveDriveConfiguration config)
{
// Get the robot's current field-relative velocity
@@ -255,9 +252,8 @@ public class SwerveMath
// Rotates the velocity vector to convert from field-relative to robot-relative
.rotateBy(robotPose.getRotation().unaryMinus())
.getAngle(),
chassisMass,
matter,
robotMass,
chassisCenterOfGravity,
config),
deltaV.getAngle());