mirror of https://github.com/rene-dev/stmbl.git
54 lines
722 B
Plaintext
54 lines
722 B
Plaintext
component lin2rev "Calculate motor angle from position in machine units";
|
|
pin in float scale;
|
|
|
|
pin in float cmd_in;
|
|
pin out float cmd_out;
|
|
|
|
pin in float fb_in;
|
|
pin out float fb_out;
|
|
|
|
variable double pi = 3.141592654;
|
|
variable double s;
|
|
variable int lastq = 0;
|
|
variable int q = 0;
|
|
variable int rev = 0;
|
|
|
|
function _;
|
|
license "GPL";
|
|
;;
|
|
|
|
s = scale;
|
|
if(s == 0){
|
|
s = 1;
|
|
}
|
|
|
|
cmd_out = (cmd_in/s)*2.0*pi;
|
|
|
|
while(cmd_out < -pi){
|
|
cmd_out += 2.0 * pi;
|
|
}
|
|
|
|
while(cmd_out > pi){
|
|
cmd_out -= 2.0 * pi;
|
|
}
|
|
|
|
q = 0;
|
|
if(fb_in > pi/2.0){
|
|
q = 2;
|
|
}
|
|
if(fb_in < -pi/2.0){
|
|
q = 3;
|
|
}
|
|
|
|
if(q != 0 && q == 3 && lastq == 2){
|
|
rev++;
|
|
}
|
|
|
|
if(q != 0 && q == 2 && lastq == 3){
|
|
rev--;
|
|
}
|
|
|
|
lastq = q;
|
|
|
|
fb_out = ((fb_in+rev*pi*2.0)*s)/(2.0*pi);
|