Diff: main.cpp

Revision:

0:9646fc4e3fae

diff -r 000000000000 -r 9646fc4e3fae main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri May 19 09:12:45 2017 +0000
@@ -0,0 +1,543 @@
+#include "mbed.h"
+#include "cmsis_os.h"
+#include "main.h"
+#include "stm32f7xx_hal.h"
+#include <ros.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Vector3.h>
+#include <openlab/mobileRobot.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Bool.h>
+
+#define pi 3.1416  //for angular velocity calculation
+#define length 0.3 //distance between 2 wheel - meter
+#define radius 0.036 //radius of wheel with load - meter 
+#define timeConst 0.00926 //time constant
+
+/*Global variables*/
+int8_t ADC_buffer[2];
+int32_t encoder1_1 = 0;
+int32_t encoder1=0;
+int32_t encoder2_1 = 0;
+int32_t encoder2=0;
+float pwmLeft=0;
+float pwmRight=0;
+float dc_current1=0;
+float dc_current2=0;
+double real_velocity_linx=0;
+double real_velocity_angz=0;
+double cmd_velocity_linx=0;
+double cmd_velocity_angz=0;
+
+float kp_left = 39;
+float kd_left = 2;
+float ki_left = 10;
+float kp_right = 39;
+float kd_right = 2;
+float ki_right = 10;
+double errDirVel_1 = 0;
+double errAngVel_1 = 0;
+double errorLeft = 0;
+double errorRight = 0;
+double errorLeft_1 = 0;
+double errorRight_1 = 0;
+double linearLeft = 0;
+double linearRight = 0;
+double driveLeft = 0;
+double driveRight = 0;
+double iLeft = 0;
+double iRight = 0;
+
+float i_max = 1000;
+float dLeft = 0;
+float dRight = 0;
+
+int diff1=0;
+int diff2=0;
+float current1;
+float current2;
+
+
+
+ros::NodeHandle  nh;
+openlab::mobileRobot str_msg;
+ros::Publisher RobotFeedback("RobotFeedback", &str_msg);
+
+void callback_vel( const geometry_msgs::Twist& vel){
+  
+  cmd_velocity_linx = vel.linear.x;
+  cmd_velocity_angz = vel.angular.z;
+ 
+  
+   nh.spinOnce();
+   osDelay(10);
+}
+
+ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", callback_vel );
+
+/*loop control*/
+//DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+/* USER CODE BEGIN Includes */
+
+/* USER CODE END Includes */
+
+/* Private variables ---------------------------------------------------------*/
+ADC_HandleTypeDef hadc2;
+DMA_HandleTypeDef hdma_adc2;
+
+TIM_HandleTypeDef htim1;
+TIM_HandleTypeDef htim4;
+TIM_HandleTypeDef htim9;
+
+
+/* Private function prototypes -----------------------------------------------*/
+
+void Error_Handler(void);
+static void MX_GPIO_Init(void);
+static void MX_DMA_Init(void);
+static void MX_ADC2_Init(void);
+static void MX_TIM1_Init(void);
+static void MX_TIM4_Init(void);
+static void MX_TIM9_Init(void);
+
+void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
+
+void feedback_thread(void const *args)
+{
+    while (true) {
+     
+      
+      str_msg.pwm_1 = pwmLeft;
+      str_msg.pwm_2 = pwmRight;
+      str_msg.real_velocity_linx = real_velocity_linx;
+      str_msg.real_velocity_angz = real_velocity_angz;
+      str_msg.cmd_velocity_linx = cmd_velocity_linx;
+      str_msg.cmd_velocity_angz = cmd_velocity_angz;
+      str_msg.dc_current_1 = current1;
+      str_msg.dc_current_2 = current2;
+      str_msg.encoder_1 = encoder1;
+      str_msg.encoder_2 = encoder2;
+      RobotFeedback.publish( &str_msg );
+      nh.spinOnce();
+     osDelay(10);
+        
+    }
+}
+
+void velocity_control(void const *args)
+{
+    
+    while (true) {
+
+    current1 = ADC_buffer[0];
+    current2 = ADC_buffer[1];
+    encoder1 = TIM1->CNT;
+    encoder2 = TIM4->CNT;
+   
+
+    diff1=encoder1 - encoder1_1;
+    diff2=encoder2 - encoder2_1;
+    
+    if(diff1 < - 30000) diff1 = diff1 + 65536;
+    if(diff1 > 30000)  diff1 = diff1 - 65536;
+    if(diff2 < - 30000) diff2 = diff2 + 65536;
+    if(diff2> 30000)  diff2 = diff2 - 65536;
+
+    linearLeft = diff1 * 0.000748*200*radius; 
+    linearRight = diff2 * 0.000748*200*radius; 
+ 
+    real_velocity_linx = (linearLeft + linearRight) * 0.5;
+    real_velocity_angz = (linearRight - linearLeft) * 5.8824;
+
+    encoder1_1 = encoder1;
+    encoder2_1 = encoder2;
+
+    driveLeft = cmd_velocity_linx - (0.17 * cmd_velocity_angz * 0.5);
+    driveRight = cmd_velocity_linx + (0.17 * cmd_velocity_angz * 0.5);
+
+    errorLeft = driveLeft - linearLeft;
+    errorRight = driveRight - linearRight;
+
+    iRight = iRight + (errorRight * 1);
+    iLeft = iLeft + (errorLeft * 1);
+    dLeft = (errorLeft - errorLeft_1) * 200;
+    dRight = (errorRight - errorRight_1) * 200;
+
+    errorLeft_1 = errorLeft;
+    errorRight_1 = errorRight;
+    
+    if(iRight > i_max) iRight = i_max;
+    if(iRight < -i_max) iRight = i_max;
+    if(iLeft > i_max) iLeft = i_max;
+    if(iLeft < -i_max) iLeft = i_max;
+
+   pwmLeft = kp_left * errorLeft + ki_left * iLeft + kd_left * dLeft;
+   pwmRight = kp_right * errorRight + ki_right * iRight + kd_right * dRight;
+   //HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_0);
+  
+
+    if(cmd_velocity_linx || cmd_velocity_angz) 
+  {
+
+   
+   
+
+    if(pwmLeft>0)
+    {
+        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_3, GPIO_PIN_SET);
+        //wait(0.005);
+        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_4, GPIO_PIN_RESET);
+        //wait(0.005);
+        pwmLeft = 61 + pwmLeft;
+    }
+    if (pwmLeft<0)
+    {
+        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_3, GPIO_PIN_RESET);
+       // wait(0.005);
+        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_4, GPIO_PIN_SET);
+        //wait(0.005);
+        pwmLeft = 61 - pwmLeft;
+    }
+    if(pwmRight>0)
+    {
+        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET);
+        //wait(0.005);
+        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_15, GPIO_PIN_RESET);
+        //wait(0.005);
+        pwmRight = 60 + pwmRight;
+    }
+     if(pwmRight<0)
+    {
+        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET);
+        //wait(0.005);
+        HAL_GPIO_WritePin(GPIOD, GPIO_PIN_15, GPIO_PIN_SET);
+       // wait(0.005);
+        pwmRight = 60 - pwmRight;
+    }
+    
+    
+  }
+  else
+  {
+    pwmRight = 0;
+    pwmLeft = 0;
+    iRight = 0;
+    iLeft = 0;
+  }
+
+    if(pwmRight > 99) pwmRight = 99;
+    if(pwmRight < -99) pwmRight = -99;
+    if(pwmLeft > 99) pwmLeft = 99;
+    if(pwmLeft < -99) pwmLeft = -99;
+    
+   __HAL_TIM_SetCompare(&htim9, TIM_CHANNEL_1, pwmLeft);
+   __HAL_TIM_SetCompare(&htim9, TIM_CHANNEL_2, pwmRight);
+  
+    //htim9.Instance->CCR1 = pwm1;
+   // htim9.Instance->CCR2 = pwm2;
+
+
+      osDelay(5);
+
+        
+    }
+}
+
+ osThreadDef(feedback_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
+ osThreadDef(velocity_control, osPriorityNormal, DEFAULT_STACK_SIZE);
+
+
+int main()
+{
+
+/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
+  HAL_Init();
+
+
+  /* Initialize all configured peripherals */
+  MX_GPIO_Init();
+  MX_DMA_Init();
+  MX_ADC2_Init();
+  MX_TIM1_Init();
+  MX_TIM4_Init();
+  MX_TIM9_Init();
+  
+  /*Initialize necessery configuration*/
+    HAL_ADC_Start_DMA(&hadc2, (uint32_t *)ADC_buffer, 2);
+    HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_1|TIM_CHANNEL_2);
+    HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_1|TIM_CHANNEL_2);
+    HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_1);
+    HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_2);
+
+    nh.initNode();
+    nh.advertise(RobotFeedback);
+    nh.subscribe(sub);
+
+    printf("\n\n*** RTOS basic example ***\n");
+    
+    
+   osThreadCreate(osThread(feedback_thread), NULL);
+   osThreadCreate(osThread(velocity_control), NULL);
+    
+
+    while (true) {
+        led2 = !led2;
+        osDelay(500);
+    }
+}
+
+ /* TIM1 init function */
+static void MX_TIM1_Init(void)
+{
+
+  TIM_Encoder_InitTypeDef sConfig;
+  TIM_MasterConfigTypeDef sMasterConfig;
+
+  htim1.Instance = TIM1;
+  htim1.Init.Prescaler = 0;
+  htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim1.Init.Period = 65535;
+  htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim1.Init.RepetitionCounter = 0;
+  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;
+  if (HAL_TIM_Encoder_Init(&htim1, &sConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+}
+
+/* TIM4 init function */
+static void MX_TIM4_Init(void)
+{
+
+  TIM_Encoder_InitTypeDef sConfig;
+  TIM_MasterConfigTypeDef sMasterConfig;
+
+  htim4.Instance = TIM4;
+  htim4.Init.Prescaler = 0;
+  htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim4.Init.Period = 65535;
+  htim4.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;
+  if (HAL_TIM_Encoder_Init(&htim4, &sConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+}
+
+/* ADC2 init function */
+static void MX_ADC2_Init(void)
+{
+
+  ADC_ChannelConfTypeDef sConfig;
+
+    /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) 
+    */
+  hadc2.Instance = ADC2;
+  hadc2.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2;
+  hadc2.Init.Resolution = ADC_RESOLUTION_8B;
+  hadc2.Init.ScanConvMode = ENABLE;
+  hadc2.Init.ContinuousConvMode = ENABLE;
+  hadc2.Init.DiscontinuousConvMode = DISABLE;
+  hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
+  hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START;
+  hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
+  hadc2.Init.NbrOfConversion = 2;
+  hadc2.Init.DMAContinuousRequests = ENABLE;
+  hadc2.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+  if (HAL_ADC_Init(&hadc2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+    /**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. 
+    */
+  sConfig.Channel = ADC_CHANNEL_5;
+  sConfig.Rank = 1;
+  sConfig.SamplingTime = ADC_SAMPLETIME_144CYCLES;
+  if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+    /**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. 
+    */
+  sConfig.Channel = ADC_CHANNEL_6;
+  sConfig.Rank = 2;
+  if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+}
+
+/* TIM9 init function */
+static void MX_TIM9_Init(void)
+{
+
+  TIM_OC_InitTypeDef sConfigOC;
+
+  htim9.Instance = TIM9;
+  htim9.Init.Prescaler = 215;
+  htim9.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim9.Init.Period = 99;
+  htim9.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  if (HAL_TIM_PWM_Init(&htim9) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+  sConfigOC.OCMode = TIM_OCMODE_PWM1;
+  sConfigOC.Pulse = 0;
+  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
+  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
+  if (HAL_TIM_PWM_ConfigChannel(&htim9, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+  if (HAL_TIM_PWM_ConfigChannel(&htim9, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+  HAL_TIM_MspPostInit(&htim9);
+
+}
+
+/** 
+  * Enable DMA controller clock
+  */
+static void MX_DMA_Init(void) 
+{
+  /* DMA controller clock enable */
+  __HAL_RCC_DMA2_CLK_ENABLE();
+
+  /* DMA interrupt init */
+  /* DMA2_Stream2_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn);
+
+}
+
+/** Configure pins as 
+        * Analog 
+        * Input 
+        * Output
+        * EVENT_OUT
+        * EXTI
+*/
+static void MX_GPIO_Init(void)
+{
+
+  GPIO_InitTypeDef GPIO_InitStruct;
+
+  /* GPIO Ports Clock Enable */
+  __HAL_RCC_GPIOE_CLK_ENABLE();
+  __HAL_RCC_GPIOA_CLK_ENABLE();
+  __HAL_RCC_GPIOB_CLK_ENABLE();
+  __HAL_RCC_GPIOD_CLK_ENABLE();
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14|GPIO_PIN_15|GPIO_PIN_3|GPIO_PIN_4, GPIO_PIN_RESET);
+
+  /*Configure GPIO pin : PB0 */
+  GPIO_InitStruct.Pin = GPIO_PIN_0;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : PD14 PD15 PD3 PD4 */
+  GPIO_InitStruct.Pin = GPIO_PIN_14|GPIO_PIN_15|GPIO_PIN_3|GPIO_PIN_4;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
+
+}
+/* USER CODE BEGIN 4 */
+
+/* USER CODE END 4 */
+
+/**
+  * @brief  This function is executed in case of error occurrence.
+  * @param  None
+  * @retval None
+  */
+void Error_Handler(void)
+{
+  /* USER CODE BEGIN Error_Handler */
+  /* User can add his own implementation to report the HAL error return state */
+  while(1) 
+  {
+  }
+  /* USER CODE END Error_Handler */ 
+}
+
+void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim)
+{
+
+  GPIO_InitTypeDef GPIO_InitStruct;
+  if(htim->Instance==TIM9)
+  {
+  /* USER CODE BEGIN TIM9_MspPostInit 0 */
+
+  /* USER CODE END TIM9_MspPostInit 0 */
+  
+    /**TIM9 GPIO Configuration    
+    PE5     ------> TIM9_CH1
+    PE6     ------> TIM9_CH2 
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_5|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_AF3_TIM9;
+    HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN TIM9_MspPostInit 1 */
+
+  /* USER CODE END TIM9_MspPostInit 1 */
+  }
+
+}
\ No newline at end of file