From fdcbec647002db463e2399b1569451998fbe96e5 Mon Sep 17 00:00:00 2001 From: crinq Date: Wed, 11 Apr 2018 02:24:36 +0200 Subject: [PATCH] enc error fix, adc amp --- conf/festo.txt | 4 +++- conf/sm060.txt | 11 ++++++++--- conf/template/enc_fb0.txt | 2 ++ shared/comps/fb_switch.c | 14 +++++++------- src/comps/adc.c | 18 ++++++++++++++++++ src/comps/enc_fb.c | 7 ++----- 6 files changed, 40 insertions(+), 16 deletions(-) diff --git a/conf/festo.txt b/conf/festo.txt index cea7605e..76e91334 100644 --- a/conf/festo.txt +++ b/conf/festo.txt @@ -1,6 +1,7 @@ link pid link pmsm link res_fb0 +link jog_cmd link misc conf0.r = 9.6 conf0.l = 0.01 @@ -11,4 +12,5 @@ io0.out0 = fault0.mot_brake conf0.max_ac_cur = 10 conf0.max_force = 3 res0.freq = 5000 -res0.phase = 0.4 \ No newline at end of file +res0.phase = 0.4 +conf0.mot_fb_offset = -0.255 \ No newline at end of file diff --git a/conf/sm060.txt b/conf/sm060.txt index 059338d8..1b1d09ec 100644 --- a/conf/sm060.txt +++ b/conf/sm060.txt @@ -2,12 +2,17 @@ link pid link pmsm link enc_fb0 link uvw_fb1 +link jog_cmd link misc +#link sserial +#linrev0.scale = 6 conf0.r = 1.6 -conf0.l = 0.005 -conf0.j = 0.000056 +conf0.l = 0.008 +conf0.j = 0.00007 +conf0.cur_i = 0.0004 +conf0.mot_fb_offset = 0.1 conf0.max_force = 5.7 -conf0.max_ac_cur = 5 +conf0.max_ac_cur = 9 conf0.mot_fb_res = 10000 conf0.cmd_res = 10000 conf0.mot_fb_rev = 1 diff --git a/conf/template/enc_fb0.txt b/conf/template/enc_fb0.txt index 4ef1a74a..34b12efa 100644 --- a/conf/template/enc_fb0.txt +++ b/conf/template/enc_fb0.txt @@ -11,3 +11,5 @@ fb_switch0.mot_state = enc_fb0.state fault0.mot_fb_error = enc_fb0.error io0.fb0y = enc_fb0.error io0.fb0g = 1 +enc_fb0.vel = vel1.vel +enc_fb0.amp = adc0.amp0 diff --git a/shared/comps/fb_switch.c b/shared/comps/fb_switch.c index a31baf8f..cc443a39 100644 --- a/shared/comps/fb_switch.c +++ b/shared/comps/fb_switch.c @@ -120,18 +120,18 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ } else { PIN(state) = 1.0; if(PIN(joint_state) >= 2.0 && ctx->current_com_pos > 3.0) { - ctx->current_com_pos = 3; + ctx->current_com_pos = 3; // joint fb absolute 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->current_com_pos = 2; // com fb absolute 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; + ctx->current_com_pos = 1; // mot fb absolute ctx->com_offset = 0.0; } - if(ctx->current_com_pos > 4.0) { + if(ctx->current_com_pos > 4.0) { // autophasing PIN(com_fb) = 0.0; ctx->phase_timer += period; @@ -146,7 +146,6 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ } } - PIN(current_com_pos) = ctx->current_com_pos; switch(ctx->current_com_pos) { case 4: @@ -155,7 +154,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ case 3: if(PIN(joint_state) != 3.0) { - PIN(com_fb) = mod(mot_pos * PIN(polecount) / PIN(mot_polecount) + ctx->com_offset); + PIN(com_fb) = mod(mot_pos * PIN(polecount) / PIN(mot_polecount) + ctx->com_offset); // tracking } else { PIN(com_fb) = mod((joint_abs_pos + joint_offset) * PIN(polecount)); } @@ -163,7 +162,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ case 2: if(PIN(com_state) != 3.0) { - PIN(com_fb) = mod(mot_pos * PIN(polecount) / PIN(mot_polecount) + ctx->com_offset); + PIN(com_fb) = mod(mot_pos * PIN(polecount) / PIN(mot_polecount) + ctx->com_offset); // tracking } else { PIN(com_fb) = mod((com_abs_pos + com_offset) * PIN(polecount) / PIN(com_polecount)); } @@ -181,6 +180,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ PIN(state) = 0.0; } } + PIN(current_com_pos) = ctx->current_com_pos; } hal_comp_t fb_switch_comp_struct = { diff --git a/src/comps/adc.c b/src/comps/adc.c index 3a0cf9f2..f98aab45 100644 --- a/src/comps/adc.c +++ b/src/comps/adc.c @@ -19,9 +19,13 @@ HAL_PIN(cos0); //cos output HAL_PIN(sin0l); //sin output, last group only HAL_PIN(cos0l); //cos output, last group only HAL_PIN(quad); //quadrant of sin/cos +HAL_PIN(amp0); +HAL_PIN(sin1); //sin output +HAL_PIN(cos1); //cos output HAL_PIN(sin1l); //sin output, last group only HAL_PIN(cos1l); //cos output, last group only +HAL_PIN(amp1); HAL_PIN(res_mode); //polarity flip mode for resolvers @@ -77,6 +81,9 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ float si1[ADC_GROUPS]; uint32_t sii1; uint32_t coi1; + //scaled, all groups + float sin1all = 0.0; + float cos1all = 0.0; #endif float s_o = PIN(sin_offset); @@ -120,6 +127,8 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ } si1[i] = s_g * V_DIFF(sii1, ADC_OVER_FB1) + s_o; co1[i] = c_g * V_DIFF(coi1, ADC_OVER_FB1) + c_o; + sin1all += si1[i]; + cos1all += co1[i]; #endif } if(ctx->send == 0) { @@ -127,13 +136,22 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ ctx->send = 1; } + float s = V_DIFF(ADC_DMA_Buffer[0] & 0x0000ffff, 1); + float c = V_DIFF(ADC_DMA_Buffer[0] >> 16, 1); + PIN(sin0l) = si0[ADC_GROUPS - 1]; PIN(cos0l) = co0[ADC_GROUPS - 1]; PIN(sin0) = sin0all / (float)ADC_GROUPS; PIN(cos0) = cos0all / (float)ADC_GROUPS; + PIN(amp0) = PIN(amp0) * 0.99 + sqrtf(s * s + c * c) * 0.01; #ifdef FB1 + s = V_DIFF(ADC_DMA_Buffer[ADC_OVER_FB0] & 0x0000ffff, 1); + c = V_DIFF(ADC_DMA_Buffer[ADC_OVER_FB0] >> 16, 1); PIN(sin1l) = si1[ADC_GROUPS - 1]; PIN(cos1l) = co1[ADC_GROUPS - 1]; + PIN(sin1) = sin1all / (float)ADC_GROUPS; + PIN(cos1) = cos1all / (float)ADC_GROUPS; + PIN(amp1) = PIN(amp1) * 0.99 + sqrtf(s * s + c * c) * 0.01; #endif // if(PIN(res_en) > 0.0) { diff --git a/src/comps/enc_fb.c b/src/comps/enc_fb.c index f8364513..a5789ec8 100644 --- a/src/comps/enc_fb.c +++ b/src/comps/enc_fb.c @@ -25,6 +25,7 @@ HAL_PIN(oquadoff); HAL_PIN(qdiff); HAL_PIN(error); HAL_PIN(amp); +HAL_PIN(vel); HAL_PIN(ccr3); HAL_PIN(indexprint); @@ -135,9 +136,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ float s = PIN(sin); float c = PIN(cos); - float a = sqrtf(s * s + c * c); - PIN(amp) = a; int q; //calculate quadrant of timer @@ -199,9 +198,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ PIN(abs_pos) = minus(p, ctx->absoffset); PIN(index) = GPIO_ReadInputDataBit(FB0_Z_PORT, FB0_Z_PIN); - - //TODO: fix EDGE - if(a > 0.15 || EDGE(tim)) { + if(PIN(amp) > 0.25 || ABS(PIN(vel)) > 0.15) { PIN(error) = 0.0; PIN(state) = MAX(PIN(state), 1.0); PIN(ipos) = mod(p + ((int)(ir * mod(atan2f(s, c) * 4.0 + M_PI) / M_PI)) / ir * M_PI / (float)ctx->e_res);