aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
Diff: odometry/encoder.h
- Revision:
- 3:a45557a0dcb8
- Parent:
- 1:bdd17feaa4ce
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/odometry/encoder.h Tue Dec 11 17:51:47 2018 +0000 @@ -0,0 +1,135 @@ +#ifndef _ENCODER_H_ +#define _ENCODER_H_ + +#include "mbed.h" + +#define ENC_ZERO_POINT (0x7FFF) + +class TIM2Encoder{ +public: + TIM2Encoder(){ + GPIO_InitTypeDef GPIO_InitStruct; + + __HAL_RCC_TIM2_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + + GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF1_TIM2; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + TIM_Encoder_InitTypeDef sConfig; + TIM_MasterConfigTypeDef sMasterConfig; + + htim2.Instance = TIM2; + htim2.Init.Prescaler = 0; + htim2.Init.CounterMode = TIM_COUNTERMODE_UP; + htim2.Init.Period = 65535; + htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + sConfig.EncoderMode = TIM_ENCODERMODE_TI12; + sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; + sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; + sConfig.IC1Prescaler = TIM_ICPSC_DIV1; + sConfig.IC1Filter = 0; + sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; + sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; + sConfig.IC2Prescaler = TIM_ICPSC_DIV1; + sConfig.IC2Filter = 0; + + HAL_TIM_Encoder_Init(&htim2, &sConfig); + + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig); + + HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL); + __HAL_TIM_SET_COUNTER(&htim2, ENC_ZERO_POINT); + + encoder = 0; + } + int32_t get_encoder_pulse(){ + int32_t diff; + diff = (__HAL_TIM_GET_COUNTER(&htim2) - ENC_ZERO_POINT); + __HAL_TIM_SET_COUNTER(&htim2, ENC_ZERO_POINT); + encoder -= diff; + + return -diff; + } + int32_t get_encoder(){ + return encoder; + } +private: + TIM_HandleTypeDef htim2; + uint32_t encoder; +}; + +class TIM3Encoder{ +public: + TIM3Encoder(){ + GPIO_InitTypeDef GPIO_InitStruct; + + __HAL_RCC_TIM3_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + + GPIO_InitStruct.Pin = GPIO_PIN_6; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF2_TIM3; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = GPIO_PIN_5; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF2_TIM3; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + TIM_Encoder_InitTypeDef sConfig; + TIM_MasterConfigTypeDef sMasterConfig; + + htim3.Instance = TIM3; + htim3.Init.Prescaler = 0; + htim3.Init.CounterMode = TIM_COUNTERMODE_UP; + htim3.Init.Period = 65535; + htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + sConfig.EncoderMode = TIM_ENCODERMODE_TI12; + sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; + sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; + sConfig.IC1Prescaler = TIM_ICPSC_DIV1; + sConfig.IC1Filter = 0; + sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; + sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; + sConfig.IC2Prescaler = TIM_ICPSC_DIV1; + sConfig.IC2Filter = 0; + HAL_TIM_Encoder_Init(&htim3, &sConfig); + + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig); + + HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); + __HAL_TIM_SET_COUNTER(&htim3, ENC_ZERO_POINT); + + encoder = 0; + } + int32_t get_encoder_pulse(){ + int32_t diff; + diff = (__HAL_TIM_GET_COUNTER(&htim3) - ENC_ZERO_POINT); + __HAL_TIM_SET_COUNTER(&htim3, ENC_ZERO_POINT); + encoder -= diff; + + return -diff; + } + int32_t get_encoder(){ + return encoder; + } +private: + TIM_HandleTypeDef htim3; + uint32_t encoder; +}; + +#endif \ No newline at end of file