ST Expansion SW Team / X_NUCLEO_IHM14A1

Dependencies:   ST_INTERFACES

Dependents:   HelloWorld_IHM14A1

Fork of X_NUCLEO_IHM14A1 by Davide Aliprandi

Components/STSPIN820/STSPIN820.h

Committer:
Davidroid
Date:
2018-09-27
Revision:
2:4fd08b67958c
Parent:
1:bc265521eb00

File content as of revision 2:4fd08b67958c:

/**
 *******************************************************************************
 * @file    STSPIN820.h
 * @author  Davide Aliprandi, STMicroelectronics
 * @version V1.0.0
 * @date    May 3rd, 2018
 * @brief   This file contains the class definition of an STSPIN820 Motor
 *          Control component.
 * @note    (C) COPYRIGHT 2018 STMicroelectronics
 *******************************************************************************
 * @attention
 *
 * <h2><center>&copy; COPYRIGHT(c) 2018 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.
 *
 *******************************************************************************
 */


/* Generated with STM32CubeTOO -----------------------------------------------*/


/* Revision ------------------------------------------------------------------*/
/*
    Repository:       http://svn.x-nucleodev.codex.cro.st.com/svnroot/X-NucleoDev
    Branch/Trunk/Tag: trunk
    Based on:         X-CUBE-SPN14/trunk/Drivers/BSP/Components/STSPIN820/STSPIN820.h
    Revision:         0
*/


/* Define to prevent recursive inclusion -------------------------------------*/

#ifndef __STSPIN820_CLASS_H
#define __STSPIN820_CLASS_H


/* Includes ------------------------------------------------------------------*/

/* ACTION 1 ------------------------------------------------------------------*
 * Include here platform specific header files.                               *
 *----------------------------------------------------------------------------*/        
#include "mbed.h"
/* ACTION 2 ------------------------------------------------------------------*
 * Include here component specific header files.                              *
 *----------------------------------------------------------------------------*/        
#include "STSPIN820_def.h"
/* ACTION 3 ------------------------------------------------------------------*
 * Include here interface specific header files.                              *
 *                                                                            *
 * Example:                                                                   *
 *   #include "../Interfaces/Humidity.h"                                      *
 *   #include "../Interfaces/Temperature.h"                                   *
 *----------------------------------------------------------------------------*/
#include "StepperMotor.h"


/* Classes -------------------------------------------------------------------*/

/**
 * @brief Class representing an STSPIN820 component.
 */
class STSPIN820 : public StepperMotor
{
public:

    /*** Constructor and Destructor Methods ***/

    /**
     * @brief Constructor.
     * @param en_fault_irq  pin name of the EN_FAULT pin of the component.
     * @param standby_reset pin name of the STBY\RST pin of the component.
     * @param direction     pin name of the DIR pin of the component.
     * @param ref_pwm       pin name of the REF_PWM pin of the component.
     * @param decay         pin name of the DECAY pin of the component.
     * @param step_clock    pin name of the STCK pin of the component.
     * @param mode1         pin name of the MODE1 pin of the component.
     * @param mode2         pin name of the MODE2 pin of the component.
     * @param mode3         pin name of the MODE3 pin of the component.
     */
    STSPIN820(PinName en_fault_irq, PinName standby_reset, PinName direction,
              PinName ref_pwm, PinName decay, PinName step_clock,
              PinName mode1, PinName mode2, PinName mode3) :
              StepperMotor(),
              en_fault_irq(en_fault_irq),
              standby_reset(standby_reset),
              direction(direction),
              ref_pwm(ref_pwm),
              decay(decay),
              step_clock(step_clock),
              mode1(mode1),
              mode2(mode2),
              mode3(mode3)
    {
        /* Checking stackability. */
        if (!(number_of_devices < MAX_NUMBER_OF_DEVICES)) {
            error("Instantiation of the STSPIN820 component failed: it can be stacked up to %d times.\r\n", MAX_NUMBER_OF_DEVICES);
        }

        /* ACTION 4 ----------------------------------------------------------*
         * Initialize here the component's member variables, one variable per *
         * line.                                                              *
         *                                                                    *
         * Example:                                                           *
         *   measure = 0;                                                     *
         *   instance_id = number_of_instances++;                             *
         *--------------------------------------------------------------------*/
        flag_interrupt_callback = 0;
        error_handler_callback = 0;
        toggle_odd = 0;
        pwm_period = 1;
        pwm_duty_cycle = 0;
        device_instance = number_of_devices++;
    }
    
