X-CUBE-SPN1-20150128 example source code for one motor compiled under mbed. Tested OK on Nucleo F401. l6474.cpp is modified from original with defines in l6474_target_config.h to select the original behaviour (motor de-energised when halted), or new mode to continue powering with a (reduced) current in the coils (braking/position hold capability). On F401 avoid using mbed's InterruptIn on pins 10-15 (any port). Beware of other conflicts! L0 & F0 are included but untested.

Dependencies:   mbed

IHM01A1/stm32f4xx_nucleo_ihm01a1.cpp

Committer:
gregeric
Date:
2015-10-13
Revision:
6:19c1b4a04c24
Parent:
0:b9444a40a999

File content as of revision 6:19c1b4a04c24:

/**
  ******************************************************************************
  * @file    stm32f4xx_nucleo_ihm01a1.c
  * @author  IPC Rennes
  * @version V1.5.0
  * @date    November 12, 2014
  * @brief   BSP driver for x-nucleo-ihm01a1 Nucleo extension board 
  *  (based on L6474)
  ******************************************************************************
* @attention
  *
  * <h2><center>&copy; COPYRIGHT(c) 2014 STMicroelectronics</center></h2>
  *
  * Redistribution and use in source and binary forms, with or without modification,
  * are permitted provided that the following conditions are met:
  *   1. Redistributions of source code must retain the above copyright notice,
  *      this list of conditions and the following disclaimer.
  *   2. Redistributions in binary form must reproduce the above copyright notice,
  *      this list of conditions and the following disclaimer in the documentation
  *      and/or other materials provided with the distribution.
  *   3. Neither the name of STMicroelectronics nor the names of its contributors
  *      may be used to endorse or promote products derived from this software
  *      without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */ 
#ifdef TARGET_STM32F4  
/* Includes ------------------------------------------------------------------*/
#include "mbed.h"
#include "stm32f4xx_nucleo_ihm01a1.h"

/** @addtogroup BSP
  * @{
  */ 

/** @defgroup STM32F4XX_NUCLEO_IHM01A1
  * @{
  */   
    
/* Private constants ---------------------------------------------------------*/    

/** @defgroup IHM01A1_Private_Constants
  * @{
  */   
    
/// Timer Prescaler
#define TIMER_PRESCALER (1024)

/// SPI Maximum Timeout values for flags waiting loops
#define SPIx_TIMEOUT_MAX                      ((uint32_t)0x1000)

/**
  * @}
  */ 

/* Private variables ---------------------------------------------------------*/

/** @defgroup IHM01A1_Board_Private_Variables
  * @{
  */       
/// SPI handler declaration
static SPI_HandleTypeDef SpiHandle;
/// Timer handler for PWM1
TIM_HandleTypeDef hTimPwm1;
/// Timer handler for PWM2
TIM_HandleTypeDef hTimPwm2;
/// Timer handler for PWM3
TIM_HandleTypeDef hTimPwm3;
/**
  * @}
  */ 

/** @defgroup IHM01A1_Board_Private_Function_Prototypes
  * @{
  */   
   
//mbed moved to header file

/**
  * @}
  */


/** @defgroup  IHM01A1_Board_Private_Functions
  * @{
  */   

/******************************************************//**
 * @brief This function provides an accurate delay in milliseconds
 * @param[in] delay  time length in milliseconds
 * @retval None
 **********************************************************/
void BSP_MotorControlBoard_Delay(uint32_t delay)
{
  HAL_Delay(delay);
}

/******************************************************//**
 * @brief This function disable the interruptions
 * @param None
 * @retval None
 **********************************************************/
void BSP_MotorControlBoard_DisableIrq(void)
{
  __disable_irq();
}

/******************************************************//**
 * @brief This function enable the interruptions
 * @param None
 * @retval None
 **********************************************************/
void BSP_MotorControlBoard_EnableIrq(void)
{
  __enable_irq();
}

/******************************************************//**
 * @brief  Initiliases the GPIOs used by the L6474s
 * @param[in] nbDevices number of L6474 devices
 * @retval None
  **********************************************************/
