Simulando...Simulink

Bueno aquí van las primeras capturas de las simulaciones:

Aún queda mucho por desarrollar y comprobar, pero mis primeros controles están dando los resultados esperados.