mirror of
https://github.com/BroncBotz3481/YAGSL
synced 2026-06-19 06:21:40 +00:00
395 lines
26 KiB
HTML
395 lines
26 KiB
HTML
<!DOCTYPE HTML>
|
|
<html lang="en">
|
|
<head>
|
|
<!-- Generated by javadoc (17) on Tue Feb 14 22:37:00 CST 2023 -->
|
|
<title>SwerveMath</title>
|
|
<meta name="viewport" content="width=device-width, initial-scale=1">
|
|
<meta http-equiv="Content-Type" content="text/html; charset=utf-8">
|
|
<meta name="dc.created" content="2023-02-14">
|
|
<meta name="description" content="declaration: package: swervelib.math, class: SwerveMath">
|
|
<meta name="generator" content="javadoc/ClassWriterImpl">
|
|
<link rel="stylesheet" type="text/css" href="../../stylesheet.css" title="Style">
|
|
<link rel="stylesheet" type="text/css" href="../../script-dir/jquery-ui.min.css" title="Style">
|
|
<link rel="stylesheet" type="text/css" href="../../jquery-ui.overrides.css" title="Style">
|
|
<script type="text/javascript" src="../../script.js"></script>
|
|
<script type="text/javascript" src="../../script-dir/jquery-3.5.1.min.js"></script>
|
|
<script type="text/javascript" src="../../script-dir/jquery-ui.min.js"></script>
|
|
</head>
|
|
<body class="class-declaration-page">
|
|
<script type="text/javascript">var evenRowColor = "even-row-color";
|
|
var oddRowColor = "odd-row-color";
|
|
var tableTab = "table-tab";
|
|
var activeTableTab = "active-table-tab";
|
|
var pathtoroot = "../../";
|
|
loadScripts(document, 'script');</script>
|
|
<noscript>
|
|
<div>JavaScript is disabled on your browser.</div>
|
|
</noscript>
|
|
<div class="flex-box">
|
|
<header role="banner" class="flex-header">
|
|
<nav role="navigation">
|
|
<!-- ========= START OF TOP NAVBAR ======= -->
|
|
<div class="top-nav" id="navbar-top">
|
|
<div class="skip-nav"><a href="#skip-navbar-top" title="Skip navigation links">Skip navigation links</a></div>
|
|
<ul id="navbar-top-firstrow" class="nav-list" title="Navigation">
|
|
<li><a href="../../index.html">Overview</a></li>
|
|
<li><a href="package-summary.html">Package</a></li>
|
|
<li class="nav-bar-cell1-rev">Class</li>
|
|
<li><a href="package-tree.html">Tree</a></li>
|
|
<li><a href="../../index-files/index-1.html">Index</a></li>
|
|
<li><a href="../../help-doc.html#class">Help</a></li>
|
|
</ul>
|
|
</div>
|
|
<div class="sub-nav">
|
|
<div>
|
|
<ul class="sub-nav-list">
|
|
<li>Summary: </li>
|
|
<li>Nested | </li>
|
|
<li>Field | </li>
|
|
<li><a href="#constructor-summary">Constr</a> | </li>
|
|
<li><a href="#method-summary">Method</a></li>
|
|
</ul>
|
|
<ul class="sub-nav-list">
|
|
<li>Detail: </li>
|
|
<li>Field | </li>
|
|
<li><a href="#constructor-detail">Constr</a> | </li>
|
|
<li><a href="#method-detail">Method</a></li>
|
|
</ul>
|
|
</div>
|
|
<div class="nav-list-search"><label for="search-input">SEARCH:</label>
|
|
<input type="text" id="search-input" value="search" disabled="disabled">
|
|
<input type="reset" id="reset-button" value="reset" disabled="disabled">
|
|
</div>
|
|
</div>
|
|
<!-- ========= END OF TOP NAVBAR ========= -->
|
|
<span class="skip-nav" id="skip-navbar-top"></span></nav>
|
|
</header>
|
|
<div class="flex-content">
|
|
<main role="main">
|
|
<!-- ======== START OF CLASS DATA ======== -->
|
|
<div class="header">
|
|
<div class="sub-title"><span class="package-label-in-type">Package</span> <a href="package-summary.html">swervelib.math</a></div>
|
|
<h1 title="Class SwerveMath" class="title">Class SwerveMath</h1>
|
|
</div>
|
|
<div class="inheritance" title="Inheritance Tree"><a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html" title="class or interface in java.lang" class="external-link">java.lang.Object</a>
|
|
<div class="inheritance">swervelib.math.SwerveMath</div>
|
|
</div>
|
|
<section class="class-description" id="class-description">
|
|
<hr>
|
|
<div class="type-signature"><span class="modifiers">public class </span><span class="element-name type-name-label">SwerveMath</span>
|
|
<span class="extends-implements">extends <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html" title="class or interface in java.lang" class="external-link">Object</a></span></div>
|
|
<div class="block">Mathematical functions which pertain to swerve drive.</div>
|
|
</section>
|
|
<section class="summary">
|
|
<ul class="summary-list">
|
|
<!-- ======== CONSTRUCTOR SUMMARY ======== -->
|
|
<li>
|
|
<section class="constructor-summary" id="constructor-summary">
|
|
<h2>Constructor Summary</h2>
|
|
<div class="caption"><span>Constructors</span></div>
|
|
<div class="summary-table two-column-summary">
|
|
<div class="table-header col-first">Constructor</div>
|
|
<div class="table-header col-last">Description</div>
|
|
<div class="col-constructor-name even-row-color"><code><a href="#%3Cinit%3E()" class="member-name-link">SwerveMath</a>()</code></div>
|
|
<div class="col-last even-row-color"> </div>
|
|
</div>
|
|
</section>
|
|
</li>
|
|
<!-- ========== METHOD SUMMARY =========== -->
|
|
<li>
|
|
<section class="method-summary" id="method-summary">
|
|
<h2>Method Summary</h2>
|
|
<div id="method-summary-table">
|
|
<div class="table-tabs" role="tablist" aria-orientation="horizontal"><button id="method-summary-table-tab0" role="tab" aria-selected="true" aria-controls="method-summary-table.tabpanel" tabindex="0" onkeydown="switchTab(event)" onclick="show('method-summary-table', 'method-summary-table', 3)" class="active-table-tab">All Methods</button><button id="method-summary-table-tab1" role="tab" aria-selected="false" aria-controls="method-summary-table.tabpanel" tabindex="-1" onkeydown="switchTab(event)" onclick="show('method-summary-table', 'method-summary-table-tab1', 3)" class="table-tab">Static Methods</button><button id="method-summary-table-tab4" role="tab" aria-selected="false" aria-controls="method-summary-table.tabpanel" tabindex="-1" onkeydown="switchTab(event)" onclick="show('method-summary-table', 'method-summary-table-tab4', 3)" class="table-tab">Concrete Methods</button></div>
|
|
<div id="method-summary-table.tabpanel" role="tabpanel">
|
|
<div class="summary-table three-column-summary" aria-labelledby="method-summary-table-tab0">
|
|
<div class="table-header col-first">Modifier and Type</div>
|
|
<div class="table-header col-second">Method</div>
|
|
<div class="table-header col-last">Description</div>
|
|
<div class="col-first even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code>static double</code></div>
|
|
<div class="col-second even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code><a href="#applyDeadband(double,boolean,double)" class="member-name-link">applyDeadband</a><wbr>(double value,
|
|
boolean scaled,
|
|
double deadband)</code></div>
|
|
<div class="col-last even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4">
|
|
<div class="block">Algebraically apply a deadband using a piece wise function.</div>
|
|
</div>
|
|
<div class="col-first odd-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code>private static double</code></div>
|
|
<div class="col-second odd-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code><a href="#calcMaxAccel(edu.wpi.first.math.geometry.Rotation2d,double,double,edu.wpi.first.math.geometry.Translation3d,swervelib.parser.SwerveDriveConfiguration)" class="member-name-link">calcMaxAccel</a><wbr>(edu.wpi.first.math.geometry.Rotation2d angle,
|
|
double chassisMass,
|
|
double robotMass,
|
|
edu.wpi.first.math.geometry.Translation3d chassisCenterOfGravity,
|
|
<a href="../parser/SwerveDriveConfiguration.html" title="class in swervelib.parser">SwerveDriveConfiguration</a> config)</code></div>
|
|
<div class="col-last odd-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4">
|
|
<div class="block">Calculates the maximum acceleration allowed in a direction without tipping the robot.</div>
|
|
</div>
|
|
<div class="col-first even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code>static double</code></div>
|
|
<div class="col-second even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code><a href="#calculateAngleKV(double,double,double)" class="member-name-link">calculateAngleKV</a><wbr>(double optimalVoltage,
|
|
double motorFreeSpeedRPM,
|
|
double angleGearRatio)</code></div>
|
|
<div class="col-last even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4">
|
|
<div class="block">Calculate the angle kV which will be multiplied by the radians per second for the feedforward.</div>
|
|
</div>
|
|
<div class="col-first odd-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code>static double</code></div>
|
|
<div class="col-second odd-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code><a href="#calculateDegreesPerSteeringRotation(double,double)" class="member-name-link">calculateDegreesPerSteeringRotation</a><wbr>(double angleGearRatio,
|
|
double pulsePerRotation)</code></div>
|
|
<div class="col-last odd-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4">
|
|
<div class="block">Calculate the degrees per steering rotation for the integrated encoder.</div>
|
|
</div>
|
|
<div class="col-first even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code>static double</code></div>
|
|
<div class="col-second even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code><a href="#calculateMaxAcceleration(double)" class="member-name-link">calculateMaxAcceleration</a><wbr>(double cof)</code></div>
|
|
<div class="col-last even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4">
|
|
<div class="block">Calculate the practical maximum acceleration of the robot using the wheel coefficient of friction.</div>
|
|
</div>
|
|
<div class="col-first odd-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code>static double</code></div>
|
|
<div class="col-second odd-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code><a href="#calculateMaxAcceleration(double,double,double,double,double)" class="member-name-link">calculateMaxAcceleration</a><wbr>(double stallTorqueNm,
|
|
double gearRatio,
|
|
double moduleCount,
|
|
double wheelDiameter,
|
|
double robotMass)</code></div>
|
|
<div class="col-last odd-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4">
|
|
<div class="block">Calculate the maximum theoretical acceleration without friction.</div>
|
|
</div>
|
|
<div class="col-first even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code>static double</code></div>
|
|
<div class="col-second even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code><a href="#calculateMaxAngularVelocity(double,double,double)" class="member-name-link">calculateMaxAngularVelocity</a><wbr>(double maxSpeed,
|
|
double furthestModuleX,
|
|
double furthestModuleY)</code></div>
|
|
<div class="col-last even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4">
|
|
<div class="block">Calculate the maximum angular velocity.</div>
|
|
</div>
|
|
<div class="col-first odd-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code>static double</code></div>
|
|
<div class="col-second odd-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code><a href="#calculateMetersPerRotation(double,double,double)" class="member-name-link">calculateMetersPerRotation</a><wbr>(double wheelDiameter,
|
|
double driveGearRatio,
|
|
double pulsePerRotation)</code></div>
|
|
<div class="col-last odd-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4">
|
|
<div class="block">Calculate the meters per rotation for the integrated encoder.</div>
|
|
</div>
|
|
<div class="col-first even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code>static edu.wpi.first.math.geometry.Translation2d</code></div>
|
|
<div class="col-second even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4"><code><a href="#limitVelocity(edu.wpi.first.math.geometry.Translation2d,edu.wpi.first.math.kinematics.ChassisSpeeds,edu.wpi.first.math.geometry.Pose2d,double,double,double,edu.wpi.first.math.geometry.Translation3d,swervelib.parser.SwerveDriveConfiguration)" class="member-name-link">limitVelocity</a><wbr>(edu.wpi.first.math.geometry.Translation2d commandedVelocity,
|
|
edu.wpi.first.math.kinematics.ChassisSpeeds fieldVelocity,
|
|
edu.wpi.first.math.geometry.Pose2d robotPose,
|
|
double loopTime,
|
|
double chassisMass,
|
|
double robotMass,
|
|
edu.wpi.first.math.geometry.Translation3d chassisCenterOfGravity,
|
|
<a href="../parser/SwerveDriveConfiguration.html" title="class in swervelib.parser">SwerveDriveConfiguration</a> config)</code></div>
|
|
<div class="col-last even-row-color method-summary-table method-summary-table-tab1 method-summary-table-tab4">
|
|
<div class="block">Limits a commanded velocity to prevent exceeding the maximum acceleration given by
|
|
<a href="#calcMaxAccel(edu.wpi.first.math.geometry.Rotation2d,double,double,edu.wpi.first.math.geometry.Translation3d,swervelib.parser.SwerveDriveConfiguration)"><code>calcMaxAccel(Rotation2d, double, double, Translation3d, SwerveDriveConfiguration)</code></a>.</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
<div class="inherited-list">
|
|
<h3 id="methods-inherited-from-class-java.lang.Object">Methods inherited from class java.lang.<a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html" title="class or interface in java.lang" class="external-link">Object</a></h3>
|
|
<code><a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#clone()" title="class or interface in java.lang" class="external-link">clone</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#equals(java.lang.Object)" title="class or interface in java.lang" class="external-link">equals</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#finalize()" title="class or interface in java.lang" class="external-link">finalize</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#getClass()" title="class or interface in java.lang" class="external-link">getClass</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#hashCode()" title="class or interface in java.lang" class="external-link">hashCode</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#notify()" title="class or interface in java.lang" class="external-link">notify</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#notifyAll()" title="class or interface in java.lang" class="external-link">notifyAll</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#toString()" title="class or interface in java.lang" class="external-link">toString</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#wait()" title="class or interface in java.lang" class="external-link">wait</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#wait(long)" title="class or interface in java.lang" class="external-link">wait</a>, <a href="https://docs.oracle.com/en/java/javase/17/docs/api/java.base/java/lang/Object.html#wait(long,int)" title="class or interface in java.lang" class="external-link">wait</a></code></div>
|
|
</section>
|
|
</li>
|
|
</ul>
|
|
</section>
|
|
<section class="details">
|
|
<ul class="details-list">
|
|
<!-- ========= CONSTRUCTOR DETAIL ======== -->
|
|
<li>
|
|
<section class="constructor-details" id="constructor-detail">
|
|
<h2>Constructor Details</h2>
|
|
<ul class="member-list">
|
|
<li>
|
|
<section class="detail" id="<init>()">
|
|
<h3>SwerveMath</h3>
|
|
<div class="member-signature"><span class="modifiers">public</span> <span class="element-name">SwerveMath</span>()</div>
|
|
</section>
|
|
</li>
|
|
</ul>
|
|
</section>
|
|
</li>
|
|
<!-- ============ METHOD DETAIL ========== -->
|
|
<li>
|
|
<section class="method-details" id="method-detail">
|
|
<h2>Method Details</h2>
|
|
<ul class="member-list">
|
|
<li>
|
|
<section class="detail" id="calculateAngleKV(double,double,double)">
|
|
<h3>calculateAngleKV</h3>
|
|
<div class="member-signature"><span class="modifiers">public static</span> <span class="return-type">double</span> <span class="element-name">calculateAngleKV</span><wbr><span class="parameters">(double optimalVoltage,
|
|
double motorFreeSpeedRPM,
|
|
double angleGearRatio)</span></div>
|
|
<div class="block">Calculate the angle kV which will be multiplied by the radians per second for the feedforward. Volt * seconds /
|
|
degree <=> (maxVolts) / (maxSpeed)</div>
|
|
<dl class="notes">
|
|
<dt>Parameters:</dt>
|
|
<dd><code>optimalVoltage</code> - Optimal voltage to use when calculating the angle kV.</dd>
|
|
<dd><code>motorFreeSpeedRPM</code> - Motor free speed in Rotations per Minute.</dd>
|
|
<dd><code>angleGearRatio</code> - Angle gear ratio, the amount of times the motor as to turn for the wheel rotation.</dd>
|
|
<dt>Returns:</dt>
|
|
<dd>angle kV for feedforward.</dd>
|
|
</dl>
|
|
</section>
|
|
</li>
|
|
<li>
|
|
<section class="detail" id="calculateMetersPerRotation(double,double,double)">
|
|
<h3>calculateMetersPerRotation</h3>
|
|
<div class="member-signature"><span class="modifiers">public static</span> <span class="return-type">double</span> <span class="element-name">calculateMetersPerRotation</span><wbr><span class="parameters">(double wheelDiameter,
|
|
double driveGearRatio,
|
|
double pulsePerRotation)</span></div>
|
|
<div class="block">Calculate the meters per rotation for the integrated encoder. Calculation: 4in diameter wheels * pi [circumfrence]
|
|
/ gear ratio.</div>
|
|
<dl class="notes">
|
|
<dt>Parameters:</dt>
|
|
<dd><code>wheelDiameter</code> - Wheel diameter in meters.</dd>
|
|
<dd><code>driveGearRatio</code> - The gear ratio of the drive motor.</dd>
|
|
<dd><code>pulsePerRotation</code> - The number of encoder pulses per rotation. 1 if using an integrated encoder.</dd>
|
|
<dt>Returns:</dt>
|
|
<dd>Meters per rotation for the drive motor.</dd>
|
|
</dl>
|
|
</section>
|
|
</li>
|
|
<li>
|
|
<section class="detail" id="applyDeadband(double,boolean,double)">
|
|
<h3>applyDeadband</h3>
|
|
<div class="member-signature"><span class="modifiers">public static</span> <span class="return-type">double</span> <span class="element-name">applyDeadband</span><wbr><span class="parameters">(double value,
|
|
boolean scaled,
|
|
double deadband)</span></div>
|
|
<div class="block">Algebraically apply a deadband using a piece wise function.</div>
|
|
<dl class="notes">
|
|
<dt>Parameters:</dt>
|
|
<dd><code>value</code> - value to apply deadband to.</dd>
|
|
<dd><code>scaled</code> - Use algebra to determine deadband by starting the value at 0 past deadband.</dd>
|
|
<dd><code>deadband</code> - The deadbnad to apply.</dd>
|
|
<dt>Returns:</dt>
|
|
<dd>Value with deadband applied.</dd>
|
|
</dl>
|
|
</section>
|
|
</li>
|
|
<li>
|
|
<section class="detail" id="calculateDegreesPerSteeringRotation(double,double)">
|
|
<h3>calculateDegreesPerSteeringRotation</h3>
|
|
<div class="member-signature"><span class="modifiers">public static</span> <span class="return-type">double</span> <span class="element-name">calculateDegreesPerSteeringRotation</span><wbr><span class="parameters">(double angleGearRatio,
|
|
double pulsePerRotation)</span></div>
|
|
<div class="block">Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion values. Drive converts
|
|
motor rotations to linear wheel distance and steering converts motor rotations to module azimuth.</div>
|
|
<dl class="notes">
|
|
<dt>Parameters:</dt>
|
|
<dd><code>angleGearRatio</code> - The gear ratio of the steering motor.</dd>
|
|
<dd><code>pulsePerRotation</code> - The number of pulses in a complete rotation for the encoder, 1 if integrated.</dd>
|
|
<dt>Returns:</dt>
|
|
<dd>Degrees per steering rotation for the angle motor.</dd>
|
|
</dl>
|
|
</section>
|
|
</li>
|
|
<li>
|
|
<section class="detail" id="calculateMaxAngularVelocity(double,double,double)">
|
|
<h3>calculateMaxAngularVelocity</h3>
|
|
<div class="member-signature"><span class="modifiers">public static</span> <span class="return-type">double</span> <span class="element-name">calculateMaxAngularVelocity</span><wbr><span class="parameters">(double maxSpeed,
|
|
double furthestModuleX,
|
|
double furthestModuleY)</span></div>
|
|
<div class="block">Calculate the maximum angular velocity.</div>
|
|
<dl class="notes">
|
|
<dt>Parameters:</dt>
|
|
<dd><code>maxSpeed</code> - Max speed of the robot in meters per second.</dd>
|
|
<dd><code>furthestModuleX</code> - X of the furthest module in meters.</dd>
|
|
<dd><code>furthestModuleY</code> - Y of the furthest module in meters.</dd>
|
|
<dt>Returns:</dt>
|
|
<dd>Maximum angular velocity in rad/s.</dd>
|
|
</dl>
|
|
</section>
|
|
</li>
|
|
<li>
|
|
<section class="detail" id="calculateMaxAcceleration(double)">
|
|
<h3>calculateMaxAcceleration</h3>
|
|
<div class="member-signature"><span class="modifiers">public static</span> <span class="return-type">double</span> <span class="element-name">calculateMaxAcceleration</span><wbr><span class="parameters">(double cof)</span></div>
|
|
<div class="block">Calculate the practical maximum acceleration of the robot using the wheel coefficient of friction.</div>
|
|
<dl class="notes">
|
|
<dt>Parameters:</dt>
|
|
<dd><code>cof</code> - Coefficient of Friction of the wheel grip tape.</dd>
|
|
<dt>Returns:</dt>
|
|
<dd>Practical maximum acceleration in m/s/s.</dd>
|
|
</dl>
|
|
</section>
|
|
</li>
|
|
<li>
|
|
<section class="detail" id="calculateMaxAcceleration(double,double,double,double,double)">
|
|
<h3>calculateMaxAcceleration</h3>
|
|
<div class="member-signature"><span class="modifiers">public static</span> <span class="return-type">double</span> <span class="element-name">calculateMaxAcceleration</span><wbr><span class="parameters">(double stallTorqueNm,
|
|
double gearRatio,
|
|
double moduleCount,
|
|
double wheelDiameter,
|
|
double robotMass)</span></div>
|
|
<div class="block">Calculate the maximum theoretical acceleration without friction.</div>
|
|
<dl class="notes">
|
|
<dt>Parameters:</dt>
|
|
<dd><code>stallTorqueNm</code> - Stall torque of driving motor in nM.</dd>
|
|
<dd><code>gearRatio</code> - Gear ratio for driving motor number of motor rotations until one wheel rotation.</dd>
|
|
<dd><code>moduleCount</code> - Number of swerve modules.</dd>
|
|
<dd><code>wheelDiameter</code> - Wheel diameter in meters.</dd>
|
|
<dd><code>robotMass</code> - Mass of the robot in kg.</dd>
|
|
<dt>Returns:</dt>
|
|
<dd>Theoretical maximum acceleration in m/s/s.</dd>
|
|
</dl>
|
|
</section>
|
|
</li>
|
|
<li>
|
|
<section class="detail" id="calcMaxAccel(edu.wpi.first.math.geometry.Rotation2d,double,double,edu.wpi.first.math.geometry.Translation3d,swervelib.parser.SwerveDriveConfiguration)">
|
|
<h3>calcMaxAccel</h3>
|
|
<div class="member-signature"><span class="modifiers">private static</span> <span class="return-type">double</span> <span class="element-name">calcMaxAccel</span><wbr><span class="parameters">(edu.wpi.first.math.geometry.Rotation2d angle,
|
|
double chassisMass,
|
|
double robotMass,
|
|
edu.wpi.first.math.geometry.Translation3d chassisCenterOfGravity,
|
|
<a href="../parser/SwerveDriveConfiguration.html" title="class in swervelib.parser">SwerveDriveConfiguration</a> config)</span></div>
|
|
<div class="block">Calculates the maximum acceleration allowed in a direction without tipping the robot. Reads arm position from
|
|
NetworkTables and is passed the direction in question.</div>
|
|
<dl class="notes">
|
|
<dt>Parameters:</dt>
|
|
<dd><code>angle</code> - The direction in which to calculate max acceleration, as a Rotation2d. Note that this
|
|
is robot-relative.</dd>
|
|
<dd><code>chassisMass</code> - Chassis mass in kg. (The weight of just the chassis not anything else)</dd>
|
|
<dd><code>robotMass</code> - The weight of the robot in kg. (Including manipulators, etc).</dd>
|
|
<dd><code>chassisCenterOfGravity</code> - Chassis center of gravity.</dd>
|
|
<dd><code>config</code> - The swerve drive configuration.</dd>
|
|
<dt>Returns:</dt>
|
|
<dd>Maximum acceleration allowed in the robot direction.</dd>
|
|
</dl>
|
|
</section>
|
|
</li>
|
|
<li>
|
|
<section class="detail" id="limitVelocity(edu.wpi.first.math.geometry.Translation2d,edu.wpi.first.math.kinematics.ChassisSpeeds,edu.wpi.first.math.geometry.Pose2d,double,double,double,edu.wpi.first.math.geometry.Translation3d,swervelib.parser.SwerveDriveConfiguration)">
|
|
<h3>limitVelocity</h3>
|
|
<div class="member-signature"><span class="modifiers">public static</span> <span class="return-type">edu.wpi.first.math.geometry.Translation2d</span> <span class="element-name">limitVelocity</span><wbr><span class="parameters">(edu.wpi.first.math.geometry.Translation2d commandedVelocity,
|
|
edu.wpi.first.math.kinematics.ChassisSpeeds fieldVelocity,
|
|
edu.wpi.first.math.geometry.Pose2d robotPose,
|
|
double loopTime,
|
|
double chassisMass,
|
|
double robotMass,
|
|
edu.wpi.first.math.geometry.Translation3d chassisCenterOfGravity,
|
|
<a href="../parser/SwerveDriveConfiguration.html" title="class in swervelib.parser">SwerveDriveConfiguration</a> config)</span></div>
|
|
<div class="block">Limits a commanded velocity to prevent exceeding the maximum acceleration given by
|
|
<a href="#calcMaxAccel(edu.wpi.first.math.geometry.Rotation2d,double,double,edu.wpi.first.math.geometry.Translation3d,swervelib.parser.SwerveDriveConfiguration)"><code>calcMaxAccel(Rotation2d, double, double, Translation3d, SwerveDriveConfiguration)</code></a>. Note that
|
|
this takes and returns field-relative velocities.</div>
|
|
<dl class="notes">
|
|
<dt>Parameters:</dt>
|
|
<dd><code>commandedVelocity</code> - The desired velocity</dd>
|
|
<dd><code>fieldVelocity</code> - The velocity of the robot within a field relative state.</dd>
|
|
<dd><code>robotPose</code> - The current pose of the robot.</dd>
|
|
<dd><code>loopTime</code> - 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></dd>
|
|
<dd><code>chassisMass</code> - Chassis mass in kg. (The weight of just the chassis not anything else)</dd>
|
|
<dd><code>robotMass</code> - The weight of the robot in kg. (Including manipulators, etc).</dd>
|
|
<dd><code>chassisCenterOfGravity</code> - Chassis center of gravity.</dd>
|
|
<dd><code>config</code> - The swerve drive configuration.</dd>
|
|
<dt>Returns:</dt>
|
|
<dd>The limited velocity. This is either the commanded velocity, if attainable, or the closest attainable
|
|
velocity.</dd>
|
|
</dl>
|
|
</section>
|
|
</li>
|
|
</ul>
|
|
</section>
|
|
</li>
|
|
</ul>
|
|
</section>
|
|
<!-- ========= END OF CLASS DATA ========= -->
|
|
</main>
|
|
</div>
|
|
</div>
|
|
</body>
|
|
</html>
|