Changed the motor test program
Position control added