Implement velocity and acceleration limited trajectories

With this feature, the major position mode commands still have the
same inputs, but when either velocity or acceleration limits are
engaged, their semantics change significantly.

Previously, if a command like "d pos 3 0 0.5" were issue, the
controller would instantaneously attempt to achieve position 3.  Now,
if instead "d pos 3 0 0.5 v1 a2" is issued, the controller will
gradually accelerate at 2rev/s^2, max out at a velocity of 1rev/s, and
then decelerate at 2rev/s^2 to come to a gentle stop at position 3.

The limits can be configured globally, or overidden on a per-command
basis in the same manner as the watchdog timeout or maximum torque.

docs/getting_started.md

@@ -60,7 +60,7 @@ are somewhat unorthodox, so as to be easily amenable to idempotent
 commands sent from a higher level controller.  Each command looks
 like:
 
- * Position: The desired position *right now* in revolutions
+ * Position: The desired position in revolutions
  * Velocity: The rate at which the desired position changes in
    revolutions / s
  * Maximum torque: Never use more than this amount of torque when controlling
@@ -70,22 +70,22 @@ like:
    from this target.
  * kp scale: Scale the proportional constant by this factor
  * kd scale: Scale the derivative constant by this factor
+ * Velocity limit override: If non-special, override the configured
+   velocity limit, which constrains how quickly the target position is
+   reached.
+ * Acceleration limit override: If non-special, override the
+   configured acceleration limit, which constrains how quickly the
+   target position is reached.
 
 Additionally, the position may be set as a "special value" (NaN for
 floating point and the debug interface, maximal negative for integer
 encodings).  In that case, the position selected is "wherever you are
 right now".
 
