diff --git a/conf/fanuc_a06b.txt b/conf/fanuc_a06b.txt index 8450c974..72511eb6 100644 --- a/conf/fanuc_a06b.txt +++ b/conf/fanuc_a06b.txt @@ -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 diff --git a/conf/template/fanuc_io.txt b/conf/template/fanuc_io.txt index 805a0ddf..c40f8b54 100644 --- a/conf/template/fanuc_io.txt +++ b/conf/template/fanuc_io.txt @@ -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 diff --git a/inc/hw/hw.h b/inc/hw/hw.h index e4a6d721..58ccd198 100644 --- a/inc/hw/hw.h +++ b/inc/hw/hw.h @@ -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 diff --git a/src/comps/enc_cmd.c b/src/comps/enc_cmd.c index f4e93198..c49d3e8b 100644 --- a/src/comps/enc_cmd.c +++ b/src/comps/enc_cmd.c @@ -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 = { diff --git a/src/comps/hw/io4.c b/src/comps/hw/io4.c index a88677da..6c2dc552 100644 --- a/src/comps/hw/io4.c +++ b/src/comps/hw/io4.c @@ -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,