Mirrors_Framework/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2026-08-18 01:18:58 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

enc error fix, adc amp

Browse Source
This commit is contained in:
crinq
2018-04-11 02:24:36 +02:00
parent d9579c7662
commit fdcbec6470
6 changed files with 40 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/festo.txt
+3 -1
View File
@@ -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
res0.phase = 0.4
conf0.mot_fb_offset = -0.255
conf/sm060.txt
+8 -3
View File
@@ -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
conf/template/enc_fb0.txt
+2
View File
@@ -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
shared/comps/fb_switch.c
+7 -7
View File
@@ -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 = {
src/comps/adc.c
+18
View File
@@ -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) {
src/comps/enc_fb.c
+2 -5
View File
@@ -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);