# Java: Motion Profiling

Motion profiling allows the robot to drive in S-curves, which are awesome. For more resources on motion profiling, see the 254 championships presentation video and their source code, or our own code from 2016. Note that motion profiling requires calculus, so a solid mathematical background is recommended.

## Path Finding

A path is the set of positions that the robot will move through. We model the path with a cubic spline (some teams use quintic splines, but they are just showoffs). To define the spline, we create a variable, which we will call s, that increases from zero at the beginning of the path to one at the end of the path. We can then describe the x and y positions of the robot as 3rd degree polynomial functions of s:

```  x(s) = Axs3 + Bxs2 + Cxs + Dx
y(s) = Ays3 + Bys2 + Cys + Dy
```

Now, we need to solve for the 8 constants to find the actual equations. We can plug in s = 0 for the initial position, s = 1 for the final position, as well as take the derivatives of both equations with respect to s and plug in s = 0 and s = 1 again for the initial and final velocity. Now we have 8 unknown constants and 8 equations, so we can solve algebraically. This yields the following values for the 8 constants. The subscripts zero and one represent the initial and final values respectively.

``` Ax = x'1 + x'0 - 2*(x1-x0)
Bx = -x'1 - 2*x'0 + 3*(x1-x0)
Cx = x'0
Dx = x0
And likewise for the y constants
```

Note that these velocity values are with respect to s, not to time. Thus the initial and final "velocity" values are merely used to specify the initial and final direction that the robot is facing, as well as the "curviness" of the curve. We have found experimentally that setting the magnitude of the velocity to be twice the net displacement generates the nicest looking curves. The formulas for the initial and final velocity can then be calculated in terms of the bearing theta (measured clockwise).

``` d = sqrt((x1-x0)2 + (y1-y0)2)
x'0 = 2*d*sin(theta0)
y'0 = 2*d*cos(theta0)
x'1 = 2*d*sin(theta1)
y'1 = 2*d*cos(theta1)
```

## Trajectory Finding

A trajectory is like a path, except that each point has a specific time value associated with it. The ideal trajectory for a given path will travel at the maximum possible velocity at each point in time. To model a trajectory in code, we choose a set of sample points on the curve and assign a velocity to each one. However, that velocity is limited by numerous constraints, including the robot's maximum linear velocity, linear acceleration, angular velocity, and angular acceleration. Therefore, at each point the robot should travel at the maximum velocity that meets all four of these limitations.

### Finding Robot Maximum Values

We can determine the maximum velocity and acceleration of the robot experimentally by graphing velocity on SmartDashboard and finding the slope and maximum value of the velocity vs time graph. The same procedure also works for finding angular velocity and acceleration. It is preferable to choose a slightly smaller value than the actual maximum, so that if the performance of the robot changes the error can be corrected by PID.

### Velocity Limit due to Linear Acceleration

The main limitation to the velocity is that it takes time to accelerate and decelerate from the robot's max speed. The max speed and acceleration vary along the curve due to other factors, so the easiest way to determine the maximum achievable velocity at each point is by going incrementally through the set of sample points. At each point, compute the maximum velocity allowed by the other constraints. Then, if the robot is traveling slower than the maximum velocity for that point, the acceleration should be at the maximum allowed value, and if the robot is traveling at or above the maximum velocity the acceleration should be zero. Additionally, if the robot is traveling faster than the maximum velocity then the velocity should be set to the maximum velocity value.

To actually compute the new velocity value, we can use the following kinematics equation from physics: vf = sqrt(vi2 + 2*a*(x1-x0))

The linear displacement can be obtained by multiplying the change in arc length with respect to s by the size of the s-step between points, and thus the equation can be rewritten to express the new velocity: vn = sqrt(vn-12 + 2*amax*(1.0/nmax*dL/ds))

While this approach will limit the velocities to those that can be reached given the robot's acceleration, it does not yet account for the time needed to decelerate when the maximum velocity decreases. However, this can be accomplished by carrying out the same process in reverse. This time we start at the end of the path and for each point if the robot is traveling slower than the maximum velocity then the acceleration should be at maximum, and if the robot is traveling at or above the maximum velocity the acceleration should be zero. Additionally, if the previously computed velocity value is lower than the new velocity value, then the previous value will be used instead.

### Velocity Limit due to Angular Velocity

When using tank drive (i.e. the robot turns by rotating the left and right wheels at different speeds), the linear and angular velocity outputs are arithmetic reciprocals. The reason for this is that the linear velocity is proportional to the average motor output, whereas the angular velocity is proportional to the difference between the left and right motor output.

``` v = vmax*(Left+Right)/2
w = wmax*(Left-Right)/2
At maximum output: v = vmax - w*(vmax/wmax)
```

From these equations, and the fact that dƟ/dt = dƟ/dL * dL/dt, we can derive a limit on the linear velocity based on the change in angle with respect to s along the path. vmax1 = vmax/(1 + dtheta/dL*vmax/wmax)

### Velocity Limit due to Angular Acceleration

As velocity increases, so will the angular acceleration. Using the relationship that alpha is change in angular velocity with respect to time, we can then derive a limit on the velocity:

``` alpha = dw/dt = dw/dL * dL/dt = d/dL(dtheta/dt) * dL/dt
= d/dL(dtheta/dL * dL/dt) * dL/dt
Assume constant velocity: alpha = d2theta/dL2 * (dL/dt)2
v = dL/dt = sqrt(alpha / d2theta/dL2)
vmax2 = sqrt(alphamax / d2theta/dL2)
```

Note that this formula assumes zero acceleration. However, that is okay, because we will later calculate the maximum linear acceleration, so if necessary the acceleration will be limited to zero.

### Acceleration Limit due to Angular Acceleration

In addition to the limitations on the linear velocity, there is also a limit to the linear acceleration as a result of the robot's maximum angular acceleration. We have so far been unable to solve for the limit analytically, and instead used a brute force numerical approach. However, once we develop a more elegant solution we will document it here.

## Trajectory Following

Now that the robot's trajectory has been determined, the final step is to make the robot actually travel at the proper velocity for each point on the trajectory. This can be accomplished by assuming output velocity is proportional to motor output and adding an acceleration term and velocity PID control to correct for any error. For more information on using feedback control, see the PID Control page. The code itself is fairly straightforward.

``` outputV = velocityPID.getOutput() + kV*targetV + kA*targetA;
outputW = angularVelocityPID.getOutput() + kW*targetW + kAlpha*targetAlpha;