JCurvatureDrive.h

Go to the documentation of this file.

#ifndef J_CURVATURE_DRIVE_H
#define J_CURVATURE_DRIVE_H
#include "JTwoDTransform.h"
#include <Arduino.h>
 
namespace JCurvatureDrive {
inline JTwoDTransform calculate(bool curvatureEnabled, JTwoDTransform input, float minRadius, bool reverseTurning = false)
{
    JTwoDTransform output = input;
    if (reverseTurning)
        output.theta = -output.theta;
    if (curvatureEnabled) {
        float speed = sqrt(sq(input.x) + sq(input.y));
        output.theta = output.theta * speed / minRadius;
    }
 
    return output;
}
 
} // end of namespace
#endif