File content as of revision 1:0f99619433e0:

#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 */
  }

}