C Controller V4

Apparently all bugs fixed. Unoptimized first working version. Still
takes twice of time than Matlab to compute every cycle.