From 7dd28617550e45900d187ffcdd9959820c294bfa Mon Sep 17 00:00:00 2001 From: crinq Date: Fri, 28 Jul 2017 03:47:46 +0200 Subject: [PATCH] step dir poc --- inc/hw/hw.h | 34 +++++++++++++++++++-- src/comps/enc_cmd.c | 37 +++++++++-------------- src/comps/hw/io4.c | 63 ++++++++++++++++++++++++++++++++++++++ src/comps/o_fb.c | 74 +++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 183 insertions(+), 25 deletions(-) create mode 100644 src/comps/o_fb.c diff --git a/inc/hw/hw.h b/inc/hw/hw.h index 8cab1e72..7118f7ae 100644 --- a/inc/hw/hw.h +++ b/inc/hw/hw.h @@ -185,9 +185,17 @@ #define CMD_A_PIN_SOURCE GPIO_PinSource15 #define CMD_A_PORT GPIOA -#define CMD_B_PIN GPIO_Pin_0 -#define CMD_B_PIN_SOURCE GPIO_PinSource0 -#define CMD_B_PORT GPIOA +#define CMD_B_PIN GPIO_Pin_3 +#define CMD_B_PIN_SOURCE GPIO_PinSource3 +#define CMD_B_PORT GPIOB + +#define CMD_C_PIN GPIO_Pin_5 +#define CMD_C_PIN_SOURCE GPIO_PinSource5 +#define CMD_C_PORT GPIOB + +#define CMD_D_PIN GPIO_Pin_8 +#define CMD_D_PIN_SOURCE GPIO_PinSource8 +#define CMD_D_PORT GPIOB #define CMD_A_EN_PIN GPIO_Pin_6 #define CMD_A_EN_PIN_SOURCE GPIO_PinSource6 @@ -197,6 +205,14 @@ #define CMD_B_EN_PIN_SOURCE GPIO_PinSource7 #define CMD_B_EN_PORT GPIOB +#define CMD_C_EN_PIN GPIO_Pin_9 +#define CMD_C_EN_PIN_SOURCE GPIO_PinSource9 +#define CMD_C_EN_PORT GPIOB + +#define CMD_D_EN_PIN GPIO_Pin_2 +#define CMD_D_EN_PIN_SOURCE GPIO_PinSource2 +#define CMD_D_EN_PORT GPIOB + //FB0 TIM4 #define FB0_A_PIN GPIO_Pin_12 #define FB0_A_PIN_SOURCE GPIO_PinSource12 @@ -210,6 +226,18 @@ #define FB0_Z_PIN_SOURCE GPIO_PinSource14 #define FB0_Z_PORT GPIOD +#define FB0_A_EN_PIN GPIO_Pin_11 +#define FB0_A_EN_PIN_SOURCE GPIO_PinSource11 +#define FB0_A_EN_PORT GPIOD + +#define FB0_B_EN_PIN GPIO_Pin_10 +#define FB0_B_EN_PIN_SOURCE GPIO_PinSource10 +#define FB0_B_EN_PORT GPIOD + +#define FB0_Z_EN_PIN GPIO_Pin_15 +#define FB0_Z_EN_PIN_SOURCE GPIO_PinSource15 +#define FB0_Z_EN_PORT GPIOD + #define FB0_Z_TXEN_PIN GPIO_Pin_15 #define FB0_Z_TXEN_PORT GPIOD diff --git a/src/comps/enc_cmd.c b/src/comps/enc_cmd.c index f654c6f6..e4674fad 100644 --- a/src/comps/enc_cmd.c +++ b/src/comps/enc_cmd.c @@ -38,9 +38,10 @@ static void hw_init(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){ GPIO_InitTypeDef GPIO_InitStruct; GPIO_StructInit(&GPIO_InitStruct); GPIO_InitStruct.GPIO_Pin = CMD_A_PIN; - GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; - GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; GPIO_Init(CMD_A_PORT, &GPIO_InitStruct); GPIO_InitStruct.GPIO_Pin = CMD_B_PIN; @@ -50,34 +51,26 @@ static void hw_init(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){ GPIO_PinAFConfig(CMD_A_PORT, CMD_A_PIN_SOURCE, CMD_ENC_TIM_AF); GPIO_PinAFConfig(CMD_B_PORT, CMD_B_PIN_SOURCE, CMD_ENC_TIM_AF); - //Enable Timer clock - RCC_APB2PeriphClockCmd(CMD_ENC_TIM_RCC, ENABLE); - TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct; - TIM_TimeBaseStruct.TIM_ClockDivision = TIM_CKD_DIV1; - TIM_TimeBaseStruct.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseStruct.TIM_Prescaler = 1; - TIM_TimeBaseStruct.TIM_RepetitionCounter = 0; - TIM_TimeBaseInit(CMD_ENC_TIM, &TIM_TimeBaseStruct); - - TIM_EncoderInterfaceConfig (CMD_ENC_TIM, TIM_EncoderMode_TI1, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); - + RCC_APB1PeriphClockCmd(CMD_ENC_TIM_RCC, ENABLE); + + TIM_SetAutoreload(CMD_ENC_TIM, ctx->e_res - 1); + // quad + TIM_Cmd(CMD_ENC_TIM, DISABLE); + TIM_EncoderInterfaceConfig(CMD_ENC_TIM, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); TIM_ICInitTypeDef TIM_ICInitStruct; TIM_ICInitStruct.TIM_Channel = TIM_Channel_1; TIM_ICInitStruct.TIM_ICFilter = 0x0f; //Digital filtering @ 1/32 fDTS - TIM_ICInitStruct.TIM_ICPolarity = TIM_ICPolarity_Falling; //Just trigger at the rising edge, because its the clock + TIM_ICInitStruct.TIM_ICPolarity = TIM_ICPolarity_Rising; //Just trigger at the rising edge, because its the clock TIM_ICInitStruct.TIM_ICPrescaler = 1; //no prescaler, capture is done each time an edge is detected on the capture input TIM_ICInitStruct.TIM_ICSelection = TIM_ICSelection_DirectTI; //IC1 mapped to TI1 TIM_ICInit(CMD_ENC_TIM, &TIM_ICInitStruct); - + TIM_ICInitStruct.TIM_Channel = TIM_Channel_2; TIM_ICInitStruct.TIM_ICFilter = 0x0f; //Digital filtering @ 1/32 fDTS TIM_ICInitStruct.TIM_ICPolarity = TIM_ICPolarity_BothEdge; //Trigger at every edge, because its the direction TIM_ICInitStruct.TIM_ICPrescaler = 1; //no prescaler, capture is done each time an edge is detected on the capture input TIM_ICInitStruct.TIM_ICSelection = TIM_ICSelection_IndirectTI; //IC2 mapped to TI1 TIM_ICInit(CMD_ENC_TIM, &TIM_ICInitStruct); - - TIM_SetAutoreload(CMD_ENC_TIM, ctx->e_res - 1); - TIM_Cmd(CMD_ENC_TIM, ENABLE); } @@ -85,10 +78,10 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst struct enc_cmd_ctx_t * ctx = (struct enc_cmd_ctx_t *)ctx_ptr; struct enc_cmd_pin_ctx_t * pins = (struct enc_cmd_pin_ctx_t *)pin_ptr; - int32_t tim = TIM_GetCounter(CMD_ENC_TIM);//TODO: interrupt here? + int32_t tim = TIM_GetCounter(CMD_ENC_TIM); - PIN(a) = CMD_A_PORT->IDR & CMD_A_PIN > 0; - PIN(b) = CMD_B_PORT->IDR & CMD_B_PIN > 0; + PIN(a) = (CMD_A_PORT->IDR & CMD_A_PIN) > 0; + PIN(b) = (CMD_B_PORT->IDR & CMD_B_PIN) > 0; float p = 0.0; p = mod(tim * 2.0f * M_PI / (float)ctx->e_res); @@ -100,7 +93,7 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst if(ctx->e_res != r){ ctx->e_res = r; - TIM_SetAutoreload(FB0_ENC_TIM, ctx->e_res - 1); + TIM_SetAutoreload(CMD_ENC_TIM, ctx->e_res - 1); } } diff --git a/src/comps/hw/io4.c b/src/comps/hw/io4.c index d5d592ae..74bf8b95 100644 --- a/src/comps/hw/io4.c +++ b/src/comps/hw/io4.c @@ -48,6 +48,10 @@ HAL_PIN(fb0y); HAL_PIN(fb1g); HAL_PIN(fb1y); +HAL_PIN(cmdc); +HAL_PIN(cmdc_en); +HAL_PIN(cmdd); +HAL_PIN(cmdd_en); HAL_PIN(cmdg); HAL_PIN(cmdy); @@ -166,6 +170,13 @@ static void nrt_init(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr) GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8; GPIO_Init(GPIOE, &GPIO_InitStructure); + //cmd + // GPIO_InitStructure.GPIO_Pin = CMD_C_EN_PIN; + // GPIO_Init(CMD_C_EN_PORT, &GPIO_InitStructure); + // + // GPIO_InitStructure.GPIO_Pin = CMD_D_EN_PIN; + // GPIO_Init(CMD_D_EN_PORT, &GPIO_InitStructure); + //cmd yellow GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6; GPIO_Init(GPIOD, &GPIO_InitStructure); @@ -222,6 +233,7 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst static void nrt_func(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; + GPIO_InitTypeDef GPIO_InitStructure; uint32_t red = 0; uint32_t green = 0; @@ -315,6 +327,57 @@ static void nrt_func(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr) else GPIO_ResetBits(GPIOE, GPIO_Pin_8); + + // if(PIN(cmdc_en) > 0){ + // GPIO_SetBits(CMD_C_EN_PORT, CMD_C_EN_PIN); + // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + // GPIO_InitStructure.GPIO_Pin = CMD_C_PIN; + // GPIO_Init(CMD_C_PORT, &GPIO_InitStructure); + // if(PIN(cmdc) > 0){ + // GPIO_SetBits(CMD_C_PORT, CMD_C_PIN); + // } + // else{ + // GPIO_ResetBits(CMD_C_PORT, CMD_C_PIN); + // } + // } + // else{ + // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN; + // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + // GPIO_InitStructure.GPIO_Pin = CMD_C_PIN; + // GPIO_Init(CMD_C_PORT, &GPIO_InitStructure); + // GPIO_ResetBits(CMD_C_EN_PORT, CMD_C_EN_PIN); + // PIN(cmdc) = (CMD_C_PORT->IDR & CMD_C_PIN) > 0; + // } + // + // if(PIN(cmdd_en) > 0){ + // GPIO_SetBits(CMD_D_EN_PORT, CMD_D_EN_PIN); + // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + // GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + // GPIO_InitStructure.GPIO_Pin = CMD_C_PIN; + // GPIO_Init(CMD_D_PORT, &GPIO_InitStructure); + // if(PIN(cmdd) > 0){ + // GPIO_SetBits(CMD_D_PORT, CMD_D_PIN); + // } + // else{ + // GPIO_ResetBits(CMD_D_PORT, CMD_D_PIN); + // } + // } + // else{ + // GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN; + // GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + // GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + // GPIO_InitStructure.GPIO_Pin = CMD_D_PIN; + // GPIO_Init(CMD_D_PORT, &GPIO_InitStructure); + // GPIO_ResetBits(CMD_D_EN_PORT, CMD_D_EN_PIN); + // PIN(cmdd) = (CMD_D_PORT->IDR & CMD_D_PIN) > 0; + // } + if(PIN(cmdy) > 0) GPIO_SetBits(GPIOD, GPIO_Pin_6); else diff --git a/src/comps/o_fb.c b/src/comps/o_fb.c new file mode 100644 index 00000000..c9287a9b --- /dev/null +++ b/src/comps/o_fb.c @@ -0,0 +1,74 @@ +#include "commands.h" +#include "hal.h" +#include "math.h" +#include "defines.h" +#include "angle.h" +#include "stm32f4xx_conf.h" +#include "hw/hw.h" + +HAL_COMP(o_fb); + +HAL_PIN(a); +HAL_PIN(b); + +static void hw_init(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){ + // struct o_fb_ctx_t * ctx = (struct o_fb_ctx_t *)ctx_ptr; + struct o_fb_pin_ctx_t * pins = (struct o_fb_pin_ctx_t *)pin_ptr; + + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + + GPIO_InitStructure.GPIO_Pin = FB0_A_EN_PIN; + GPIO_Init(FB0_A_EN_PORT, &GPIO_InitStructure); + + GPIO_InitStructure.GPIO_Pin = FB0_B_EN_PIN; + GPIO_Init(FB0_B_EN_PORT, &GPIO_InitStructure); + + GPIO_SetBits(FB0_A_EN_PORT, FB0_A_EN_PIN); + GPIO_SetBits(FB0_B_EN_PORT, FB0_B_EN_PIN); + + GPIO_InitStructure.GPIO_Pin = FB0_A_PIN; + GPIO_Init(FB0_A_PORT, &GPIO_InitStructure); + + GPIO_InitStructure.GPIO_Pin = FB0_B_PIN; + GPIO_Init(FB0_B_PORT, &GPIO_InitStructure); +} + + + +static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){ + // struct o_fb_ctx_t * ctx = (struct o_fb_ctx_t *)ctx_ptr; + struct o_fb_pin_ctx_t * pins = (struct o_fb_pin_ctx_t *)pin_ptr; + + if(PIN(a) > 0){ + GPIO_SetBits(FB0_A_PORT, FB0_A_PIN); + } + else{ + GPIO_ResetBits(FB0_A_PORT, FB0_A_PIN); + } + + if(PIN(b) > 0){ + GPIO_SetBits(FB0_B_PORT, FB0_B_PIN); + } + else{ + GPIO_ResetBits(FB0_B_PORT, FB0_B_PIN); + } +} + +const hal_comp_t o_fb_comp_struct = { + .name = "o_fb", + .nrt = 0, + .rt = rt_func, + .frt = 0, + .nrt_init = 0, + .hw_init = hw_init, + .rt_start = 0, + .frt_start = 0, + .rt_stop = 0, + .frt_stop = 0, + .ctx_size = 0, + .pin_count = sizeof(struct o_fb_pin_ctx_t) / sizeof(struct hal_pin_inst_t), +};