Add nominal voltage constraint (#857)

Writerside/topics/gui-Editing-Paths-and-Autos.md

@@ -73,10 +73,15 @@ Max Acceleration
 : The maximum acceleration of the robot in meters/sec^2.
 
 Max Angular Velocity
-: The maximum angular velocity of the robot in degrees/sec. This is only available when holonomic mode is on.
+: The maximum angular velocity of the robot in degrees/sec.
 
 Max Angular Acceleration
-: The maximum angular acceleration of the robot in degrees/sec^2. This is only available when holonomic mode is on.
+: The maximum angular acceleration of the robot in degrees/sec^2.
+
+Nominal Voltage
+: The nominal battery voltage during the path in volts. This is effectively a max velocity and max angular velocity
+constraint that can be used to properly limit those speeds when you expect the battery voltage to sag during auto due to
+other systems running such as a shooter or intake. This constraint is still used for unlimited constraints.
 
 Use Default Constraints
 : Ties these constraints to the default constraints in the settings menu.
@@ -183,7 +188,7 @@ used. Constraint zones can be edited via the constraint zones tree.
 <img src="constraint_zones_tree.png" alt="constraint zones tree" border-effect="rounded"/>
 
 Constraints
-: Identical configuration as global constraints.
+: Identical configuration as global constraints. Minus the ability to use defaults or unlimited constraints.
 
 Start Position Slider
 : Controls the waypoint relative position of the start of the zone.

Writerside/topics/pplib-Create-a-Path-On-the-fly.md

@@ -32,10 +32,13 @@ List<Waypoint> waypoints = PathPlannerPath.waypointsFromPoses(
         new Pose2d(5.0, 3.0, Rotation2d.fromDegrees(90))
 );
 
+PathConstraints constraints = new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI); // The constraints for this path.
+// PathConstraints constraints = PathConstraints.unlimitedConstraints(12.0); // You can also use unlimited constraints, only limited by motor torque and nominal battery voltage
+
 // Create the path using the waypoints created above
 PathPlannerPath path = new PathPlannerPath(
         waypoints,
-        new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI), // The constraints for this path. If using a differential drivetrain, the angular constraints have no effect.
+        constraints,
         null, // The ideal starting state, this is only relevant for pre-planned paths, so can be null for on-the-fly paths.
         new GoalEndState(0.0, Rotation2d.fromDegrees(-90)) // Goal end state. You can set a holonomic rotation here. If using a differential drivetrain, the rotation will have no effect.
 );
@@ -62,11 +65,14 @@ std::vector<frc::Pose2d> poses{
 };
 std::vector<Waypoint> waypoints = PathPlannerPath::waypointsFromPoses(poses);
 
+PathConstraints constraints(3.0_mps, 3.0_mps_sq, 360_deg_per_s, 720_deg_per_s_sq); // The constraints for this path.
+// PathConstraints constraints = PathConstraints::unlimitedConstraints(12_V); // You can also use unlimited constraints, only limited by motor torque and nominal battery voltage
+
 // Create the path using the waypoints created above
 // We make a shared pointer here since the path following commands require a shared pointer
 auto path = std::make_shared<PathPlannerPath>(
     waypoints,
-    PathConstraints(3.0_mps, 3.0_mps_sq, 360_deg_per_s, 720_deg_per_s_sq), // The constraints for this path. If using a differential drivetrain, the angular constraints have no effect.
+    constraints,
     std::nullopt, // The ideal starting state, this is only relevant for pre-planned paths, so can be nullopt for on-the-fly paths.
     GoalEndState(0.0_mps, frc::Rotation2d(-90_deg)) // Goal end state. You can set a holonomic rotation here. If using a differential drivetrain, the rotation will have no effect.
 );
@@ -91,10 +97,13 @@ waypoints = PathPlannerPath.waypointsFromPoses(
     Pose2d(5.0, 3.0, Rotation2d.fromDegrees(90))
 )
 
+constraints = PathConstraints(3.0, 3.0, 2 * math.pi, 4 * math.pi) # The constraints for this path.
+# constraints = PathConstraints.unlimitedConstraints(12.0) # You can also use unlimited constraints, only limited by motor torque and nominal battery voltage
+
 # Create the path using the waypoints created above
 path = new PathPlannerPath(
     waypoints,
-    PathConstraints(3.0, 3.0, 2 * math.pi, 4 * math.pi), # The constraints for this path. If using a differential drivetrain, the angular constraints have no effect.
+    constraints,
     None, # The ideal starting state, this is only relevant for pre-planned paths, so can be None for on-the-fly paths.
     GoalEndState(0.0, Rotation2d.fromDegrees(-90)) # Goal end state. You can set a holonomic rotation here. If using a differential drivetrain, the rotation will have no effect.
 )

