changed some PID loops to match https://www.youtube.com/watch?v=Rj5G_qM1Wf0