Almost finished with P control for joints