Created funktion to control leg 1
Merge branch 'master' of https://github.com/samli177/S.M.A.U.G