Adding angle calcule and control
Adding Arduino code to read and control both motors