C Controller V1

Matlab controller improved for C compatibility.
C Controller implemented but full with bugs.