    /**
     * @brief Destructor.
     */
    virtual ~STSPIN820(void) {}
    

    /*** Public Component Related Methods ***/

    /* ACTION 5 --------------------------------------------------------------*
     * Implement here the component's public methods, as wrappers of the C    *
     * component's functions.                                                 *
     * They should be:                                                        *
     *   + Methods with the same name of the C component's virtual table's    *
     *     functions (1);                                                     *
     *   + Methods with the same name of the C component's extended virtual   *
     *     table's functions, if any (2).                                     *
     *                                                                        *
     * Example:                                                               *
     *   virtual int get_value(float *p_data) //(1)                           *
     *   {                                                                    *
     *     return COMPONENT_get_value(float *pf_data);                        *
     *   }                                                                    *
     *                                                                        *
     *   virtual int enable_feature(void) //(2)                               *
     *   {                                                                    *
     *     return COMPONENT_enable_feature();                                 *
     *   }                                                                    *
     *------------------------------------------------------------------------*/
    virtual int init(void *init = NULL)
    {
        return (int) STSPIN820_Init((void *) init);
    }

    virtual int read_id(uint8_t *id = NULL)
    {
        return (int) STSPIN820_ReadId((uint8_t *) id);
    }

    virtual void attach_error_handler(void (*callback)(uint16_t error))
    {
        STSPIN820_AttachErrorHandler((void (*)(uint16_t error)) callback);
    }

    virtual void attach_flag_interrupt(void (*callback)(void))
    {
        STSPIN820_AttachFlagInterrupt((void (*)(void)) callback);
    }

    virtual void flag_interrupt_handler(void)
    {
        STSPIN820_FlagInterruptHandler();
    }

    virtual unsigned int get_acceleration(void)
    {
        return (unsigned int) STSPIN820_GetAcceleration();
    }

    virtual unsigned int get_speed(void)
    {
        return (unsigned int) STSPIN820_GetCurrentSpeed();
    }

    virtual unsigned int get_deceleration(void)
    {
        return (unsigned int) STSPIN820_GetDeceleration();
    }

    virtual motor_state_t get_device_state(void)
    {
        return (motor_state_t) STSPIN820_GetDeviceState();
    }

    virtual uint32_t get_fw_version(void)
    {
        return (uint32_t) STSPIN820_GetFwVersion();
    }

    virtual signed int get_mark(void)
    {
        return (signed int) STSPIN820_GetMark();
    }

    virtual unsigned int get_max_speed(void)
    {
        return (unsigned int) STSPIN820_GetMaxSpeed();
    }

    virtual unsigned int get_min_speed(void)
    {
        return (unsigned int) STSPIN820_GetMinSpeed();
    }

    virtual signed int get_position(void)
    {
        return (signed int) STSPIN820_GetPosition();
    }

    virtual void go_home(void)
    {
        STSPIN820_GoHome();
    }

    virtual void go_mark(void)
    {
        STSPIN820_GoMark();
    }

    virtual void go_to(signed int position)
    {
        STSPIN820_GoTo((signed int) position);
    }

    /**
     * @brief  Stopping the motor through an immediate deceleration up to zero speed.
     * @param  None.
     * @retval None.
     */
    virtual void soft_stop(void)
    {
        STSPIN820_SoftStop();
    }

    /**
     * @brief  Stopping the motor through an immediate infinite deceleration.
     * @param  None.
     * @retval None.
     */
    virtual void hard_stop(void)
    {
        STSPIN820_HardStop();
    }

    /**
     * @brief  Disabling the power bridge after performing a deceleration to zero.
     * @param  None.
     * @retval None.
     */
    virtual void soft_hiz(void) {}

