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; }