lib/pages/project/project_page.dart

@@ -265,7 +265,9 @@ class _ProjectPageState extends State<ProjectPage> {
 
       for (PathPlannerPath path in _paths) {
         if (path.useDefaultConstraints) {
-          path.globalConstraints = defaultConstraints.clone();
+          PathConstraints cloned = defaultConstraints.clone();
+          cloned.unlimited = path.globalConstraints.unlimited;
+          path.globalConstraints = cloned;
           path.generateAndSavePath();
         }
       }
@@ -1872,6 +1874,8 @@ class _ProjectPageState extends State<ProjectPage> {
       maxAngularAccelerationDeg:
           widget.prefs.getDouble(PrefsKeys.defaultMaxAngAccel) ??
               Defaults.defaultMaxAngAccel,
+      nominalVoltage: widget.prefs.getDouble(PrefsKeys.defaultNominalVoltage) ??
+          Defaults.defaultNominalVoltage,
     );
   }
 }

lib/path/path_constraints.dart

@@ -3,13 +3,15 @@ class PathConstraints {
   num maxAccelerationMPSSq;
   num maxAngularVelocityDeg;
   num maxAngularAccelerationDeg;
+  num nominalVoltage;
   bool unlimited;
 
   PathConstraints({
     this.maxVelocityMPS = 3,
     this.maxAccelerationMPSSq = 3,
     this.maxAngularVelocityDeg = 540,
     this.maxAngularAccelerationDeg = 720,
+    this.nominalVoltage = 12.0,
     this.unlimited = false,
   });
 
@@ -18,6 +20,7 @@ class PathConstraints {
         maxAccelerationMPSSq = json['maxAcceleration'] ?? 3,
         maxAngularVelocityDeg = json['maxAngularVelocity'] ?? 540,
         maxAngularAccelerationDeg = json['maxAngularAcceleration'] ?? 720,
+        nominalVoltage = json['nominalVoltage'] ?? 12.0,
         unlimited = json['unlimited'] ?? false;
 
   PathConstraints clone() {
@@ -26,6 +29,7 @@ class PathConstraints {
       maxAccelerationMPSSq: maxAccelerationMPSSq,
       maxAngularVelocityDeg: maxAngularVelocityDeg,
       maxAngularAccelerationDeg: maxAngularAccelerationDeg,
+      nominalVoltage: nominalVoltage,
       unlimited: unlimited,
     );
   }
@@ -36,6 +40,7 @@ class PathConstraints {
       'maxAcceleration': maxAccelerationMPSSq,
       'maxAngularVelocity': maxAngularVelocityDeg,
       'maxAngularAcceleration': maxAngularAccelerationDeg,
+      'nominalVoltage': nominalVoltage,
       'unlimited': unlimited,
     };
   }
@@ -47,10 +52,16 @@ class PathConstraints {
       other.maxVelocityMPS == maxVelocityMPS &&
       other.maxAccelerationMPSSq == maxAccelerationMPSSq &&
       other.maxAngularVelocityDeg == maxAngularVelocityDeg &&
-      other.maxAngularAccelerationDeg == other.maxAngularAccelerationDeg &&
+      other.maxAngularAccelerationDeg == maxAngularAccelerationDeg &&
+      other.nominalVoltage == nominalVoltage &&
       other.unlimited == unlimited;
 
   @override
-  int get hashCode => Object.hash(maxVelocityMPS, maxAccelerationMPSSq,
-      maxAngularVelocityDeg, maxAngularAccelerationDeg, unlimited);
+  int get hashCode => Object.hash(
+      maxVelocityMPS,
+      maxAccelerationMPSSq,
+      maxAngularVelocityDeg,
+      maxAngularAccelerationDeg,
+      nominalVoltage,
+      unlimited);
 }

lib/path/pathplanner_path.dart

@@ -376,6 +376,7 @@ class PathPlannerPath {
         maxAccelerationMPSSq: double.infinity,
         maxAngularVelocityDeg: double.infinity,
         maxAngularAccelerationDeg: double.infinity,
+        nominalVoltage: globalConstraints.nominalVoltage,
         unlimited: true,
       );
     }

lib/trajectory/trajectory.dart

