Added some UnitTest for Herculex motor

Not still functionnal