Mirrors_Framework/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2026-02-06 02:02:34 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

fb_switch offset rev

Browse Source
This commit is contained in:
crinq
2017-10-02 22:27:34 +02:00
parent 8a2d270277
commit 8bff2467e4
2 changed files with 26 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
conf/p50b.txt Normal file
View File

@@ -0,0 +1,13 @@
link pid
link pmsm
link enc_fb0
link misc
conf0.r = 0.4
conf0.l = 0.01
conf0.j = 0.0001
conf0.polecount = 2
conf0.max_force = 5.7
conf0.max_ac_cur = 5
conf0.mot_fb_res = 16384
conf0.mot_fb_offset = -0.57
conf0.mot_fb_rev = 1

18
shared/comps/fb_switch.c
View File

@@ -74,19 +74,27 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
float com_abs_pos = PIN(com_abs_pos);
float joint_abs_pos = PIN(joint_abs_pos);
float mot_offset = PIN(mot_offset);
float com_offset = PIN(com_offset);
float joint_offset = PIN(joint_offset);
if(PIN(mot_rev) > 0.0){
mot_pos *= -1.0;
mot_abs_pos *= -1.0;
mot_offset *= -1.0;
}
if(PIN(com_rev) > 0.0){
com_pos *= -1.0;
com_abs_pos *= -1.0;
com_offset *= -1.0;
}
if(PIN(joint_rev) > 0.0){
joint_pos *= -1.0;
joint_abs_pos *= -1.0;
joint_offset *= -1.0;
}
PIN(pos_fb) = mod(mot_pos + ctx->cmd_offset);
@@ -102,11 +110,11 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
PIN(state) = 1.0;
if(PIN(joint_state) >= 2.0 && ctx->current_com_pos > 3.0) {
ctx->current_com_pos = 3;
ctx->com_offset = minus(mod((joint_abs_pos + PIN(joint_offset)) * PIN(polecount) / PIN(mot_joint_ratio)), mod(mot_pos * PIN(polecount) / PIN(mot_polecount)));
ctx->com_offset = minus(mod((joint_abs_pos + joint_offset) * PIN(polecount) / PIN(mot_joint_ratio)), mod(mot_pos * PIN(polecount) / PIN(mot_polecount)));
}
if(PIN(com_state) >= 2.0 && ctx->current_com_pos > 2.0) {
ctx->current_com_pos = 2;
ctx->com_offset = minus(mod((com_abs_pos + PIN(com_offset)) * PIN(polecount) / PIN(com_polecount)), mod(mot_pos * PIN(polecount) / PIN(mot_polecount)));
ctx->com_offset = minus(mod((com_abs_pos + com_offset) * PIN(polecount) / PIN(com_polecount)), mod(mot_pos * PIN(polecount) / PIN(mot_polecount)));
}
if(PIN(mot_state) >= 2.0 && ctx->current_com_pos > 1.0) {
ctx->current_com_pos = 1;
@@ -130,7 +138,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
if(PIN(joint_state) != 3.0) {
PIN(com_fb) = mod(mot_pos * PIN(polecount) / PIN(mot_polecount) + ctx->com_offset);
} else {
PIN(com_fb) = mod((joint_abs_pos + PIN(joint_offset)) * PIN(polecount));
PIN(com_fb) = mod((joint_abs_pos + joint_offset) * PIN(polecount));
}
break;
@@ -138,7 +146,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
if(PIN(com_state) != 3.0) {
PIN(com_fb) = mod(mot_pos * PIN(polecount) / PIN(mot_polecount) + ctx->com_offset);
} else {
PIN(com_fb) = mod((com_abs_pos + PIN(com_offset)) * PIN(polecount) / PIN(com_polecount));
PIN(com_fb) = mod((com_abs_pos + com_offset) * PIN(polecount) / PIN(com_polecount));
}
break;
@@ -146,7 +154,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
if(PIN(mot_state) != 3.0) {
PIN(state) = 0.0;
} else {
PIN(com_fb) = mod((mot_abs_pos + PIN(mot_offset)) * PIN(polecount) / PIN(mot_polecount));
PIN(com_fb) = mod((mot_abs_pos + mot_offset) * PIN(polecount) / PIN(mot_polecount));
}
break;