void BSP_MotorControlBoard_GpioInit(uint8_t nbDevices)
{
   GPIO_InitTypeDef GPIO_InitStruct;
  
  /* GPIO Ports Clock Enable */
  __GPIOC_CLK_ENABLE();
  __GPIOA_CLK_ENABLE();
  __GPIOB_CLK_ENABLE();

  /* Configure L6474 - DIR pin for device 1 -------------------------------*/
  GPIO_InitStruct.Pin = BSP_MOTOR_CONTROL_BOARD_DIR_1_PIN;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_MEDIUM;
  HAL_GPIO_Init(BSP_MOTOR_CONTROL_BOARD_DIR_1_PORT, &GPIO_InitStruct);
  
  /* Configure L6474 - Flag pin -------------------------------------------*/
  GPIO_InitStruct.Pin = BSP_MOTOR_CONTROL_BOARD_FLAG_PIN;
  GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
  GPIO_InitStruct.Pull = GPIO_PULLUP;
  GPIO_InitStruct.Speed = GPIO_SPEED_MEDIUM;
  HAL_GPIO_Init(BSP_MOTOR_CONTROL_BOARD_FLAG_PORT, &GPIO_InitStruct);
  
 /* Set Priority of External Line Interrupt used for the Flag interrupt*/ 
  HAL_NVIC_SetPriority(EXTI_MCU_LINE_IRQn, 5, 0);
    
  /* Enable the External Line Interrupt used for the Flag interrupt*/
  HAL_NVIC_EnableIRQ(EXTI_MCU_LINE_IRQn);    

  /* Configure L6474 - CS pin ---------------------------------------------*/
  GPIO_InitStruct.Pin = BSP_MOTOR_CONTROL_BOARD_CS_PIN;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_MEDIUM;
  HAL_GPIO_Init(BSP_MOTOR_CONTROL_BOARD_CS_PORT, &GPIO_InitStruct);
  HAL_GPIO_WritePin(BSP_MOTOR_CONTROL_BOARD_CS_PORT, BSP_MOTOR_CONTROL_BOARD_CS_PIN, GPIO_PIN_SET); 
  
  /* Configure L6474 - STBY/RESET pin -------------------------------------*/
  GPIO_InitStruct.Pin = BSP_MOTOR_CONTROL_BOARD_RESET_PIN;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_MEDIUM;
  HAL_GPIO_Init(BSP_MOTOR_CONTROL_BOARD_RESET_PORT, &GPIO_InitStruct);
  BSP_MotorControlBoard_Reset();  

  if (nbDevices > 1) 
  {
    /* Configure L6474 - DIR pin for device  2 ----------------------------*/
    GPIO_InitStruct.Pin = BSP_MOTOR_CONTROL_BOARD_DIR_2_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_MEDIUM;
    HAL_GPIO_Init(BSP_MOTOR_CONTROL_BOARD_DIR_2_PORT, &GPIO_InitStruct);    
  }
  if (nbDevices > 2) 
  {
    /* Configure L6474 - DIR pin for device  3 ----------------------------*/
    GPIO_InitStruct.Pin = BSP_MOTOR_CONTROL_BOARD_DIR_3_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_MEDIUM;
    HAL_GPIO_Init(BSP_MOTOR_CONTROL_BOARD_DIR_3_PORT, &GPIO_InitStruct);    
  }  
}

/******************************************************//**
 * @brief  Sets the frequency of PWM1 used by device 0
 * @param[in] newFreq in Hz
 * @retval None
 * @note The frequency is directly the current speed of the device
 **********************************************************/
void BSP_MotorControlBoard_Pwm1SetFreq(uint16_t newFreq)
{
  uint32_t sysFreq = HAL_RCC_GetSysClockFreq();
  uint32_t period = (sysFreq/ (TIMER_PRESCALER * BSP_MOTOR_CONTROL_BOARD_PWM1_FREQ_RESCALER * (uint32_t)newFreq)) - 1;
  
  __HAL_TIM_SetAutoreload(&hTimPwm1, period);
  __HAL_TIM_SetCompare(&hTimPwm1, BSP_MOTOR_CONTROL_BOARD_CHAN_TIMER_PWM1, period >> 1);
  HAL_TIM_PWM_Start_IT(&hTimPwm1, BSP_MOTOR_CONTROL_BOARD_CHAN_TIMER_PWM1);  
}

/******************************************************//**
 * @brief  Sets the frequency of PWM2 used by device 1
 * @param[in] newFreq in Hz
 * @retval None
 * @note The frequency is directly the current speed of the device
 **********************************************************/
void BSP_MotorControlBoard_Pwm2SetFreq(uint16_t newFreq)
{
  uint32_t sysFreq = HAL_RCC_GetSysClockFreq();
  uint32_t period = (sysFreq/ (TIMER_PRESCALER * BSP_MOTOR_CONTROL_BOARD_PWM2_FREQ_RESCALER  * (uint32_t)newFreq)) - 1;
  
  __HAL_TIM_SetAutoreload(&hTimPwm2, period);
  __HAL_TIM_SetCompare(&hTimPwm2, BSP_MOTOR_CONTROL_BOARD_CHAN_TIMER_PWM2, period >> 1);
  HAL_TIM_PWM_Start_IT(&hTimPwm2, BSP_MOTOR_CONTROL_BOARD_CHAN_TIMER_PWM2);
}
/******************************************************//**
 * @brief  Sets the frequency of PWM3 used by device 2
 * @param[in] newFreq in Hz
 * @retval None
 * @note The frequency is directly the current speed of the device
 **********************************************************/
