java机器人转向不足

Question

我正在用java构建一个机器人，我正在实现一个转向系统，该系统计算两个坐标（x1，y1），（x2，y2）之间的角度，但是机器人目前正在转向不足，即如果我告诉它转向90度，它只会转向40度，因此我需要输入更多转向。机器人可以左转或右转，这取决于所需的转弯是否大于180。下面的代码工作，但只有当开始航向是在0度，所以我需要代码考虑到当前的位置

        double xDiff = x2 - x1;
        double yDiff = y2 - y1;
        double angle =  Math.toDegrees(Math.atan2(yDiff, xDiff));

        double currentAngle = 0; //is changed after first run 

        angle = (angle + 360) % 360; 

        angle = angle - currentAngle;

使角度介于-180和180之间

     if (angle > 180)  
         angle -= 360;

        if(angle < 0 )
        {
            angle = angle - 45;

        }
        else if(angle > 0)
        {
            angle = angle + 45;

        }