aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
odometry/encoder.h@12:f726eb78b54c, 2019-08-30 (annotated)
- Committer:
- nakedt555
- Date:
- Fri Aug 30 07:55:38 2019 +0000
- Revision:
- 12:f726eb78b54c
- Parent:
- 3:a45557a0dcb8
aaa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nakedt555 | 1:bdd17feaa4ce | 1 | #ifndef _ENCODER_H_ |
nakedt555 | 1:bdd17feaa4ce | 2 | #define _ENCODER_H_ |
nakedt555 | 1:bdd17feaa4ce | 3 | |
nakedt555 | 1:bdd17feaa4ce | 4 | #include "mbed.h" |
nakedt555 | 1:bdd17feaa4ce | 5 | |
nakedt555 | 1:bdd17feaa4ce | 6 | #define ENC_ZERO_POINT (0x7FFF) |
nakedt555 | 1:bdd17feaa4ce | 7 | |
nakedt555 | 1:bdd17feaa4ce | 8 | class TIM2Encoder{ |
nakedt555 | 1:bdd17feaa4ce | 9 | public: |
nakedt555 | 1:bdd17feaa4ce | 10 | TIM2Encoder(){ |
nakedt555 | 1:bdd17feaa4ce | 11 | GPIO_InitTypeDef GPIO_InitStruct; |
nakedt555 | 1:bdd17feaa4ce | 12 | |
nakedt555 | 1:bdd17feaa4ce | 13 | __HAL_RCC_TIM2_CLK_ENABLE(); |
nakedt555 | 1:bdd17feaa4ce | 14 | __HAL_RCC_GPIOA_CLK_ENABLE(); |
nakedt555 | 1:bdd17feaa4ce | 15 | |
nakedt555 | 1:bdd17feaa4ce | 16 | GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1; |
nakedt555 | 1:bdd17feaa4ce | 17 | GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; |
nakedt555 | 1:bdd17feaa4ce | 18 | GPIO_InitStruct.Pull = GPIO_NOPULL; |
nakedt555 | 1:bdd17feaa4ce | 19 | GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; |
nakedt555 | 1:bdd17feaa4ce | 20 | GPIO_InitStruct.Alternate = GPIO_AF1_TIM2; |
nakedt555 | 1:bdd17feaa4ce | 21 | HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); |
nakedt555 | 1:bdd17feaa4ce | 22 | |
nakedt555 | 1:bdd17feaa4ce | 23 | TIM_Encoder_InitTypeDef sConfig; |
nakedt555 | 1:bdd17feaa4ce | 24 | TIM_MasterConfigTypeDef sMasterConfig; |
nakedt555 | 1:bdd17feaa4ce | 25 | |
nakedt555 | 1:bdd17feaa4ce | 26 | htim2.Instance = TIM2; |
nakedt555 | 1:bdd17feaa4ce | 27 | htim2.Init.Prescaler = 0; |
nakedt555 | 1:bdd17feaa4ce | 28 | htim2.Init.CounterMode = TIM_COUNTERMODE_UP; |
nakedt555 | 1:bdd17feaa4ce | 29 | htim2.Init.Period = 65535; |
nakedt555 | 1:bdd17feaa4ce | 30 | htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; |
nakedt555 | 1:bdd17feaa4ce | 31 | sConfig.EncoderMode = TIM_ENCODERMODE_TI12; |
nakedt555 | 1:bdd17feaa4ce | 32 | sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; |
nakedt555 | 1:bdd17feaa4ce | 33 | sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; |
nakedt555 | 1:bdd17feaa4ce | 34 | sConfig.IC1Prescaler = TIM_ICPSC_DIV1; |
nakedt555 | 1:bdd17feaa4ce | 35 | sConfig.IC1Filter = 0; |
nakedt555 | 1:bdd17feaa4ce | 36 | sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; |
nakedt555 | 1:bdd17feaa4ce | 37 | sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; |
nakedt555 | 1:bdd17feaa4ce | 38 | sConfig.IC2Prescaler = TIM_ICPSC_DIV1; |
nakedt555 | 1:bdd17feaa4ce | 39 | sConfig.IC2Filter = 0; |
nakedt555 | 1:bdd17feaa4ce | 40 | |
nakedt555 | 1:bdd17feaa4ce | 41 | HAL_TIM_Encoder_Init(&htim2, &sConfig); |
nakedt555 | 1:bdd17feaa4ce | 42 | |
nakedt555 | 1:bdd17feaa4ce | 43 | sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; |
nakedt555 | 1:bdd17feaa4ce | 44 | sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; |
nakedt555 | 1:bdd17feaa4ce | 45 | HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig); |
nakedt555 | 1:bdd17feaa4ce | 46 | |
nakedt555 | 1:bdd17feaa4ce | 47 | HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL); |
nakedt555 | 1:bdd17feaa4ce | 48 | __HAL_TIM_SET_COUNTER(&htim2, ENC_ZERO_POINT); |
nakedt555 | 1:bdd17feaa4ce | 49 | |
nakedt555 | 1:bdd17feaa4ce | 50 | encoder = 0; |
nakedt555 | 1:bdd17feaa4ce | 51 | } |
nakedt555 | 1:bdd17feaa4ce | 52 | int32_t get_encoder_pulse(){ |
nakedt555 | 1:bdd17feaa4ce | 53 | int32_t diff; |
nakedt555 | 1:bdd17feaa4ce | 54 | diff = (__HAL_TIM_GET_COUNTER(&htim2) - ENC_ZERO_POINT); |
nakedt555 | 1:bdd17feaa4ce | 55 | __HAL_TIM_SET_COUNTER(&htim2, ENC_ZERO_POINT); |
nakedt555 | 1:bdd17feaa4ce | 56 | encoder -= diff; |
nakedt555 | 1:bdd17feaa4ce | 57 | |
nakedt555 | 1:bdd17feaa4ce | 58 | return -diff; |
nakedt555 | 1:bdd17feaa4ce | 59 | } |
nakedt555 | 1:bdd17feaa4ce | 60 | int32_t get_encoder(){ |
nakedt555 | 1:bdd17feaa4ce | 61 | return encoder; |
nakedt555 | 1:bdd17feaa4ce | 62 | } |
nakedt555 | 1:bdd17feaa4ce | 63 | private: |
nakedt555 | 1:bdd17feaa4ce | 64 | TIM_HandleTypeDef htim2; |
nakedt555 | 1:bdd17feaa4ce | 65 | uint32_t encoder; |
nakedt555 | 1:bdd17feaa4ce | 66 | }; |
nakedt555 | 1:bdd17feaa4ce | 67 | |
nakedt555 | 1:bdd17feaa4ce | 68 | class TIM3Encoder{ |
nakedt555 | 1:bdd17feaa4ce | 69 | public: |
nakedt555 | 1:bdd17feaa4ce | 70 | TIM3Encoder(){ |
nakedt555 | 1:bdd17feaa4ce | 71 | GPIO_InitTypeDef GPIO_InitStruct; |
nakedt555 | 1:bdd17feaa4ce | 72 | |
nakedt555 | 1:bdd17feaa4ce | 73 | __HAL_RCC_TIM3_CLK_ENABLE(); |
nakedt555 | 1:bdd17feaa4ce | 74 | __HAL_RCC_GPIOA_CLK_ENABLE(); |
nakedt555 | 1:bdd17feaa4ce | 75 | __HAL_RCC_GPIOB_CLK_ENABLE(); |
nakedt555 | 1:bdd17feaa4ce | 76 | |
nakedt555 | 1:bdd17feaa4ce | 77 | GPIO_InitStruct.Pin = GPIO_PIN_6; |
nakedt555 | 1:bdd17feaa4ce | 78 | GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; |
nakedt555 | 1:bdd17feaa4ce | 79 | GPIO_InitStruct.Pull = GPIO_NOPULL; |
nakedt555 | 1:bdd17feaa4ce | 80 | GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; |
nakedt555 | 1:bdd17feaa4ce | 81 | GPIO_InitStruct.Alternate = GPIO_AF2_TIM3; |
nakedt555 | 1:bdd17feaa4ce | 82 | HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); |
nakedt555 | 1:bdd17feaa4ce | 83 | |
nakedt555 | 1:bdd17feaa4ce | 84 | GPIO_InitStruct.Pin = GPIO_PIN_5; |
nakedt555 | 1:bdd17feaa4ce | 85 | GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; |
nakedt555 | 1:bdd17feaa4ce | 86 | GPIO_InitStruct.Pull = GPIO_NOPULL; |
nakedt555 | 1:bdd17feaa4ce | 87 | GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; |
nakedt555 | 1:bdd17feaa4ce | 88 | GPIO_InitStruct.Alternate = GPIO_AF2_TIM3; |
nakedt555 | 1:bdd17feaa4ce | 89 | HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); |
nakedt555 | 1:bdd17feaa4ce | 90 | |
nakedt555 | 1:bdd17feaa4ce | 91 | TIM_Encoder_InitTypeDef sConfig; |
nakedt555 | 1:bdd17feaa4ce | 92 | TIM_MasterConfigTypeDef sMasterConfig; |
nakedt555 | 1:bdd17feaa4ce | 93 | |
nakedt555 | 1:bdd17feaa4ce | 94 | htim3.Instance = TIM3; |
nakedt555 | 1:bdd17feaa4ce | 95 | htim3.Init.Prescaler = 0; |
nakedt555 | 1:bdd17feaa4ce | 96 | htim3.Init.CounterMode = TIM_COUNTERMODE_UP; |
nakedt555 | 1:bdd17feaa4ce | 97 | htim3.Init.Period = 65535; |
nakedt555 | 1:bdd17feaa4ce | 98 | htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; |
nakedt555 | 1:bdd17feaa4ce | 99 | sConfig.EncoderMode = TIM_ENCODERMODE_TI12; |
nakedt555 | 1:bdd17feaa4ce | 100 | sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; |
nakedt555 | 1:bdd17feaa4ce | 101 | sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; |
nakedt555 | 1:bdd17feaa4ce | 102 | sConfig.IC1Prescaler = TIM_ICPSC_DIV1; |
nakedt555 | 1:bdd17feaa4ce | 103 | sConfig.IC1Filter = 0; |
nakedt555 | 1:bdd17feaa4ce | 104 | sConfig.IC2Polarity = TIM_ICPOLARITY_RISING; |
nakedt555 | 1:bdd17feaa4ce | 105 | sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI; |
nakedt555 | 1:bdd17feaa4ce | 106 | sConfig.IC2Prescaler = TIM_ICPSC_DIV1; |
nakedt555 | 1:bdd17feaa4ce | 107 | sConfig.IC2Filter = 0; |
nakedt555 | 1:bdd17feaa4ce | 108 | HAL_TIM_Encoder_Init(&htim3, &sConfig); |
nakedt555 | 1:bdd17feaa4ce | 109 | |
nakedt555 | 1:bdd17feaa4ce | 110 | sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; |
nakedt555 | 1:bdd17feaa4ce | 111 | sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; |
nakedt555 | 1:bdd17feaa4ce | 112 | HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig); |
nakedt555 | 1:bdd17feaa4ce | 113 | |
nakedt555 | 1:bdd17feaa4ce | 114 | HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); |
nakedt555 | 1:bdd17feaa4ce | 115 | __HAL_TIM_SET_COUNTER(&htim3, ENC_ZERO_POINT); |
nakedt555 | 1:bdd17feaa4ce | 116 | |
nakedt555 | 1:bdd17feaa4ce | 117 | encoder = 0; |
nakedt555 | 1:bdd17feaa4ce | 118 | } |
nakedt555 | 1:bdd17feaa4ce | 119 | int32_t get_encoder_pulse(){ |
nakedt555 | 1:bdd17feaa4ce | 120 | int32_t diff; |
nakedt555 | 1:bdd17feaa4ce | 121 | diff = (__HAL_TIM_GET_COUNTER(&htim3) - ENC_ZERO_POINT); |
nakedt555 | 1:bdd17feaa4ce | 122 | __HAL_TIM_SET_COUNTER(&htim3, ENC_ZERO_POINT); |
nakedt555 | 1:bdd17feaa4ce | 123 | encoder -= diff; |
nakedt555 | 1:bdd17feaa4ce | 124 | |
nakedt555 | 1:bdd17feaa4ce | 125 | return -diff; |
nakedt555 | 1:bdd17feaa4ce | 126 | } |
nakedt555 | 1:bdd17feaa4ce | 127 | int32_t get_encoder(){ |
nakedt555 | 1:bdd17feaa4ce | 128 | return encoder; |
nakedt555 | 1:bdd17feaa4ce | 129 | } |
nakedt555 | 1:bdd17feaa4ce | 130 | private: |
nakedt555 | 1:bdd17feaa4ce | 131 | TIM_HandleTypeDef htim3; |
nakedt555 | 1:bdd17feaa4ce | 132 | uint32_t encoder; |
nakedt555 | 1:bdd17feaa4ce | 133 | }; |
nakedt555 | 1:bdd17feaa4ce | 134 | |
nakedt555 | 1:bdd17feaa4ce | 135 | #endif |