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)