fixed a bug with lag in the p loop. Need to impliment i and d as p is causing a large bouncing problem