Configure all INPUT pins
Added more method to control the motor
Redefined the gains
Veery simple (and wrong!) stabilization logic