Finished implementing control algorithm
Added integral term to the formerly PD controller