Removed debug printing
Created Param class (everything that has to be changed must go here)
Realized that both Controllers were acutally the same (but taking into
account the Inerty matrix)