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

home

Browse Source
This commit is contained in:
crinq
2020-02-25 04:38:03 +01:00
parent ff9473c4ec
commit ee37974ce8
2 changed files with 16 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
conf/move.txt
View File

@@ -21,12 +21,13 @@ conf0.l = 0.007
conf0.j = 0.000015
conf0.j_load = 0.00005
conf0.polecount = 5
conf0.max_ac_cur = 2.5
conf0.max_ac_cur = 3
conf0.mot_fb_res = 20000
conf0.max_force = 1.5
load hx
hx0.rt_prio = 1.5
hx0.gain = 200
hx0.gain = -200
hx0.time = 0.010000
term0.wave4 = hx0.out0
@@ -34,7 +35,7 @@ term0.wave4 = hx0.out0
load home
home0.rt_prio = 9
rev0.in = home0.pos_out
home0.home_vel = -6
home0.home_vel = -2
home0.en_in = en0.en_out1
home0.home_acc = 500
@@ -43,10 +44,7 @@ fmove0.rt_prio = 10
fmove0.en = home0.en_out
fmove0.force_in = hx0.out0
home0.pos_in = fmove0.mpos
fmove0.max_usr_vel = 0.1
fmove0.max_usr_acc = 0.1
term0.wave0 = fmove0.mpos
hx0.gain = 200
fmove0.damping = 0
fmove0.real_mass = 0
@@ -54,19 +52,22 @@ fmove0.virtual_mass = 5
fmove0.friction = 0.5
fmove0.min_pos = 0
fmove0.max_pos = 1
#fmove0.scale = 210
fmove0.scale = 300
fmove0.max_vel = 0.025
fmove0.max_acc = 0.15
fmove0.max_usr_vel = 0.2
fmove0.max_usr_vel = 0.15
fmove0.max_usr_acc = 5
fmove0.force_th = 2.5
fmove0.force_th = 2
term0.send_step = 0
fmove0.print_freq = 10
fmove0.force_offset_lpf = 0.001
conf0.j = 0.00003
conf0.vel_p = 1500
conf0.vel_p = 1000
conf0.vel_i = 1
conf0.pos_p = 10
#fmove0.print_freq = 0
#term0.send_step = 41
fault0.com_fb_error = 0
fault0.mot_fb_error = 0
@@ -82,6 +83,7 @@ th0.rt_prio = 4
th0.in = pid0.torque_cor_cmd
th0.min = -1
th0.max = 1
th0.in_lpf = 0.5
th0.in_lpf = 0.25
home0.home_in = th0.out_not
home0.home_pos = 1.5
home0.home_pos = 1.5
io0.fan = 1

7
shared/comps/home.c
View File

@@ -86,10 +86,10 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
}
break;
case 3: // set home pos
case 3: // go to home pos
vel = SIGN(PIN(home_offset) - PIN(offset)) * sqrtf(ABS(PIN(home_offset) - PIN(offset)) * 2.0 * PIN(home_acc));
if(ABS(PIN(offset) - PIN(home_offset)) < 0.0001 && PIN(vel) < PIN(home_vel) * 0.001){
if(ABS(PIN(offset) - PIN(home_offset)) < 0.01 && PIN(vel) < PIN(home_vel) * 0.01){
PIN(state) = 4;
}
break;
@@ -105,9 +105,8 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
PIN(vel) = CLAMP(vel, PIN(vel) - PIN(home_acc) * period, PIN(vel) + PIN(home_acc) * period);
PIN(offset) += PIN(vel) * period;
PIN(offset) = mod(PIN(offset));
PIN(pos_out) = mod(PIN(pos_in) + PIN(offset));
PIN(pos_out) = mod(PIN(pos_in) + mod(PIN(offset)));
}
hal_comp_t home_comp_struct = {