Mirrors_Framework/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2026-09-26 10:13:59 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

enc_cmd fault

Browse Source
This commit is contained in:
crinq
2017-07-28 18:10:28 +02:00
parent 1223912140
commit faa58a1afd
5 changed files with 110 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/fanuc_a06b.txt
+5 -3
View File
@@ -9,12 +9,14 @@ enc_cmd0.remap = 2
io0.fb1g = 1
conf0.r = 0.6
conf0.l = 0.01
conf0.j = 0.001
conf0.psi = 0
conf0.j = 0.0003
conf0.polecount = 4
conf0.fb_res = 8000
conf0.mot_fb_res = 8000
conf0.cmd_res = 8000
conf0.max_ac_cur = 15
conf0.max_vel = 210
conf0.max_acc = 20000
conf0.max_force = 20
conf0.mot_fb_rev = 1
conf0.cur_i = 0.0001
conf0.com_fb_polecount = 4
conf/template/fanuc_io.txt
+3 -2
View File
@@ -1,9 +1,10 @@
load fanuc
fanuc0.rt_prio = 2
fanuc0.C1 = io0.CTX
fanuc0.C2 = io0.C12
fanuc0.C2 = io0.CTX
fanuc0.C1 = io0.C12
fanuc0.C4 = io0.C36
fanuc0.C8 = io0.C54
io0.cmd_remap = 1
rev2.in = fanuc0.pos
fb_switch0.com_abs_pos = rev2.out
fb_switch0.com_state = 3
inc/hw/hw.h
+5 -1
View File
@@ -258,9 +258,13 @@
#define FB1_B_PORT GPIOE
#define FB1_Z_PIN GPIO_Pin_13
#define FB1_Z_PIN_SOURCE GPIO_PinSource14
#define FB1_Z_PIN_SOURCE GPIO_PinSource13
#define FB1_Z_PORT GPIOE
#define FB1_Z_TXEN_PIN GPIO_Pin_14
#define FB1_Z_TXEN_PIN_SOURCE GPIO_PinSource14
#define FB1_Z_TXEN_PORT GPIOE
#define FB1_ENC_TIM TIM1
#define FB1_ENC_TIM_AF GPIO_AF_TIM1
#define FB1_ENC_TIM_RCC RCC_APB2Periph_TIM1
src/comps/enc_cmd.c
+36 -4
View File
@@ -12,13 +12,14 @@ HAL_PIN(res);
HAL_PIN(pos);
HAL_PIN(a);
HAL_PIN(b);
HAL_PIN(fault);
HAL_PIN(mode); // 0 = quad, 1 = step/dir, 2 = dir/step, 3 = up/down
HAL_PIN(remap); // 0 = cmd, 1 = fb0, 2 = fb1
struct enc_cmd_ctx_t{
int e_res;
uint32_t a_pin, b_pin, a_pin_source, b_pin_source, tim_af, tim_rcc;
GPIO_TypeDef * a_port, * b_port;
uint32_t a_pin, b_pin, c_pin, c_en_pin, a_pin_source, b_pin_source, tim_af, tim_rcc;
GPIO_TypeDef * a_port, * b_port, * c_port, * c_en_port;
TIM_TypeDef * tim;
};
@@ -53,35 +54,50 @@ static void hw_init(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){
case 0:
ctx->a_pin = CMD_A_PIN;
ctx->b_pin = CMD_B_PIN;
ctx->c_pin = CMD_C_PIN;
ctx->c_en_pin = CMD_C_EN_PIN;
ctx->a_port = CMD_A_PORT;
ctx->b_port = CMD_B_PORT;
ctx->c_port = CMD_C_PORT;
ctx->c_en_port = CMD_C_EN_PORT;
ctx->a_pin_source = CMD_A_PIN_SOURCE;
ctx->b_pin_source = CMD_B_PIN_SOURCE;
ctx->tim_af = CMD_ENC_TIM_AF;
ctx->tim_rcc = CMD_ENC_TIM_RCC;
ctx->tim = CMD_ENC_TIM;
RCC_APB1PeriphClockCmd(ctx->tim_rcc, ENABLE);
break;
case 1:
ctx->a_pin = FB0_A_PIN;
ctx->b_pin = FB0_B_PIN;
ctx->c_pin = FB0_Z_PIN;
ctx->c_en_pin = FB0_Z_TXEN_PIN;
ctx->a_port = FB0_A_PORT;
ctx->b_port = FB0_B_PORT;
ctx->c_port = FB0_Z_PORT;
ctx->c_en_port = FB0_Z_TXEN_PORT;
ctx->a_pin_source = FB0_A_PIN_SOURCE;
ctx->b_pin_source = FB0_B_PIN_SOURCE;
ctx->tim_af = FB0_ENC_TIM_AF;
ctx->tim_rcc = FB0_ENC_TIM_RCC;
ctx->tim = FB0_ENC_TIM;
RCC_APB1PeriphClockCmd(ctx->tim_rcc, ENABLE);
break;
case 2:
ctx->a_pin = FB1_A_PIN;
ctx->b_pin = FB1_B_PIN;
ctx->c_pin = FB1_Z_PIN;
ctx->c_en_pin = FB1_Z_TXEN_PIN;
ctx->a_port = FB1_A_PORT;
ctx->b_port = FB1_B_PORT;
ctx->c_port = FB1_Z_PORT;
ctx->c_en_port = FB1_Z_TXEN_PORT;
ctx->a_pin_source = FB1_A_PIN_SOURCE;
ctx->b_pin_source = FB1_B_PIN_SOURCE;
ctx->tim_af = FB1_ENC_TIM_AF;
ctx->tim_rcc = FB1_ENC_TIM_RCC;
ctx->tim = FB1_ENC_TIM;
RCC_APB2PeriphClockCmd(ctx->tim_rcc, ENABLE);
break;
default:
return;
@@ -92,12 +108,21 @@ static void hw_init(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){
GPIO_InitStruct.GPIO_Pin = ctx->b_pin;
GPIO_Init(ctx->b_port, &GPIO_InitStruct);
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStruct.GPIO_Pin = ctx->c_pin;
GPIO_Init(ctx->c_port, &GPIO_InitStruct);
GPIO_InitStruct.GPIO_Pin = ctx->c_en_pin;
GPIO_Init(ctx->c_en_port, &GPIO_InitStruct);
GPIO_SetBits(ctx->c_en_port, ctx->c_en_pin);
//Bind pins to Timer
GPIO_PinAFConfig(ctx->a_port, ctx->a_pin_source, ctx->tim_af);
GPIO_PinAFConfig(ctx->b_port, ctx->b_pin_source, ctx->tim_af);
RCC_APB1PeriphClockCmd(ctx->tim_rcc, ENABLE);
TIM_SetAutoreload(ctx->tim, ctx->e_res - 1);
// quad
TIM_Cmd(ctx->tim, DISABLE);
@@ -142,6 +167,13 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst
ctx->e_res = r;
TIM_SetAutoreload(ctx->tim, ctx->e_res - 1);
}
if(PIN(fault) > 0.0){
GPIO_SetBits(ctx->c_port, ctx->c_pin);
}
else{
GPIO_ResetBits(ctx->c_port, ctx->c_pin);
}
}
const hal_comp_t enc_cmd_comp_struct = {
src/comps/hw/io4.c
+61 -10
View File
@@ -36,6 +36,10 @@ HAL_PIN(in0);
HAL_PIN(in1);
HAL_PIN(ind0);
HAL_PIN(ind1);
HAL_PIN(ind0n);
HAL_PIN(ind1n);
HAL_PIN(th0);
HAL_PIN(th1);
HAL_PIN(CTX);
HAL_PIN(CRX);
@@ -60,10 +64,16 @@ HAL_PIN(fb1y);
HAL_PIN(cmdg);
HAL_PIN(cmdy);
HAL_PIN(io0);
HAL_PIN(io1);
HAL_PIN(fb0);
HAL_PIN(fb1);
HAL_PIN(fbd0);
HAL_PIN(fbd1);
HAL_PIN(fbd0n);
HAL_PIN(fbd1n);
HAL_PIN(fbth0);
HAL_PIN(fbth1);
HAL_PIN(fb0a);
HAL_PIN(fb0b);
@@ -75,6 +85,14 @@ HAL_PIN(fb1z);
HAL_PIN(fbsd);//fb shutdown
static void nrt_init(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){
struct io_pin_ctx_t * pins = (struct io_pin_ctx_t *)pin_ptr;
PIN(th0) = 12.0;
PIN(th1) = 12.0;
PIN(fbth0) = 2.5;
PIN(fbth1) = 2.5;
}
static void hw_init(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){
// struct io_ctx_t * ctx = (struct io_ctx_t *)ctx_ptr;
struct io_pin_ctx_t * pins = (struct io_pin_ctx_t *)pin_ptr;
@@ -226,29 +244,61 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst
//TODO: unit conversion
//TODO: check if adc sample complete?
PIN(in0) = V2(ADC2V(ADC_GetInjectedConversionValue(ADC3, ADC_InjectedChannel_1)), 3.3, 1000.0, 10000.0, 1000.0);
PIN(in1) = V2(ADC2V(ADC_GetInjectedConversionValue(ADC3, ADC_InjectedChannel_2)), 3.3, 1000.0, 10000.0, 1000.0);
float in0 = V2(ADC2V(ADC_GetInjectedConversionValue(ADC3, ADC_InjectedChannel_1)), 3.3, 1000.0, 10000.0, 1000.0);
float in1 = V2(ADC2V(ADC_GetInjectedConversionValue(ADC3, ADC_InjectedChannel_2)), 3.3, 1000.0, 10000.0, 1000.0);
PIN(in0) = in0;
PIN(in1) = in1;
if(PIN(in0) > 12.0){
if(in0 > PIN(th0) + 0.1){
PIN(ind0) = 1.0;
PIN(ind0n) = 0.0;
GPIO_SetBits(GPIOE, GPIO_Pin_1);
}else{
}
else if(in0 < PIN(th0) - 0.1){
PIN(ind0) = 0.0;
PIN(ind0n) = 1.0;
GPIO_ResetBits(GPIOE, GPIO_Pin_1);
}
if(PIN(in1) > 12.0){
if(in1 > PIN(th1) + 0.1){
PIN(ind1) = 1.0;
PIN(ind1n) = 0.0;
GPIO_SetBits(GPIOE, GPIO_Pin_0);
}else{
}
else if(in1 < PIN(th1) - 0.1){
PIN(ind1) = 0.0;
PIN(ind1n) = 1.0;
GPIO_ResetBits(GPIOE, GPIO_Pin_0);
}
PIN(fb0) = V3(ADC2V(ADC_GetInjectedConversionValue(ADC3, ADC_InjectedChannel_3)), 10000.0, 1000.0);
PIN(fb1) = V3(ADC2V(ADC_GetInjectedConversionValue(ADC3, ADC_InjectedChannel_4)), 10000.0, 1000.0);
in0 = V3(ADC2V(ADC_GetInjectedConversionValue(ADC3, ADC_InjectedChannel_3)), 10000.0, 1000.0);
in1 = V3(ADC2V(ADC_GetInjectedConversionValue(ADC3, ADC_InjectedChannel_4)), 10000.0, 1000.0);
PIN(fb0) = in0;
PIN(fb1) = in1;
ADC_SoftwareStartInjectedConv(ADC3);
if(in0 > PIN(fbth0) + 0.1){
PIN(fbd0) = 1.0;
PIN(fbd0n) = 0.0;
}
else if(in0 < PIN(fbth0) - 0.1){
PIN(fbd0) = 0.0;
PIN(fbd0n) = 1.0;
}
if(in1 > PIN(fbth1) + 0.1){
PIN(fbd1) = 1.0;
PIN(fbd1n) = 0.0;
}
else if(in1 < PIN(fbth1) - 0.1){
PIN(fbd1) = 0.0;
PIN(fbd1n) = 1.0;
}
// PIN(DIO) = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_13);
// PIN(CK) = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_14);
@@ -432,6 +482,7 @@ hal_comp_t io_comp_struct = {
.nrt = 0,
.rt = rt_func,
.frt = 0,
.nrt_init = nrt_init,
.hw_init = hw_init,
.rt_start = 0,
.frt_start = 0,