    /**
     * @brief  Disabling the power bridge immediately.
     * @param  None.
     * @retval None.
     */
    virtual void hard_hiz(void)
    {
        STSPIN820_HardHiZ();
    }

    virtual void move(direction_t direction, unsigned int steps)
    {
        STSPIN820_Move((motor_direction_t) (direction == StepperMotor::FWD ? FORWARD : BACKWARD), (unsigned int) steps);
    }

    virtual void run(direction_t direction)
    {
        STSPIN820_Run((motor_direction_t) direction);
    }

    virtual bool set_acceleration(unsigned int acceleration)
    {
        return (bool) STSPIN820_SetAcceleration((unsigned int) acceleration);
    }

    virtual bool set_deceleration(unsigned int deceleration)
    {
        return (bool) STSPIN820_SetDeceleration((unsigned int) deceleration);
    }

    virtual void set_home(void)
    {
        STSPIN820_SetHome();
    }

    virtual void set_mark(void)
    {
        STSPIN820_SetMark();
    }

    virtual bool set_max_speed(unsigned int speed)
    {
        return (bool) STSPIN820_SetMaxSpeed((unsigned int) speed);
    }

    virtual bool set_min_speed(unsigned int speed)
    {
        return (bool) STSPIN820_SetMinSpeed((unsigned int) speed);
    }

    virtual void wait_while_active(void)
    {
        STSPIN820_WaitWhileActive();
    }

    virtual void cmd_disable(void)
    {
        STSPIN820_Disable();
    }

    virtual void cmd_enable(void)
    {
        STSPIN820_Enable();
    }

    virtual bool set_step_mode(step_mode_t step_mode)
    {
        return (bool) STSPIN820_SetStepMode((motor_step_mode_t) step_mode);
    }

    virtual void set_direction(direction_t direction)
    {
        STSPIN820_SetDirection((motor_direction_t) direction);
    }

    virtual void go_to_dir(direction_t direction, int32_t targetPosition)
    {
        STSPIN820_GoToDir((motor_direction_t) direction, (int32_t) targetPosition);
    }

    virtual unsigned int get_status(void)
    {
        return (unsigned int) STSPIN820_CheckStatusHw();
    }

    virtual void cmd_reset_device(void)
    {
        STSPIN820_PutDeviceInStandby();
    }

    virtual uint8_t get_nb_devices(void)
    {
        return (uint8_t) STSPIN820_GetNbDevices();
    }

    virtual uint32_t get_bridge_input_pwm_freq(void)
    {
        return (uint32_t) STSPIN820_VrefPwmGetFreq();
    }

    virtual void set_bridge_input_pwm_freq(uint32_t newFreq)
    {
        STSPIN820_VrefPwmSetFreq((uint32_t) newFreq);
    }

    virtual void set_stop_mode(motor_stop_mode_t stopMode)
    {
        STSPIN820_SetStopMode((motor_stop_mode_t) stopMode);
    }

    virtual motor_stop_mode_t get_stop_mode(void)
    {
        return (motor_stop_mode_t) STSPIN820_GetStopMode();
    }

    virtual void set_decay_mode(motor_decay_mode_t decayMode)
    {
        STSPIN820_SetDecayMode((motor_decay_mode_t) decayMode);
    }

    virtual motor_decay_mode_t get_decay_mode(void)
    {
        return (motor_decay_mode_t) STSPIN820_GetDecayMode();
    }

    virtual motor_step_mode_t get_step_mode(void)
    {
        return (motor_step_mode_t) STSPIN820_GetStepMode();
    }

    virtual direction_t get_direction(void)
    {
        return (direction_t) (STSPIN820_GetDirection() == FORWARD ? StepperMotor::FWD : StepperMotor::BWD);
    }

    virtual void exit_device_from_reset(void)
    {
        STSPIN820_ExitDeviceFromStandby();
    }

    virtual void set_torque(motor_torque_mode_t torqueMode, uint8_t torqueValue)
    {
        STSPIN820_SetTorque((motor_torque_mode_t) torqueMode, (uint8_t) torqueValue);
    }

