Assembly code to arduino

Hi,

I want to use code for servo calculation from this project: ELM - DC Servomotor Controller

I will use Uno or Mega instead of original attiny. Problem is to decode assembly code file.
I want to use position control algolithm with implemented torque limit and Back-EMF compensation.

Can somebody help me to rewrite original algolithm to arduino? It is not clear to me how to set torque limit
and how EG compensation gain is used in calculations.

I have part of calculation that i understand but im misssing end part.

P1 = KF * 256;
P0 = (max_vel * P1 * ecoder_ppr) / 15000;
P2 = KP * 256;
P3 = KI * 256;
P4 = (motor_voltage / motor_current) * rotor_resistance * 256;
P5 = (KG/(ecoder_ppr * motor_voltage)) * 3840 * 256;

// position regulator
Pos_err = ref_pos - encoder_pos;
PosC_out = constrain(Pos_err, -P0, P0);    // Velocity limit

// velocity regulator
Vel_err = PosC_out - (P1 * act_vel);       // Velocity feedback gain
VelC_out = P2 * Vel_err;                   // P gain

ITerm += P3 * vel_err;                     // I gain
VelC_int = constrain (ITerm, -P5, P5);     // Anti-windup  
VelC_out += ITerm;  

output  = VelC_out  >> 8;
output = constrain (output , -P5, P5);     // Torque limit

It is not possible to upload original code with .asm extension so i changed it to txt.

Thanx for any help.

smc3a.txt (15.6 KB)