Walking algorithm with PID Update Cycle - Sensors were not working very well so I did a quick hack