Implement a Energy based controller to make it perform a swing up

[PeriniM]

Create a new class in the pid control folder to implement the controller which can be found in the following papers:

• Swinging up a pendulum by energy control

• Swing up and stabilization control of rotary inverted pendulum based on energy balance, fuzzy logic, and LQR
  controllers

• Swing Up and Stabilization Control of a Rotary Inverted Pendulum