From ee37974ce854f6c2e7b5e87557a92af7d200041f Mon Sep 17 00:00:00 2001
From: crinq <crinq@crinq.de>
Date: Tue, 25 Feb 2020 04:38:03 +0100
Subject: [PATCH] home

---
 conf/move.txt       | 24 +++++++++++++-----------
 shared/comps/home.c |  7 +++----
 2 files changed, 16 insertions(+), 15 deletions(-)

diff --git a/conf/move.txt b/conf/move.txt
index 60d73900..9097dde2 100644
--- a/conf/move.txt
+++ b/conf/move.txt
@@ -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
\ No newline at end of file
+home0.home_pos = 1.5
+io0.fan = 1
diff --git a/shared/comps/home.c b/shared/comps/home.c
index cc7dad45..9b86d8d2 100644
--- a/shared/comps/home.c
+++ b/shared/comps/home.c
@@ -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 = {