Okay, I've left the map function out and now looks like at least the algorithm is doing something. The DC motor accelerates, stops, accelerates again, and keeps oscillating. I've tried using some pid tuning methods I've found on web, but looks like I'm not getting anywhere. First, I let Kp = 1, and Kd and ki = 0. With Kp = 1.5 and Ki = 2, looks like the system oscillates less.