    virtual uint8_t get_torque(motor_torque_mode_t torqueMode)
    {
        return (uint8_t) STSPIN820_GetTorque((motor_torque_mode_t) torqueMode);
    }

    virtual void set_torque_boost_enable(bool enable)
    {
        STSPIN820_SetTorqueBoostEnable((bool) enable);
    }

    virtual bool get_torque_boost_enable(void)
    {
        return (bool) STSPIN820_GetTorqueBoostEnable();
    }

    virtual void set_torque_boost_threshold(uint16_t speedThreshold)
    {
        STSPIN820_SetTorqueBoostThreshold((uint16_t) speedThreshold);
    }

    virtual uint16_t get_torque_boost_threshold(void)
    {
        return (uint16_t) STSPIN820_GetTorqueBoostThreshold();
    }


    /*** Public Interrupt Related Methods ***/

    /* ACTION 6 --------------------------------------------------------------*
     * Implement here interrupt related methods, if any.                      *
     * Note that interrupt handling is platform dependent, e.g.:              *
     *   + mbed:                                                              *
     *     InterruptIn feature_irq(pin); //Interrupt object.                  *
     *     feature_irq.rise(callback);   //Attach a callback.                 *
     *     feature_irq.mode(PullNone);   //Set interrupt mode.                *
     *     feature_irq.enable_irq();     //Enable interrupt.                  *
     *     feature_irq.disable_irq();    //Disable interrupt.                 *
     *   + Arduino:                                                           *
     *     attachInterrupt(pin, callback, RISING); //Attach a callback.       *
     *     detachInterrupt(pin);                   //Detach a callback.       *
     *                                                                        *
     * Example (mbed):                                                        *
     *   void attach_feature_irq(void (*fptr) (void))                         *
     *   {                                                                    *
     *     feature_irq.rise(fptr);                                            *
     *   }                                                                    *
     *                                                                        *
     *   void enable_feature_irq(void)                                        *
     *   {                                                                    *
     *     feature_irq.enable_irq();                                          *
     *   }                                                                    *
     *                                                                        *
     *   void disable_feature_irq(void)                                       *
     *   {                                                                    *
     *     feature_irq.disable_irq();                                         *
     *   }                                                                    *
     *------------------------------------------------------------------------*/
    /**
     * @brief  Attaching an interrupt handler to the FLAG interrupt.
     * @param  fptr An interrupt handler.
     * @retval None.
     */
    void attach_en_fault_irq(void (*fptr)(void))
    {
        en_fault_irq.mode(PullUp);
        en_fault_irq.fall(fptr);
    }
    
    /**
     * @brief  Enabling the FLAG interrupt handling.
     * @param  None.
     * @retval None.
     */
    void enable_en_fault_irq(void)
    {
        en_fault_irq.enable_irq();
    }
    
    /**
     * @brief  Disabling the FLAG interrupt handling.
     * @param  None.
     * @retval None.
     */
    void disable_en_fault_irq(void)
    {
        en_fault_irq.disable_irq();
    }


protected:

    /*** Protected Component Related Methods ***/

