Thank you for your reply. I am a lab instructor at UofR, Canada and the course is about digital control. I can somehow keep sampling time constant (within an acceptable tolerance) with that stopwatch trick in above code and changing the priority of control thread to high. My original question left unanswered: How can I make SetPower() to work?
Anyway I was pressed with the time and moved to the other available library (LEGO MINDSTORMS EV3 API), but your answer on using SetPower() will be greatly appreciated, since I have found your library more reliable in reading sensors via USB.