working updated pid calculation stuff