inv_sqrt_controller calculates the inverse of the sqrt controller. This function calculates the input (aka error) to the sqrt_controller required to achieve a given output.
| 447 | // inv_sqrt_controller calculates the inverse of the sqrt controller. |
| 448 | // This function calculates the input (aka error) to the sqrt_controller required to achieve a given output. |
| 449 | float inv_sqrt_controller(float output, float p, float D_max) |
| 450 | { |
| 451 | if (is_positive(D_max) && is_zero(p)) { |
| 452 | return (output * output) / (2.0 * D_max); |
| 453 | } |
| 454 | if ((is_negative(D_max) || is_zero(D_max)) && !is_zero(p)) { |
| 455 | return output / p; |
| 456 | } |
| 457 | if ((is_negative(D_max) || is_zero(D_max)) && is_zero(p)) { |
| 458 | return 0.0; |
| 459 | } |
| 460 | |
| 461 | // calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function. |
| 462 | const float linear_velocity = D_max / p; |
| 463 | |
| 464 | if (fabsf(output) < linear_velocity) { |
| 465 | // if our current velocity is below the cross-over point we use a linear function |
| 466 | return output / p; |
| 467 | } |
| 468 | |
| 469 | const float linear_dist = D_max / sq(p); |
| 470 | const float stopping_dist = (linear_dist * 0.5f) + sq(output) / (2.0 * D_max); |
| 471 | return is_positive(output) ? stopping_dist : -stopping_dist; |
| 472 | } |
| 473 | |
| 474 | // stopping_distance calculates the stopping distance for the square root controller based deceleration path. |
| 475 | float stopping_distance(float velocity, float p, float accel_max) |