diff --git a/shared/comps/iddc.c b/shared/comps/iddc.c index a0158a1b..1a99b7d8 100644 --- a/shared/comps/iddc.c +++ b/shared/comps/iddc.c @@ -22,7 +22,6 @@ HAL_PIN(timer); HAL_PIN(r); HAL_PIN(l); -HAL_PIN(pp); HAL_PIN(com_offset); HAL_PIN(out_rev); @@ -66,7 +65,6 @@ static void nrt(void *ctx_ptr, hal_pin_inst_t *pin_ptr) { PIN(r) = 0.1; PIN(l) = 0.001; PIN(psi) = 0.055; - PIN(pp) = 3.0; PIN(com_offset) = 0.0; PIN(out_rev) = 0.0; PIN(cur_bw) = 1.0; @@ -196,14 +194,14 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { PIN(cur_bw) = 250.0; float vel_error = PIN(test_vel) - ABS(PIN(vel_fb)); - vel_error = LIMIT(vel_error, PIN(test_vel) / 10.0); + vel_error = LIMIT(vel_error, PIN(test_vel) / 100.0); PIN(cur_sum) += PIN(ki) * vel_error * period; PIN(q_cmd) = PIN(vel_bw) * period * vel_error + PIN(cur_sum); if(ABS(PIN(vel_fb)) > 0.1){ - float psi = (PIN(uq_fb) - PIN(iq_fb) * PIN(r)) / (PIN(vel_fb) * PIN(pp)); + float psi = (PIN(uq_fb) - PIN(iq_fb) * PIN(r)) / (PIN(vel_fb)); PIN(psi) = PIN(psi) * (1.0 - period / PIN(pi)) + psi * period / PIN(pi); } diff --git a/shared/comps/idm.c b/shared/comps/idm.c index e63a851b..82ef7772 100644 --- a/shared/comps/idm.c +++ b/shared/comps/idm.c @@ -54,10 +54,10 @@ static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) { PIN(ji) = 200.0; PIN(di) = 10.0; PIN(pos_bw) = 5.0; - PIN(vel_bw) = 25.0; + PIN(vel_bw) = 40.0; PIN(vel_d) = 1.0; - PIN(max_vel) = 50.0; - PIN(max_acc) = 1000.0; + PIN(max_vel) = 25.0; + PIN(max_acc) = 250.0; PIN(min_pos) = -10.0; PIN(max_pos) = 10.0; PIN(auto_step) = 1.4; diff --git a/shared/comps/idpmsm.c b/shared/comps/idpmsm.c index 7eb5f462..1a4de65d 100644 --- a/shared/comps/idpmsm.c +++ b/shared/comps/idpmsm.c @@ -292,6 +292,7 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { PIN(com_pos) = mod((PIN(pos_fb) + PIN(com_offset)) * PIN(pp)); float vel_error = PIN(test_vel) - PIN(vel_fb); + vel_error = LIMIT(vel_error, PIN(test_vel) / 100.0); PIN(cur_sum) += PIN(ki) * vel_error * period; PIN(q_cmd) = PIN(vel_bw) * period * vel_error + PIN(cur_sum); diff --git a/shared/comps/ids.c b/shared/comps/ids.c index fe5c8f71..b92f62e5 100644 --- a/shared/comps/ids.c +++ b/shared/comps/ids.c @@ -195,7 +195,7 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { if(PIN(timer) == 0.0){ PIN(min_cost) = MIN(PIN(cost), PIN(min_cost)); - PINA(max_params, 2) = PINA(params, 0) / 2.0; + PINA(max_params, 2) = PINA(params, 0) * 2.0; if(PINA(params, (int)PIN(param)) > PINA(max_params, (int)PIN(param))){ PINA(params, (int)PIN(param)) = PINA(max_params, (int)PIN(param));