    /* ACTION 7 --------------------------------------------------------------*
     * Declare here the component's specific methods.                         *
     * They should be:                                                        *
     *   + Methods with the same name of the C component's virtual table's    *
     *     functions (1);                                                     *
     *   + Methods with the same name of the C component's extended virtual   *
     *     table's functions, if any (2);                                     *
     *   + Helper methods, if any, like functions declared in the component's *
     *     source files but not pointed by the component's virtual table (3). *
     *                                                                        *
     * Example:                                                               *
     *   status_t COMPONENT_get_value(float *f);   //(1)                      *
     *   status_t COMPONENT_enable_feature(void);  //(2)                      *
     *   status_t COMPONENT_compute_average(void); //(3)                      *
     *------------------------------------------------------------------------*/
    //Apply speed
    void STSPIN820_ApplySpeed(uint16_t newSpeed);
    //Apply torque
    void STSPIN820_ApplyTorque(motor_torque_mode_t torqueMode);
    //Compute speed profile
    void STSPIN820_ComputeSpeedProfile(uint32_t nbSteps);
    //Handler for the flag interrupt
    void STSPIN820_FlagInterruptHandler(void);
    //Set device paramenters to other values
    void STSPIN820_SetDeviceParamsOtherValues(void);
    //Set device paramenters to given values
    void STSPIN820_SetDeviceParamsToGivenValues(STSPIN820_init_t* initDevicePrm);
    //Set device paramenters to predefined values
    void STSPIN820_SetDeviceParamsToPredefinedValues(void);
    //Start moving
    void STSPIN820_StartMovement(void);
    //Handle the step clock
    void STSPIN820_StepClockHandler(void);
    //Start the STSPIN820 library
    status_t STSPIN820_Init(void *init);
    //Read Id to get driver instance
    status_t STSPIN820_ReadId(uint8_t *id);
    //Attach a user callback to the error handler
    void STSPIN820_AttachErrorHandler(void (*callback)(uint16_t));
    //Attach a user callback to the flag Interrupt
    void STSPIN820_AttachFlagInterrupt(void (*callback)(void));
    //Check if STSPIN820 has a fault by reading EN pin position
    uint8_t STSPIN820_CheckStatusHw(void);
    //Disable the power stage of the specified device
    void STSPIN820_Disable(void);
    //Enable the power stage of the specified device
    void STSPIN820_Enable(void);
    //Error handler which calls the user callback
    void STSPIN820_ErrorHandler(uint16_t error);
    //Exit STSPIN820 device from standby 
    void STSPIN820_ExitDeviceFromStandby(void);
    //Return the acceleration in pps^2
    uint16_t STSPIN820_GetAcceleration(void);
    //Return the current speed in pps
    uint16_t STSPIN820_GetCurrentSpeed(void);
    //Return the device decay mode
    motor_decay_mode_t STSPIN820_GetDecayMode(void);
    //Return the deceleration in pps^2
    uint16_t STSPIN820_GetDeceleration(void);
    //Return the device state
    motor_state_t STSPIN820_GetDeviceState(void);
    //Get the motor current direction
    motor_direction_t STSPIN820_GetDirection(void);
    //Return the FW version
    uint32_t STSPIN820_GetFwVersion(void);
    //Return the mark position 
    int32_t STSPIN820_GetMark(void);
    //Return the max speed in pps
    uint16_t STSPIN820_GetMaxSpeed(void);
    //Return the min speed in pps
    uint16_t STSPIN820_GetMinSpeed(void);
    //Return the nupber of devices
    uint8_t STSPIN820_GetNbDevices(void);
    //Return the ABS_POSITION (32b signed)
    int32_t STSPIN820_GetPosition(void);
    //Get the motor step mode
    motor_step_mode_t STSPIN820_GetStepMode(void);
    //Get the selected mode to stop the motor
    motor_stop_mode_t STSPIN820_GetStopMode(void);
    //Get the torque
    uint8_t STSPIN820_GetTorque(motor_torque_mode_t torqueMode);
    //Get the torque boost feature status
    bool STSPIN820_GetTorqueBoostEnable(void);
    //Get the torque boost threshold
    uint16_t STSPIN820_GetTorqueBoostThreshold(void);
    //Move to the home position
    void STSPIN820_GoHome(void);
    //Move to the Mark position
    void STSPIN820_GoMark(void);
    //Go to the specified position
    void STSPIN820_GoTo(int32_t targetPosition);
    //Go to the specified position using the specified direction
    void STSPIN820_GoToDir(motor_direction_t direction, int32_t targetPosition);
    //Stop the motor and keep holding torque
    void STSPIN820_HardStop(void);
    //Stop the motor and disable the power bridge
    void STSPIN820_HardHiZ(void);
    //Move the motor of the specified number of steps
    void STSPIN820_Move(motor_direction_t direction, uint32_t stepCount);
    //Put STSPIN820 device in standby (low power consumption)
    void STSPIN820_PutDeviceInStandby(void);
    //Run the motor 
    void STSPIN820_Run(motor_direction_t direction);
    //Set the acceleration in pps^2
    bool STSPIN820_SetAcceleration(uint16_t newAcc);
    //Set the deceleration in pps^2
    bool STSPIN820_SetDeceleration(uint16_t newDec);
    //Set the STSPIN820 decay mode pin
    void STSPIN820_SetDecayMode(motor_decay_mode_t decay);
    //Set the STSPIN820 direction pin
    void STSPIN820_SetDirection(motor_direction_t direction);
    //Set current position to be the home position
    void STSPIN820_SetHome(void);
    //Set current position to be the mark position
    void STSPIN820_SetMark(void);
    //Set the max speed in pps
    bool STSPIN820_SetMaxSpeed(uint16_t newMaxSpeed);
    //Set the min speed in pps
    bool STSPIN820_SetMinSpeed(uint16_t newMinSpeed);
    //Set the number of devices
    bool STSPIN820_SetNbDevices(uint8_t nbDevices);
    //Step mode selection
    bool STSPIN820_SetStepMode(motor_step_mode_t stepMode);
    //Select the mode to stop the motor
    void STSPIN820_SetStopMode(motor_stop_mode_t stopMode);
    //Set the torque
    void STSPIN820_SetTorque(motor_torque_mode_t torqueMode, uint8_t torqueValue);
    //Enable or disable the torque boost feature
    void STSPIN820_SetTorqueBoostEnable(bool enable);
    //Set the torque boost threshold
    void STSPIN820_SetTorqueBoostThreshold(uint16_t speedThreshold);
    //Progressively stop the motor by using the device deceleration and set deceleration torque
    bool STSPIN820_SoftStop(void);
    //Get the frequency of REF PWM of the specified device
    uint32_t STSPIN820_VrefPwmGetFreq(void);
    //Set the frequency of REF PWM of the specified device
    void STSPIN820_VrefPwmSetFreq(uint32_t newFreq);
    //Wait for the device state becomes Inactive
    void STSPIN820_WaitWhileActive(void);