void BSP_MotorControlBoard_Pwm3SetFreq(uint16_t newFreq)
{
  uint32_t sysFreq = HAL_RCC_GetSysClockFreq();
  /* Double the frequency as the SW is generated by SW */
  uint32_t period = (sysFreq/ (TIMER_PRESCALER * BSP_MOTOR_CONTROL_BOARD_PWM3_FREQ_RESCALER * (uint32_t)newFreq)) - 1;
  
  __HAL_TIM_SetAutoreload(&hTimPwm3, period);
  __HAL_TIM_SetCompare(&hTimPwm3, BSP_MOTOR_CONTROL_BOARD_CHAN_TIMER_PWM3, period >> 1);
  HAL_TIM_PWM_Start_IT(&hTimPwm3, BSP_MOTOR_CONTROL_BOARD_CHAN_TIMER_PWM3);  
}

/******************************************************//**
 * @brief  Initialises the PWM uses by the specified device
 * @param[in] deviceId (from 0 to 2)
 * @retval None
 * @note Device 0 uses PWM1 based on timer 1 
 * Device 1 uses PWM 2 based on timer 2
 * Device 2 uses PWM3 based timer 0
 **********************************************************/
void BSP_MotorControlBoard_PwmInit(uint8_t deviceId)
{
  TIM_OC_InitTypeDef sConfigOC;
  TIM_MasterConfigTypeDef sMasterConfig;
  TIM_HandleTypeDef *pHTim;
  uint32_t  channel;

  switch (deviceId)
  {

  case 0:
  default:
      pHTim = &hTimPwm1;
      pHTim->Instance = BSP_MOTOR_CONTROL_BOARD_TIMER_PWM1;
      channel = BSP_MOTOR_CONTROL_BOARD_CHAN_TIMER_PWM1;

      break;
    case  1:
      pHTim = &hTimPwm2;
      pHTim->Instance = BSP_MOTOR_CONTROL_BOARD_TIMER_PWM2;
      channel = BSP_MOTOR_CONTROL_BOARD_CHAN_TIMER_PWM2;
      break;


    case 2:
      pHTim = &hTimPwm3;
      pHTim->Instance = BSP_MOTOR_CONTROL_BOARD_TIMER_PWM3;
      channel = BSP_MOTOR_CONTROL_BOARD_CHAN_TIMER_PWM3;
      break;
  }
  pHTim->Init.Prescaler = TIMER_PRESCALER -1;
  pHTim->Init.CounterMode = TIM_COUNTERMODE_UP;
  pHTim->Init.Period = 0;
  pHTim->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  HAL_TIM_PWM_Init(pHTim);
  
  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 0;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  HAL_TIM_PWM_ConfigChannel(pHTim, &sConfigOC, channel);
  
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  HAL_TIMEx_MasterConfigSynchronization(pHTim, &sMasterConfig);
}

/******************************************************//**
 * @brief  Stops the PWM uses by the specified device
 * @param[in] deviceId (from 0 to 2)
 * @retval None
 **********************************************************/
void BSP_MotorControlBoard_PwmStop(uint8_t deviceId)
{
  switch (deviceId)
  {
    case 0:
       HAL_TIM_PWM_Stop(&hTimPwm1,BSP_MOTOR_CONTROL_BOARD_CHAN_TIMER_PWM1);
    
      break;
    case  1:
      HAL_TIM_PWM_Stop(&hTimPwm2,BSP_MOTOR_CONTROL_BOARD_CHAN_TIMER_PWM2);
      
      break;
    case 2:
       HAL_TIM_PWM_Stop(&hTimPwm3,BSP_MOTOR_CONTROL_BOARD_CHAN_TIMER_PWM3);
      
      break;
    default:
      break;//ignore error
  }
}

/******************************************************//**
 * @brief  Releases the L6474 reset (pin set to High) of all devices
 * @param  None
 * @retval None
 **********************************************************/
void BSP_MotorControlBoard_ReleaseReset(void)
{ 
  HAL_GPIO_WritePin(BSP_MOTOR_CONTROL_BOARD_RESET_PORT, BSP_MOTOR_CONTROL_BOARD_RESET_PIN, GPIO_PIN_SET); 
}