@@ -173,8 +173,8 @@ class PathPlannerTrajectory {
         num lastVelRadPerSec =
             lastVel / robotConfig.moduleConfig.wheelRadiusMeters;
         num currentDraw = min(
-            robotConfig.moduleConfig.driveMotor
-                .getCurrent(lastVelRadPerSec, 12.0),
+            robotConfig.moduleConfig.driveMotor.getCurrent(
+                lastVelRadPerSec, states[i].constraints.nominalVoltage),
             robotConfig.moduleConfig.driveCurrentLimit);
         num availableTorque =
             robotConfig.moduleConfig.driveMotor.getTorque(currentDraw) -
@@ -322,8 +322,8 @@ class PathPlannerTrajectory {
         num lastVelRadPerSec =
             lastVel / robotConfig.moduleConfig.wheelRadiusMeters;
         num currentDraw = min(
-            robotConfig.moduleConfig.driveMotor
-                .getCurrent(lastVelRadPerSec, 12.0),
+            robotConfig.moduleConfig.driveMotor.getCurrent(
+                lastVelRadPerSec, states[i].constraints.nominalVoltage),
             robotConfig.moduleConfig.driveCurrentLimit);
         num availableTorque =
             robotConfig.moduleConfig.driveMotor.getTorque(currentDraw);

lib/util/prefs.dart

@@ -24,6 +24,7 @@ class PrefsKeys {
   static const String defaultMaxAccel = 'defaultMaxAccel';
   static const String defaultMaxAngVel = 'defaultMaxAngVel';
   static const String defaultMaxAngAccel = 'defaultMaxAngAccel';
+  static const String defaultNominalVoltage = 'defaultNominalVoltage';
   static const String seen2024Warning = 'seen2024Warning';
   static const String robotMass = 'robotMass';
   static const String robotMOI = 'robotMOI';
@@ -64,6 +65,7 @@ class Defaults {
   static const double defaultMaxAccel = 3.0;
   static const double defaultMaxAngVel = 540.0;
   static const double defaultMaxAngAccel = 720.0;
+  static const double defaultNominalVoltage = 12.0;
   static const double robotMass = 74.088;
   static const double robotMOI = 6.883;
   static const double robotWheelbase = 0.546;

lib/widgets/dialogs/settings_dialog.dart

@@ -53,6 +53,7 @@ class _SettingsDialogState extends State<SettingsDialog> {
   late num _defaultMaxAccel;
   late num _defaultMaxAngVel;
   late num _defaultMaxAngAccel;
+  late num _defaultNominalVoltage;
   late bool _holonomicMode;
   late bool _hotReload;
   late FieldImage _selectedField;
@@ -97,6 +98,9 @@ class _SettingsDialogState extends State<SettingsDialog> {
     _defaultMaxAngAccel =
         widget.prefs.getDouble(PrefsKeys.defaultMaxAngAccel) ??
             Defaults.defaultMaxAngAccel;
+    _defaultNominalVoltage =
+        widget.prefs.getDouble(PrefsKeys.defaultNominalVoltage) ??
+            Defaults.defaultNominalVoltage;
     _holonomicMode =
         widget.prefs.getBool(PrefsKeys.holonomicMode) ?? Defaults.holonomicMode;
     _hotReload = widget.prefs.getBool(PrefsKeys.hotReloadEnabled) ??
@@ -689,6 +693,35 @@ class _SettingsDialogState extends State<SettingsDialog> {
                     ],
                   ),
                   const SizedBox(height: 12),
+                  Column(
+                    children: [
+                      Row(
+                        children: [
+                          Expanded(
+                            child: NumberTextField(
+                              initialValue: _defaultNominalVoltage,
+                              label: 'Nominal Voltage (Volts)',
+                              minValue: 6.0,
+                              maxValue: 13.0,
+                              arrowKeyIncrement: 0.1,
+                              onSubmitted: (value) {
+                                if (value != null) {
+                                  widget.prefs.setDouble(
+                                      PrefsKeys.defaultNominalVoltage,
+                                      value.toDouble());
+                                  setState(() {
+                                    _defaultNominalVoltage = value;
+                                  });
+                                }
+                                widget.onSettingsChanged();
+                              },
+                            ),
+                          ),
+                        ],
+                      ),
+                    ],
+                  ),
+                  const SizedBox(height: 12),
                   Row(
                     mainAxisAlignment: MainAxisAlignment.spaceBetween,
                     crossAxisAlignment: CrossAxisAlignment.start,
@@ -698,7 +731,7 @@ class _SettingsDialogState extends State<SettingsDialog> {
                       Expanded(child: _buildTeamColorPicker(context)),
                     ],
                   ),
-                  const SizedBox(height: 12),
+                  const SizedBox(height: 2),
                   Column(
                     mainAxisSize: MainAxisSize.min,
                     crossAxisAlignment: CrossAxisAlignment.start,
@@ -732,9 +765,9 @@ class _SettingsDialogState extends State<SettingsDialog> {
                           ),
                         ],
                       ),
-                      const SizedBox(height: 12),
                     ],
                   ),
+                  const SizedBox(height: 6),
                   Column(
                     mainAxisSize: MainAxisSize.min,
                     crossAxisAlignment: CrossAxisAlignment.start,

lib/widgets/editor/tree_widgets/constraint_zones_tree.dart

@@ -320,6 +320,33 @@ class _ConstraintZonesTreeState extends State<ConstraintZonesTree> {
           ),
         ),
         const SizedBox(height: 12),
+        Padding(
+          padding: const EdgeInsets.symmetric(horizontal: 6.0),
+          child: Row(
+            children: [
+              Expanded(
+                child: NumberTextField(
+                  initialValue:
+                      constraintZones[zoneIdx].constraints.nominalVoltage,
+                  label: 'Nominal Voltage (Volts)',
+                  minValue: 6.0,
+                  maxValue: 13.0,
+                  arrowKeyIncrement: 0.1,
+                  onSubmitted: (value) {
+                    if (value != null) {
+                      _addConstraintsChange(
+                          zoneIdx,
+                          () => constraintZones[zoneIdx]
+                              .constraints
+                              .nominalVoltage = value);
+                    }
+                  },
+                ),
+              ),
+            ],
+          ),
+        ),
+        const SizedBox(height: 12),
         Row(
           children: [
             Expanded(

lib/widgets/editor/tree_widgets/global_constraints_tree.dart

@@ -113,6 +113,30 @@ class GlobalConstraintsTree extends StatelessWidget {
           ),
         ),
         const SizedBox(height: 12),
+        Padding(
+          padding: const EdgeInsets.symmetric(horizontal: 6.0),
+          child: Row(
+            children: [
+              Expanded(
+                child: NumberTextField(
+                  initialValue: path.globalConstraints.nominalVoltage,
+                  label: 'Nominal Voltage (Volts)',
+                  minValue: 6.0,
+                  maxValue: 13.0,
+                  arrowKeyIncrement: 0.1,
+                  enabled: !path.useDefaultConstraints,
+                  onSubmitted: (value) {
+                    if (value != null) {
+                      _addChange(
+                          () => path.globalConstraints.nominalVoltage = value);
+                    }
+                  },
+                ),
+              ),
+            ],
+          ),
+        ),
+        const SizedBox(height: 12),
         Padding(
           padding: const EdgeInsets.symmetric(horizontal: 6.0),
           child: Row(
@@ -122,27 +146,26 @@ class GlobalConstraintsTree extends StatelessWidget {
                   children: [
                     Checkbox(
                       value: path.useDefaultConstraints,
-                      onChanged: path.globalConstraints.unlimited
-                          ? null
-                          : (value) {
-                              undoStack.add(Change(
-                                (
-                                  path.useDefaultConstraints,
-                                  path.globalConstraints.clone()
-                                ),
-                                () {
-                                  path.useDefaultConstraints = value ?? false;
-                                  path.globalConstraints =
-                                      defaultConstraints.clone();
-                                  onPathChanged?.call();
-                                },
-                                (oldValue) {
-                                  path.useDefaultConstraints = oldValue.$1;
-                                  path.globalConstraints = oldValue.$2.clone();
-                                  onPathChanged?.call();
-                                },
-                              ));
-                            },
+                      onChanged: (value) {
+                        undoStack.add(Change(
+                          (
+                            path.useDefaultConstraints,
+                            path.globalConstraints.clone()
+                          ),
+                          () {
+                            path.useDefaultConstraints = value ?? false;
+                            PathConstraints cloned = defaultConstraints.clone();
+                            cloned.unlimited = path.globalConstraints.unlimited;
+                            path.globalConstraints = cloned;
+                            onPathChanged?.call();
+                          },
+                          (oldValue) {
+                            path.useDefaultConstraints = oldValue.$1;
+                            path.globalConstraints = oldValue.$2.clone();
+                            onPathChanged?.call();
+                          },
+                        ));
+                      },
                     ),
                     const SizedBox(width: 4),
                     const Text(