    /* ACTION 8 --------------------------------------------------------------*
     * Implement here other I/O methods beyond those already implemented      *
     * above, which are declared extern within the component's header file.   *
     *------------------------------------------------------------------------*/
    /**
     * @brief  Making the CPU wait.
     * @param  None.
     * @retval None.
     */
    void STSPIN820_Delay(uint32_t delay)
    {
        wait_ms(delay);
    }

    /**
     * @brief  Enabling the power bridges (leaving the output bridges at HiZ).
     * @param  None.
     * @retval None.
     */
    void STSPIN820_Enable_Power_Bridges(void)
    {
        if (en_fault_irq.read() == 0)
        {
            //en_fault_irq.write(1);
            wait_ms(BRIDGE_TURN_ON_DELAY_ms);
            enable_en_fault_irq();
        }
    }

    /**
     * @brief  Disabling the power bridges (forcing the output bridges at HiZ).
     * @param  None.
     * @retval None.
     */
    void STSPIN820_Disable_Power_Bridges(void)
    {
        disable_en_fault_irq();
        //en_fault_irq.write(0);
    }

    /**
     * @brief  Enabling interrupts.
     * @param  None.
     * @retval None.
     */
    void STSPIN820_EnableIrq(void)
    {
        __enable_irq();
    }

    /**
     * @brief  Disabling interrupts.
     * @param  None.
     * @retval None.
     */
    void STSPIN820_DisableIrq(void)
    {
        __disable_irq();
    }

    /**
     * @brief  Getting the state of the EN_FAULT pin.
     * @param  None.
     * @retval None.
     */
    uint32_t STSPIN820_EN_FAULT_PIN_GetState(void)
    {
        return (uint32_t) en_fault_irq;
    }

    /**
     * @brief  Initialising the GPIOs.
     * @param  None.
     * @retval None.
     */
    void STSPIN820_GpioInit(void)
    {
        disable_en_fault_irq();
        enable_en_fault_irq();
        STSPIN820_Reset();
    }

