mBed code for motor test