symulacja trajektoti wrzeciona w Simulinku

Witam.
Mam zarazem pytanie jak i prośbę dla osób które miały z tym do czynienie.
Mam za zadanie wyznaczyc trajektorie koncówki roboczej maszyny cnc wykorzystując zadanie proste i odwrotne kinematykinp podczas wyykonywania walca w materiale.
Model predkosci ipredkosci vx i Vy mam wyznaczone.

Wstępnie założyłem że przesuniecie na osi z jest o pena wartośc w czasie.
Natomiast na płaszczyznie x,y sytuacja wspólrzednych przedstawia sie nastepująco:
x=r0+ r*sin(fi);
y=r0+r*cos(fi); war. poczatkowe =0

nastepnie obliczając pochodną otrzymuje:

Vx=r*fi'*cos(fi);
Vy=-r*fi'* sin(fi);

I tu zaczynają sie schody:
- powinienem wyznaczyc z równan fi' nastepnie podłaczając integrator ma fi
- znając kat moge podstawic do równan Vx i vy
obliczjac w równaniu fi' sie skaraca


Z zwiazku z tym moja prośba . Miał ktoś do czynienia z symulacją cnc w simulinku.
Dziekuje pozdrawiam