void rotate(Side side)
{
if (!arrived_r)
{
if (side == Side.right)
{
distance_r = motorImpulseToDistance(motorLeft.getRotationTicks());
}
else
{
distance_r = motorImpulseToDistance(motorRight.getRotationTicks());
}
e_r = distance_wanted_r - distance_r;
u_r = Kp_r * e_r + Kd_r * (e_r - e_prev_r);
if (u_r > 15f)
{
u_r = 15f;
}
}
else
{
u_r = 0;
movFinished = true;
}
if (e_r <= e_r_expected && e_r >= -e_r_expected)
{
arrived_r = true;
}
if (u_r == 0)
{
motorLeft.setBrake(true);
motorRight.setBrake(true);
arrived_r = true;
}
else
{
motorLeft.setBrake(false);
motorRight.setBrake(false);
}
if (side == Side.right)
{
motorLeft.setSpeedPercent((float)u_r);
motorRight.setSpeedPercent(-(float)u_r);
}
else if (side == Side.left)
{
motorLeft.setSpeedPercent(-(float)u_r);
motorRight.setSpeedPercent((float)u_r);
}
e_prev_r = e_r;
}