/******************************************************//**
 * @brief  Resets the L6474 (reset pin set to low) of all devices
 * @param  None
 * @retval None
 **********************************************************/
void BSP_MotorControlBoard_Reset(void)
{
  HAL_GPIO_WritePin(BSP_MOTOR_CONTROL_BOARD_RESET_PORT, BSP_MOTOR_CONTROL_BOARD_RESET_PIN, GPIO_PIN_RESET); 
}

/******************************************************//**
 * @brief  Set the GPIO used for the direction
 * @param[in] deviceId (from 0 to 2)
 * @param[in] gpioState state of the direction gpio (0 to reset, 1 to set)
 * @retval None
 **********************************************************/
void BSP_MotorControlBoard_SetDirectionGpio(uint8_t deviceId, uint8_t gpioState)
{
  switch (deviceId)
  {
    case 2:
      HAL_GPIO_WritePin(BSP_MOTOR_CONTROL_BOARD_DIR_3_PORT, BSP_MOTOR_CONTROL_BOARD_DIR_3_PIN, (GPIO_PinState)gpioState); 
      break;
    case 1:
      HAL_GPIO_WritePin(BSP_MOTOR_CONTROL_BOARD_DIR_2_PORT, BSP_MOTOR_CONTROL_BOARD_DIR_2_PIN, (GPIO_PinState)gpioState); 
      break;
    case 0:
      HAL_GPIO_WritePin(BSP_MOTOR_CONTROL_BOARD_DIR_1_PORT, BSP_MOTOR_CONTROL_BOARD_DIR_1_PIN, (GPIO_PinState)gpioState); 
      break;
    default:
      ;
  }
}

/******************************************************//**
 * @brief  Initialise the SPI used by L6474
 * @param None
 * @retval HAL_OK if SPI transaction is OK, HAL_KO else
 **********************************************************/
uint8_t BSP_MotorControlBoard_SpiInit(void)
{
  HAL_StatusTypeDef status;
  
  /* Initialises the SPI  --------------------------------------------------*/
  SpiHandle.Instance               = SPIx;
  SpiHandle.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32; 
  SpiHandle.Init.Direction         = SPI_DIRECTION_2LINES;
  SpiHandle.Init.CLKPhase          = SPI_PHASE_2EDGE;    
  SpiHandle.Init.CLKPolarity       = SPI_POLARITY_HIGH;
  SpiHandle.Init.CRCCalculation    = SPI_CRCCALCULATION_DISABLED;
  SpiHandle.Init.CRCPolynomial     = 7;
  SpiHandle.Init.DataSize          = SPI_DATASIZE_8BIT;
  SpiHandle.Init.FirstBit          = SPI_FIRSTBIT_MSB;
  SpiHandle.Init.NSS               = SPI_NSS_SOFT;
  SpiHandle.Init.TIMode            = SPI_TIMODE_DISABLED;
  
  SpiHandle.Init.Mode = SPI_MODE_MASTER;
  
  status = HAL_SPI_Init(&SpiHandle);
  
  return (uint8_t) status;
}
/******************************************************//**
 * @brief  Write and read SPI byte to the L6474
 * @param[in] pByteToTransmit pointer to the byte to transmit
 * @param[in] pReceivedByte pointer to the received byte
 * @param[in] nbDevices Number of device in the SPI chain
 * @retval HAL_OK if SPI transaction is OK, HAL_KO else 
 **********************************************************/
uint8_t BSP_MotorControlBoard_SpiWriteBytes(uint8_t *pByteToTransmit, uint8_t *pReceivedByte, uint8_t nbDevices)
{
  HAL_StatusTypeDef status;
  uint32_t i;
  HAL_GPIO_WritePin(BSP_MOTOR_CONTROL_BOARD_CS_PORT, BSP_MOTOR_CONTROL_BOARD_CS_PIN, GPIO_PIN_RESET); 
  for (i = 0; i < nbDevices; i++)
  {
    status = HAL_SPI_TransmitReceive(&SpiHandle, pByteToTransmit, pReceivedByte, 1, SPIx_TIMEOUT_MAX);
    if (status != HAL_OK)
    {
      break;
    }
    pByteToTransmit++;
    pReceivedByte++;
  }
  HAL_GPIO_WritePin(BSP_MOTOR_CONTROL_BOARD_CS_PORT, BSP_MOTOR_CONTROL_BOARD_CS_PIN, GPIO_PIN_SET); 
  
  return (uint8_t) status;  
}

/**
  * @}
  */

/**
  * @}
  */    

/**
  * @}
  */ 
#endif    
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/