-Some limited amount of preprogrammed constant velocity trajectories
-can be emulated using an unset position and the stop position.  In
-that case, the sign of the velocity command is ignored, and is instead
-selected to point towards the stop position.  If that is the only
-command, or that command is repeated indefinitely, it will have the
-same effect of causing the controller to move to the stop position at
-a constant velocity.
-
 A pure velocity mode can be obtained by setting the kp scale to 0 (or
-permanently so by configuring the kp constant to 0).
+permanently so by configuring the kp constant to 0).  In this case,
+using the `servo.max_position_slip` configurable parameter may be
+valuable as per the [reference manual](reference.md#velocity-control).
 
 # Initial Configuration #
 
@@ -105,7 +105,7 @@ in your setup regardless:
 
 * `servopos.position_min` and `servopos.position_max` these define the bounds of motion which the controller will allow when in position control mode.  Attempting to start beyond this region will fault, and if outside the region in operation, no torque will be applied to go further outside.
 * `servo.max_current_A` the maximum phase current to apply to the motor.  This can be used to limit the maximum torque that the system is capable of regardless of any command sent.
-* `servo.max_velocity` provides a limit on the maximum speed in Hz. Development kits ship with a relatively low value for this, which you can increase for higher speed operation.
+* `servo.velocity_limit` / `servo.accel_limit` controls how fast the motor can accelerate and spin in order to reach position and velocity targets.  Bare boards ship with these unset, while development kits ship with human-eye pleasing values.
 * `servo.pid_position` the PID parameters for the position control loop.
 * `motor.unwrapped_position_scale` any gearbox scaling, a reducing gearbox should be configured with a number smaller than one, e.g. 0.25 for a 4x reduction gearbox.  This affects reported position, speed, and torques.
 * `id.id` the CAN-FD id used by this device

docs/reference.md

@@ -12,22 +12,27 @@ the three phases.
 
 The primary control mode, labeled as "position" mode in the rest of
 this document is a two stage cascaded controller, with both running at
-the switching frequency of 40kHz.  The outer stage is an integrated
-position/velocity PID controller with optional feedforward torque.
-The output of that loop is a desired torque/current for the Q phase of
-the FOC controller.  The inner stage is a current mode PI controller.
-Its output is the desired voltage value for the Q phase.  Then the
-magnetic encoder is used to map the D/Q phase voltage values to the 3
-phases of the motor.
+the switching frequency (by default 40kHz).
+
+The outermost stage is an optional limited acceleration and velocity
+trajectory planner.  Within that is an integrated position/velocity
+PID controller with optional feedforward torque.  The output of that
+loop is a desired torque/current for the Q phase of the FOC
+controller.
+
+The inner stage is a current mode PI controller.  Its output is the
+desired voltage value for the Q phase.  Then the magnetic encoder is
+used to map the D/Q phase voltage values to the 3 phases of the motor.
 
 ![Control Structure](control_structure.png)
 
 More precisely, the "Position Controller" implements the following
 control law:
 
 ```
+acceleration = trajectory_follower(command_position, command_velocity)
+control_velocity = command_velocity OR control_velocity + acceleration * dt OR 0.0
 control_position = command_position OR control_position + control_velocity * dt
-control_velocity = command_velocity OR control_velocity OR 0.0
 position_error = control_position - feedback_position
 velocity_error = control_velocity - feedback_velocity
 position_integrator = limit(position_integrator + ki * position_error * dt, ilimit)
@@ -79,27 +84,23 @@ torque (at the expense of overall torque bandwidth).  It may also be
 beneficial to select a lower value than default for `moteus_tool
 --cal-bw-hz` during calibration.
 
-### Internally Generated Trajectories ###
-
-moteus currently only supports constant velocity trajectories.
-Normally, the velocity commanded will continue indefinitely, either
-until a watchdog timeout occurs or the configured position limit is
-reached.
+### Constant Acceleration Trajectories ###
 
-Through the use of the optional "stop_position" of the position
-controller, moteus can set the velocity to zero when a specific
-control position is achieved.  This feature can be used in
-applications where the host commands the controller at a low update
-rate.
+Velocity and acceleration limits can be configured either globally, or
+on a per-command basis which will cause moteus to internally generate
+continuous acceleration limited trajectories to reach the given
+position and velocity.  Once the trajectory is complete, the command
+velocity is continued indefinitely.
 
-### Constant Acceleration or Jerk ###
+### Jerk Limited Trajectories ###
 
-moteus only supports constant velocity internal trajectories.  To
-approximate a constant acceleration or constant jerk trajectory, the
-host processor should send a sequence of piecewise linear constant
-velocity trajectories which approximate the desired one.  This would
-be done by sending commands consisting of at least a position and
-velocity at some moderate to high rate.
+moteus only supports acceleration limited internal trajectories.  To
+approximate a constant jerk trajectory, the host processor should send
+a sequence of piecewise linear constant velocity trajectories which
+approximate the desired one.  This would be done by sending commands
+consisting of at least a position and velocity at some moderate to
+high rate while disabling the internal velocity and acceleration
+limits.
 
 ### High Torque Bandwidth ###
 
@@ -273,6 +274,12 @@ values.
 - int16 => 1 LSB => 0.00025 Hz > 0.09 dps
 - int32 => 1 LSB => 0.00001 Hz => 0.0036 dps
 
+#### A.2.a.8 Acceleration (measured in revolutions / s^2) ####
+
+- int8 => 1 LSB => 0.05 l/s^2
+- int16 => 1 LSB => 0.001 l/s^2
+- int32 => 1 LSB => 0.00001 l/s^2
+
 ### A.2.b Registers ###
 
 #### 0x000 - Mode ####
@@ -336,6 +343,19 @@ Mode: Read only
 If an absolute encoder is configured on the ABS port, its value will
 be reported here in revolutions.
 
+#### 0x00b - Trajectory complete ####
+
+Mode: Read only
+
+Non-zero if the current acceleration or velocity limited trajectory is
+complete, and the controller is following the final velocity.
+
+#### 0x00c - Rezero state ####
+
+Mode: Read only
+
+Non-zero if the controller has been rezeroed since power on.
+
 #### 0x00d - Voltage ####
 
 Mode: Read only
@@ -510,6 +530,23 @@ controller will enter the Timeout state.  The default is 0.0, which
 means to use the system-wide configured default.  NaN / maximally
 negative means apply no enforced timeout.
 
+#### 0x028 - Velocity limit ####
+
+Mode: Read/write
+
+This can be used to override the global velocity limit for internally
+generated trajectories.  If unspecified, it is NaN / maximally
+negative, which implies to use the global configurable default.
+
+#### 0x029 - Acceleration limit ####
+
+Mode: Read/write
+
+This can be used to override the global acceleration limit for
+internally generated trajectories.  If unspecified, it is NaN /
+maximally negative, which implies to use the global configurable
+default.
+
 ### 0x030 - Proportional torque ###
 
 Mode: Read
@@ -754,6 +791,10 @@ Each optional element consists of a prefix character followed by a value.  Permi
 - `f` - feedforward torque in Nm
 - `t` - timeout: If another command is not received in this many
   seconds, enter the timeout mode.
+- `v` - velocity limit: the given value will override the global
+  velocity limit for the duration of this command.
+- `a` - acceleration limit: the given value will override the global
+  acceleration limit for the duration of this command.
 
 The position, velocity, maximum torque, and all optional fields have
 the same semantics as for the register protocol documented above.
@@ -1020,6 +1061,40 @@ thus *after* any scaling in position, velocity, and torque implied by
 These have the same semantics as the position mode PID controller, and
 affect the current control loop.
 
+## `servo.default_velocity_limit` / `servo.default_accel_limit` ##
+
+Limits to be placed on trajectories generated within moteus.  If
+either is `nan`, then that limit is unset.  The limits may also be
+overriden individually on a per command basis.  The semantics of the
+limits are as follows:
+
+- *Neither set (both nan)* In this case, position and velocity
+  commands take immediate effect.  The control position will be
+  initialized to the command position, and the control velocity will
+  be set to the command velocity.  The control position will advance
+  at the given velocity indefinitely, or until the command stop
+  position is reached.
+
+- *Only velocity limit set*: In this case, until the desired position
+  is reached, the control velocity will be set to either the positive
+  or negative velocity limit.  Once the desired position has been
+  reached, then the velocity will continue indefinitely at the command
+  velocity.
+
+- *Both set*: In this case, neither the command position nor the
+  command velocity are immediately applied.  Instead, the control
+  velocity is advanced by the configured acceleration each timestep
+  while being limited to the maximum configured velocity in order to
+  achieve the desired position and velocity.  Once the desired
+  position and velocity have been achieved, then that final velocity
+  is continued indefinitely.  Note that this may require "back
+  tracking" if the current and final velocities do not permit an
+  approach in a single pass.
+
+- *Only acceleration limit set*: This behaves like the "both set"
+  case, except that the control velocity is allowed to increase or
+  decrease arbitrarily.
+
 ## `servo.voltage_mode_control` ##
 
 When set to non-zero, the current control loop is not closed, and all
@@ -1117,6 +1192,10 @@ beyond the factory configured value can result in hardware damage.
 
 Output power will be limited if the velocity exceeds this threshold.
 
+NOTE: If this value is lower than the configured
+`servo.velocity_limit`, then the resulting behavior is unlikely to be
+useful.
+
 ## `servo.max_velocity_derate` ##
 
 Once velocity reaches the max_velocity plus this value, allowed output

fw/bldc_servo.cc

@@ -377,6 +377,12 @@ class BldcServo::Impl {
     if (next->timeout_s == 0.0f) {
       next->timeout_s = config_.default_timeout_s;
     }
+    if (std::isnan(next->velocity_limit)) {
+      next->velocity_limit = config_.default_velocity_limit;
+    }
+    if (std::isnan(next->accel_limit)) {
+      next->accel_limit = config_.default_accel_limit;
+    }
 
     telemetry_data_ = *next;
 
@@ -1266,6 +1272,7 @@ class BldcServo::Impl {
     if (!position_pid_active || force_clear == kAlwaysClear) {
       status_.pid_position.Clear();
       status_.control_position = {};
+      status_.control_velocity = {};
     }
   }
 
@@ -1885,6 +1892,7 @@ class BldcServo::Impl {
     if (!target_position) {
       status_.pid_position.Clear();
       status_.control_position = {};
+      status_.control_velocity = {};
 
       // In this region, we still apply feedforward torques if they
       // are present.