帮我用Codesys写一个求偏置曲柄滑块速度的函数

以下是用Codesys写的求偏置曲柄滑块速度的函数：

FUNCTION_BLOCK BiasCrankSliderVelocity
VAR_INPUT
    crankAngle: REAL; // 曲柄角度，单位：弧度
    sliderPosition: REAL; // 滑块位置，单位：米
    crankLength: REAL; // 曲柄长度，单位：米
    connectingRodLength: REAL; // 连杆长度，单位：米
    angularVelocity: REAL; // 曲柄角速度，单位：弧度/秒
END_VAR
VAR_OUTPUT
    sliderVelocity: REAL; // 滑块速度，单位：米/秒
END_VAR

VAR
    sliderDistance: REAL; // 滑块与曲柄轴心距离，单位：米
    crankCenterVelocity: REAL; // 曲柄中心速度，单位：米/秒
    connectingRodAngle: REAL; // 连杆与水平方向夹角，单位：弧度
    connectingRodVelocity: REAL; // 连杆速度，单位：米/秒
END_VAR

// 计算滑块与曲柄轴心距离
sliderDistance := sqrt(crankLength^2 - sliderPosition^2);

// 计算曲柄中心速度
crankCenterVelocity := crankLength * angularVelocity;

// 计算连杆与水平方向夹角
connectingRodAngle := arccos((sliderDistance^2 + connectingRodLength^2 - crankLength^2) / (2 * sliderDistance * connectingRodLength));

// 计算连杆速度
connectingRodVelocity := sqrt(crankCenterVelocity^2 + sliderDistance^2 * angularVelocity^2 - 2 * crankCenterVelocity * sliderDistance * angularVelocity * cos(connectingRodAngle));

// 计算滑块速度
sliderVelocity := connectingRodVelocity * sin(connectingRodAngle);

END_FUNCTION_BLOCK

使用方法：

将该函数保存为函数块，然后在程序中调用该函数，传入相应的参数即可求得偏置曲柄滑块速度。其中，crankAngle为曲柄角度，sliderPosition为滑块位置，crankLength为曲柄长度，connectingRodLength为连杆长度，angularVelocity为曲柄角速度。函数返回值为滑块速度。