    /**
     * @brief  Initialising the PWM.
     * @param  None.
     * @retval None.
     */
    void STSPIN820_PwmRefInit(void) {}

    /**
     * @brief  Setting the frequency and the duty-cycle of PWM.
     *         The frequency controls directly the speed of the device.
     *         The duty-cycle controls the torque of the device.
     * @param  frequency  The frequency of PWM in Hz.
     * @param  duty_cycle The duty-cycle of PWM in [0..100]%.
     * @retval None.
     */
    void STSPIN820_PwmRefSetFreqAndDutyCycle(uint32_t frequency, uint8_t duty_cycle)
    {
        pwm_period = (double) (1.0f / frequency);
        pwm_duty_cycle = (double) duty_cycle;
    }

    /**
     * @brief  Starting the PWM.
     * @param  None.
     * @retval None.
     */
    void STSPIN820_PwmRefStart(void)
    {
        /* Setting the period and the duty-cycle of PWM. */
        ref_pwm.period(pwm_period);
        ref_pwm.write(pwm_duty_cycle);

        /* Setting a callback with the same period of PWM's, to update the state machine. */
        ticker.attach(Callback<void()>(this, &STSPIN820::STSPIN820_StepClockHandler), pwm_period);
    }

    /**
     * @brief  Stopping the PWM.
     * @param  None.
     * @retval None.
     */
    void STSPIN820_PwmRefStop(void)
    {
        ref_pwm.write(0.0f);
        ticker.detach();
    }

    /**
     * @brief  Putting the device in standby mode.
     * @param  None.
     * @retval None.
     */
    void STSPIN820_ReleaseReset(void)
    {
        standby_reset = 1;
    }

    /**
     * @brief  Putting the device in reset mode.
     * @param  None.
     * @retval None.
     */
    void STSPIN820_Reset(void)
    {
        standby_reset = 0;
    }

    /**
     * @brief  Setting the decay mode pin state.
     * @param  gpio_state State of the decay mode pin ("0" to reset, "1" to set).
     * @retval None.
     */
    void STSPIN820_SetDecayGpio(uint8_t gpio_state)
    {
        decay = gpio_state;
    }

    /**
     * @brief  Getting the decay mode pin state.
     * @param  None.
     * @retval None.
     */
    uint8_t STSPIN820_GetDecayGpio(void)
    {
        return (decay != 0 ? 1 : 0);
    }

    /**
     * @brief  Setting the direction pin state.
     * @param  gpio_state State of the direction pin ("0" to reset, "1" to set).
     * @retval None.
     */
    void STSPIN820_SetDirectionGpio(uint8_t gpio_state)
    {
        direction = gpio_state;
    }

    /**
     * @brief  Setting the full step mode.
     * @param  None.
     * @retval None.
     */
    void STSPIN820_SetFullStep(void)
    {
        mode1 = mode2 = mode3 = 0;
    }

    /**
     * @brief  Selecting the STSPIN820 mode1, mode2 and mode3 pins levels.
     * @param  mode1_level Level of the mode1 gpio (0 low, 1+ high).
     * @param  mode2_level Level of the mode2 gpio (0 low, 1+ high).
     * @param  mode3_level Level of the mode3 gpio (0 low, 1+ high).
     * @retval None.
     */
    void STSPIN820_SetModePins(uint8_t mode1_level, uint8_t mode2_level, uint8_t mode3_level)
    {
        mode1 = (mode1_level != 0 ? 1 : 0);
        mode2 = (mode2_level != 0 ? 1 : 0);
        mode3 = (mode3_level != 0 ? 1 : 0);
    }

    /**
     * @brief  Initializing step clock compare value.
     * @param  None.
     * @retval None.
     */
    void STSPIN820_TimStckCompareInit(void) {}

    /**
     * @brief  Deinitializing the timer used for the step clock.
     * @param  None.
     * @retval None.
     */
    void STSPIN820_TimStckDeInit(void) {}

    /**
     * @brief  Initializing the timer used for the step clock.
     * @param  None.
     * @retval None.
     */
    void STSPIN820_TimStckInit(void) {}

