aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers encoder.h Source File

encoder.h

00001 #ifndef _ENCODER_H_
00002 #define _ENCODER_H_
00003 
00004 #include "mbed.h"
00005 
00006 #define ENC_ZERO_POINT  (0x7FFF)
00007 
00008 class TIM2Encoder{
00009 public:
00010     TIM2Encoder(){
00011         GPIO_InitTypeDef GPIO_InitStruct;
00012         
00013         __HAL_RCC_TIM2_CLK_ENABLE();
00014         __HAL_RCC_GPIOA_CLK_ENABLE();
00015         
00016         GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1;
00017         GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
00018         GPIO_InitStruct.Pull = GPIO_NOPULL;
00019         GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
00020         GPIO_InitStruct.Alternate = GPIO_AF1_TIM2;
00021         HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
00022         
00023         TIM_Encoder_InitTypeDef sConfig;
00024         TIM_MasterConfigTypeDef sMasterConfig;
00025         
00026         htim2.Instance = TIM2;
00027         htim2.Init.Prescaler = 0;
00028         htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
00029         htim2.Init.Period = 65535;
00030         htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
00031         sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
00032         sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
00033         sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
00034         sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
00035         sConfig.IC1Filter = 0;
00036         sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
00037         sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
00038         sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
00039         sConfig.IC2Filter = 0;
00040         
00041         HAL_TIM_Encoder_Init(&htim2, &sConfig);
00042         
00043         sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
00044         sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
00045         HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig);
00046         
00047         HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
00048         __HAL_TIM_SET_COUNTER(&htim2, ENC_ZERO_POINT);
00049         
00050         encoder = 0;
00051     }
00052     int32_t get_encoder_pulse(){
00053         int32_t diff;
00054         diff = (__HAL_TIM_GET_COUNTER(&htim2) - ENC_ZERO_POINT);
00055         __HAL_TIM_SET_COUNTER(&htim2, ENC_ZERO_POINT);
00056         encoder -= diff;
00057     
00058         return -diff;
00059     }
00060     int32_t get_encoder(){
00061         return encoder;   
00062     }
00063 private:
00064     TIM_HandleTypeDef htim2;
00065     uint32_t encoder;
00066 };
00067 
00068 class TIM3Encoder{
00069 public:
00070     TIM3Encoder(){
00071         GPIO_InitTypeDef GPIO_InitStruct;
00072         
00073         __HAL_RCC_TIM3_CLK_ENABLE();
00074         __HAL_RCC_GPIOA_CLK_ENABLE();
00075         __HAL_RCC_GPIOB_CLK_ENABLE();
00076       
00077         GPIO_InitStruct.Pin = GPIO_PIN_6;
00078         GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
00079         GPIO_InitStruct.Pull = GPIO_NOPULL;
00080         GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
00081         GPIO_InitStruct.Alternate = GPIO_AF2_TIM3;
00082         HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
00083     
00084         GPIO_InitStruct.Pin = GPIO_PIN_5;
00085         GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
00086         GPIO_InitStruct.Pull = GPIO_NOPULL;
00087         GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
00088         GPIO_InitStruct.Alternate = GPIO_AF2_TIM3;
00089         HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
00090         
00091         TIM_Encoder_InitTypeDef sConfig;
00092         TIM_MasterConfigTypeDef sMasterConfig;
00093         
00094         htim3.Instance = TIM3;
00095         htim3.Init.Prescaler = 0;
00096         htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
00097         htim3.Init.Period = 65535;
00098         htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
00099         sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
00100         sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
00101         sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
00102         sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
00103         sConfig.IC1Filter = 0;
00104         sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
00105         sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
00106         sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
00107         sConfig.IC2Filter = 0;
00108         HAL_TIM_Encoder_Init(&htim3, &sConfig);
00109         
00110         sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
00111         sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
00112         HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig);
00113         
00114         HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
00115         __HAL_TIM_SET_COUNTER(&htim3, ENC_ZERO_POINT);
00116         
00117         encoder = 0;
00118     }
00119     int32_t get_encoder_pulse(){
00120         int32_t diff;
00121         diff = (__HAL_TIM_GET_COUNTER(&htim3) - ENC_ZERO_POINT);
00122         __HAL_TIM_SET_COUNTER(&htim3, ENC_ZERO_POINT);
00123         encoder -= diff;
00124     
00125         return -diff;
00126     }
00127     int32_t get_encoder(){
00128         return encoder;   
00129     }
00130 private:
00131     TIM_HandleTypeDef htim3;
00132     uint32_t encoder;
00133 };
00134 
00135 #endif