diff --git a/conf/template/iddc.txt b/conf/template/iddc.txt
index 014e9292..933ebc91 100644
--- a/conf/template/iddc.txt
+++ b/conf/template/iddc.txt
@@ -23,7 +23,6 @@ iddc0.pwm_volt = hv0.pwm_volt
 iddc0.pos_fb = fb_switch0.pos_fb
 iddc0.vel_fb = vel1.vel
 
-vel0.pos_in = fb_switch0.pos_fb
 iddc0.vel_fb = vel1.vel
 
 iddc0.test_cur = conf0.max_ac_cur
\ No newline at end of file
diff --git a/conf/template/idpmsm.txt b/conf/template/idpmsm.txt
index c7655efa..edea5705 100644
--- a/conf/template/idpmsm.txt
+++ b/conf/template/idpmsm.txt
@@ -13,8 +13,8 @@ hv0.d_cmd = idpmsm0.d_cmd
 hv0.q_cmd = idpmsm0.q_cmd
 hv0.pos = idpmsm0.com_pos
 hv0.rev = idpmsm0.out_rev
-hv0.r = idq0.r
-hv0.l = idq0.l
+hv0.r = idpmsm0.r
+hv0.l = idpmsm0.l
 idpmsm0.ud_fb = hv0.ud_fb
 idpmsm0.uq_fb = hv0.uq_fb
 idpmsm0.id_fb = hv0.id_fb
@@ -24,7 +24,6 @@ idpmsm0.pwm_volt = hv0.pwm_volt
 idpmsm0.pos_fb = fb_switch0.mot_abs_fb_no_offset
 idpmsm0.vel_fb = vel1.vel
 
-vel0.pos_in = fb_switch0.pos_fb
 idpmsm0.vel_fb = vel1.vel
 
 idpmsm0.test_cur = conf0.max_ac_cur
\ No newline at end of file
diff --git a/shared/comps/idpmsm.c b/shared/comps/idpmsm.c
index cb9802e5..c126fa7e 100644
--- a/shared/comps/idpmsm.c
+++ b/shared/comps/idpmsm.c
@@ -236,8 +236,8 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
 
     case 22: // pp, out_rev
       PIN(en_out) = 1.0;
-      PIN(cmd_mode) = 0.0;
-      PIN(cur_bw) = 1.0;
+      PIN(cmd_mode) = 1.0;
+      PIN(cur_bw) = 100.0;
       PIN(q_cmd) = 0.0;
 
       PIN(d_cmd) = PIN(test_cur);
@@ -246,7 +246,7 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
       PIN(com_pos) = mod(PIN(com_pos));
 
       if(ABS(PIN(vel_fb)) > 0.1){
-        PIN(pp) = PIN(pp) * 0.99 + PIN(test_vel) / PIN(vel_fb) * 0.01;
+        PIN(pp) = PIN(pp) * 0.995 + PIN(test_vel) / PIN(vel_fb) * 0.005;
       }
 
       PIN(timer) += period;
@@ -263,17 +263,17 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
       }
     break;
 
-    case 23:
+    case 23: // mot_fb_offset
       PIN(en_out) = 1.0;
-      PIN(cmd_mode) = 0.0;
-      PIN(cur_bw) = 1.0;
+      PIN(cmd_mode) = 1.0;
+      PIN(cur_bw) = 100.0;
       PIN(q_cmd) = 0.0;
 
-      PIN(d_cmd) += PIN(ki) * period * PIN(r) * (PIN(test_cur) - PIN(id_fb));
-      PIN(d_cmd) = LIMIT(PIN(d_cmd), PIN(pwm_volt));
+      PIN(d_cmd) = PIN(test_cur);
+
       PIN(com_pos) = 0.0;
 
-      PIN(com_offset) = PIN(com_offset) * 0.95 - PIN(pos_fb) * 0.05;
+      PIN(com_offset) = PIN(com_offset) * 0.99 - PIN(pos_fb) * 0.01;
 
       PIN(timer) += period;
       if(PIN(timer) >= 2.0){