    /**
     * @brief  Setting the frequency of PWM.
     *         The frequency controls directly the speed of the device.
     * @param  frequency  The frequency of PWM in Hz.
     * @retval None.
     */
    void STSPIN820_TimStckSetFreq(uint32_t frequency)
    {
        pwm_period = (double) (1.0f / frequency);
    }

    /**
     * @brief  Starting the step clock.
     * @param  None.
     * @retval None.
     */
    void STSPIN820_TimStckStart(void) {}

    /**
     * @brief  Stopping the step clock.
     * @param  p_toggle_odd Pointer to the volatile toggleOdd variable.
     * @retval "1" if OK, "0" if the STCK pin is High (forbidden configuration).
     */
    uint8_t STSPIN820_TimStckStop(volatile uint8_t *p_toggle_odd )
    {
        __disable_irq();
        if (*p_toggle_odd == 1)
        {
            __enable_irq();
            return 1;
        }
        if (step_clock != 0)
        {
            __enable_irq();
            return 0;
        }
        //HAL_TIM_OC_Stop_IT(&hTimerStepClock, BSP_MOTOR_CONTROL_BOARD_CHAN_TIM_STCK);
        __enable_irq();
        STSPIN820_TimStckDeInit();
        return 1;
    }


    /*** Component's Instance Variables ***/

    /* ACTION 9 --------------------------------------------------------------*
     * Declare here interrupt related variables, if needed.                   *
     * Note that interrupt handling is platform dependent, see                *
     * "Interrupt Related Methods" above.                                     *
     *                                                                        *
     * Example:                                                               *
     *   + mbed:                                                              *
     *     InterruptIn feature_irq;                                           *
     *------------------------------------------------------------------------*/
    /* En Fault Interrupt. */
    InterruptIn en_fault_irq;

    /* ACTION 10 -------------------------------------------------------------*
     * Declare here other pin related variables, if needed.                   *
     *                                                                        *
     * Example:                                                               *
     *   + mbed:                                                              *
     *     DigitalOut standby_reset;                                          *
     *------------------------------------------------------------------------*/
    /* Standby/reset pin. */
    DigitalOut standby_reset;

    /* Direction of rotation pin. */
    DigitalOut direction;

    /* Pulse Width Modulation pin. */
    PwmOut ref_pwm;

    /* Decay pin. */
    DigitalOut decay;
    
    /* Step clock pin. */
    DigitalOut step_clock;
    
    /* Mode1 pin. */
    DigitalOut mode1;
    
    /* Mode2 pin. */
    DigitalOut mode2;
    
    /* Mode3 pin. */
    DigitalOut mode3;

    /* Timer to trigger the PWM callback at each PWM pulse. */
    Ticker ticker;

    /* ACTION 11 -------------------------------------------------------------*
     * Declare here communication related variables, if needed.               *
     *                                                                        *
     * Example:                                                               *
     *   + mbed:                                                              *
     *     DigitalOut ssel;                                                   *
     *     DevSPI &dev_spi;                                                   *
     *------------------------------------------------------------------------*/

    /* ACTION 12 -------------------------------------------------------------*
     * Declare here identity related variables, if needed.                    *
     * Note that there should be only a unique identifier for each component, *
     * which should be the "who_am_i" parameter.                              *
     *------------------------------------------------------------------------*/
    /* Identity */
    uint8_t who_am_i;

    /* ACTION 13 -------------------------------------------------------------*
     * Declare here the component's static and non-static data, one variable  *
     * per line.                                                              *
     *                                                                        *
     * Example:                                                               *
     *   float measure;                                                       *
     *   int instance_id;                                                     *
     *   static int number_of_instances;                                      *
     *------------------------------------------------------------------------*/
    /* Data. */
    void (*flag_interrupt_callback)(void);
    void (*error_handler_callback)(uint16_t error);
    volatile uint8_t toggle_odd;
    device_params_t device_prm;
    double pwm_period;
    double pwm_duty_cycle;
    uint8_t device_instance;
    
    /* Static data. */
    static uint8_t number_of_devices;
};

#endif /* __STSPIN820_CLASS_H */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/