main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "stm32f7xx_hal.h"
00004 #include <ros.h>
00005 #include <beginner_tutorials/RobotFeedback.h>
00006 
00007 TIM_HandleTypeDef htim1;
00008 TIM_HandleTypeDef htim2;
00009 TIM_HandleTypeDef htim3;
00010 TIM_HandleTypeDef htim4;
00011 
00012 ros::NodeHandle  nh;
00013 beginner_tutorials::RobotFeedback str_msg;
00014 ros::Publisher RobotFeedback("RobotFeedback", &str_msg);
00015 DigitalOut led1(LED1);
00016 
00017 void Error_Handler(void);
00018 static void MX_GPIO_Init(void);
00019 static void MX_TIM1_Init(void);
00020 static void MX_TIM2_Init(void);
00021 static void MX_TIM3_Init(void);
00022 static void MX_TIM4_Init(void);
00023 
00024 int32_t encoder1=0;
00025 int32_t encoder2=0;
00026 int32_t encoder3=0;
00027 int32_t encoder4=0;
00028 float pwm1=0;
00029 float pwm2=0;
00030 float pwm3=0;
00031 float pwm4=0;
00032 float dc_current1=0;
00033 float dc_current2=0;
00034 float dc_current3=0;
00035 float dc_current4=0;
00036 float sensor1=0;
00037 float sensor2=0;
00038 float sensor3=0;
00039 float sensor4=0;
00040 
00041 
00042 
00043 void feedback_thread(void const *argument)
00044 {
00045     
00046     while (true) {
00047         encoder1=TIM1->CNT;
00048         encoder2=TIM2->CNT;
00049         encoder3=TIM3->CNT;
00050         encoder4=TIM4->CNT;
00051         
00052         str_msg.encoder_1 = encoder1;
00053         str_msg.encoder_2 = encoder2;
00054         str_msg.encoder_3 = encoder3;
00055         str_msg.encoder_4 = encoder4;
00056         str_msg.pwm_1 = pwm1;
00057         str_msg.pwm_2 = pwm2;
00058         str_msg.pwm_3 = pwm3;
00059         str_msg.pwm_4 = pwm4;
00060         str_msg.dc_current_1 = dc_current1;
00061         str_msg.dc_current_2 = dc_current2;
00062         str_msg.dc_current_3 = dc_current3;
00063         str_msg.dc_current_4 = dc_current4;
00064         RobotFeedback.publish( &str_msg );
00065         nh.spinOnce();
00066         Thread::wait(10);
00067        
00068     }
00069 }
00070 
00071 int main()
00072 {
00073      HAL_Init();
00074      MX_GPIO_Init();
00075      MX_TIM1_Init();
00076      MX_TIM2_Init();
00077      MX_TIM3_Init();
00078      MX_TIM4_Init();
00079      HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_1|TIM_CHANNEL_2); 
00080      HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_1|TIM_CHANNEL_2); 
00081      HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_1|TIM_CHANNEL_2); 
00082      HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_1|TIM_CHANNEL_2); 
00083      
00084      nh.initNode();
00085      nh.advertise(RobotFeedback);
00086     
00087   Thread thread(feedback_thread, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
00088      
00089  
00090     while (true) {
00091      
00092      led1 = !led1;
00093      Thread::wait(500);
00094      
00095     }
00096 }
00097 
00098 
00099 /* TIM1 init function */
00100 static void MX_TIM1_Init(void)
00101 {
00102 
00103   TIM_Encoder_InitTypeDef sConfig;
00104   TIM_MasterConfigTypeDef sMasterConfig;
00105 
00106   htim1.Instance = TIM1;
00107   htim1.Init.Prescaler = 0;
00108   htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
00109   htim1.Init.Period = 65535;
00110   htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
00111   htim1.Init.RepetitionCounter = 0;
00112   sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
00113   sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
00114   sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
00115   sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
00116   sConfig.IC1Filter = 0;
00117   sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
00118   sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
00119   sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
00120   sConfig.IC2Filter = 0;
00121   if (HAL_TIM_Encoder_Init(&htim1, &sConfig) != HAL_OK)
00122   {
00123     Error_Handler();
00124   }
00125 
00126   sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
00127   sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
00128   sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
00129   if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
00130   {
00131     Error_Handler();
00132   }
00133 
00134 }
00135 
00136 /* TIM2 init function */
00137 static void MX_TIM2_Init(void)
00138 {
00139 
00140   TIM_Encoder_InitTypeDef sConfig;
00141   TIM_MasterConfigTypeDef sMasterConfig;
00142 
00143   htim2.Instance = TIM2;
00144   htim2.Init.Prescaler = 0;
00145   htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
00146   htim2.Init.Period = 0;
00147   htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
00148   sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
00149   sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
00150   sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
00151   sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
00152   sConfig.IC1Filter = 0;
00153   sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
00154   sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
00155   sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
00156   sConfig.IC2Filter = 0;
00157   if (HAL_TIM_Encoder_Init(&htim2, &sConfig) != HAL_OK)
00158   {
00159     Error_Handler();
00160   }
00161 
00162   sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
00163   sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
00164   if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
00165   {
00166     Error_Handler();
00167   }
00168 
00169 }
00170 
00171 /* TIM3 init function */
00172 static void MX_TIM3_Init(void)
00173 {
00174 
00175   TIM_Encoder_InitTypeDef sConfig;
00176   TIM_MasterConfigTypeDef sMasterConfig;
00177 
00178   htim3.Instance = TIM3;
00179   htim3.Init.Prescaler = 0;
00180   htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
00181   htim3.Init.Period = 0;
00182   htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
00183   sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
00184   sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
00185   sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
00186   sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
00187   sConfig.IC1Filter = 0;
00188   sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
00189   sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
00190   sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
00191   sConfig.IC2Filter = 0;
00192   if (HAL_TIM_Encoder_Init(&htim3, &sConfig) != HAL_OK)
00193   {
00194     Error_Handler();
00195   }
00196 
00197   sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
00198   sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
00199   if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
00200   {
00201     Error_Handler();
00202   }
00203 
00204 }
00205 
00206 /* TIM4 init function */
00207 static void MX_TIM4_Init(void)
00208 {
00209 
00210   TIM_Encoder_InitTypeDef sConfig;
00211   TIM_MasterConfigTypeDef sMasterConfig;
00212 
00213   htim4.Instance = TIM4;
00214   htim4.Init.Prescaler = 0;
00215   htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
00216   htim4.Init.Period = 0;
00217   htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
00218   sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
00219   sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
00220   sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
00221   sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
00222   sConfig.IC1Filter = 0;
00223   sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
00224   sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
00225   sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
00226   sConfig.IC2Filter = 0;
00227   if (HAL_TIM_Encoder_Init(&htim4, &sConfig) != HAL_OK)
00228   {
00229     Error_Handler();
00230   }
00231 
00232   sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
00233   sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
00234   if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)
00235   {
00236     Error_Handler();
00237   }
00238 
00239 }
00240 
00241 /** Pinout Configuration
00242 */
00243 static void MX_GPIO_Init(void)
00244 {
00245 
00246   /* GPIO Ports Clock Enable */
00247   __HAL_RCC_GPIOA_CLK_ENABLE();
00248   __HAL_RCC_GPIOE_CLK_ENABLE();
00249   __HAL_RCC_GPIOD_CLK_ENABLE();
00250 
00251 }
00252 
00253 
00254 void Error_Handler(void)
00255 {
00256 
00257   while(1) 
00258   {
00259   }
00260 
00261 }