STSPIN820.cpp

00001 /**
00002  *******************************************************************************
00003  * @file    STSPIN820.cpp
00004  * @author  STM
00005  * @version V1.0.0
00006  * @date    August 7th, 2017
00007  * @brief   STSPIN820 driver (fully integrated microstepping motor driver).
00008  * @note    (C) COPYRIGHT 2017 STMicroelectronics
00009  *******************************************************************************
00010  * @attention
00011  *
00012  * <h2><center>&copy; COPYRIGHT(c) 2017 STMicroelectronics</center></h2>
00013  *
00014  * Redistribution and use in source and binary forms, with or without modification,
00015  * are permitted provided that the following conditions are met:
00016  *   1. Redistributions of source code must retain the above copyright notice,
00017  *      this list of conditions and the following disclaimer.
00018  *   2. Redistributions in binary form must reproduce the above copyright notice,
00019  *      this list of conditions and the following disclaimer in the documentation
00020  *      and/or other materials provided with the distribution.
00021  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00022  *      may be used to endorse or promote products derived from this software
00023  *      without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00028  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00029  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00030  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00031  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00033  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00034  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  *******************************************************************************
00037  */
00038 
00039 
00040 /* Generated with STM32CubeTOO -----------------------------------------------*/
00041 
00042 
00043 /* Revision ------------------------------------------------------------------*/
00044 /*
00045     Repository:       http://svn.x-nucleodev.codex.cro.st.com/svnroot/X-NucleoDev
00046     Branch/Trunk/Tag: trunk
00047     Based on:         X-CUBE-SPN14/trunk/Drivers/BSP/Components/STSPIN820/STSPIN820.c
00048     Revision:         0
00049 */
00050 
00051 
00052 /* Includes ------------------------------------------------------------------*/
00053 
00054 #include "STSPIN820.h"
00055 
00056 
00057 /* Variables  ----------------------------------------------------------------*/
00058 
00059 /* Number of devices. */
00060 uint8_t STSPIN820::number_of_devices = 0;
00061 
00062 
00063 /* Private function prototypes -----------------------------------------------*/
00064 
00065 /** @defgroup STSPIN820_Private_Functions STSPIN820 Private Functions
00066   * @{
00067   */  
00068 /* Methods -------------------------------------------------------------------*/
00069 
00070 /** @defgroup STSPIN820_Exported_Variables STSPIN820 Exported Variables
00071   * @{
00072   */
00073 
00074 /** @defgroup STSPIN820_Library_Functions STSPIN820 Library Functions
00075   * @{
00076   */
00077 
00078 /**********************************************************
00079  * @brief Starts the STSPIN820 library
00080  * @param  init Initialization structure.
00081  * @retval COMPONENT_OK in case of success.
00082  **********************************************************/
00083 status_t STSPIN820::STSPIN820_Init(void *init)
00084 {
00085   /* Initialise the GPIOs */
00086   STSPIN820_GpioInit();
00087 
00088   /* Initialise the timer used for the step clock and ------------------------*/
00089   /* the PWM for the reference voltage generation ----------------------------*/
00090   STSPIN820_TimStckInit();
00091   STSPIN820_PwmRefInit();
00092 
00093   if (init == 0)
00094   {
00095     /* Set all context variables to the predefined values */
00096     /* from STSPIN820_config.h */
00097     STSPIN820_SetDeviceParamsToPredefinedValues();
00098   }
00099   else
00100   {
00101     STSPIN820_SetDeviceParamsToGivenValues((STSPIN820_init_t*) init);
00102   }
00103 
00104   return COMPONENT_OK;
00105 }
00106 
00107 /**********************************************************
00108  * @brief Read id
00109  * @param id pointer to the identifier to be read.
00110  * @retval COMPONENT_OK in case of success.
00111  **********************************************************/
00112 status_t STSPIN820::STSPIN820_ReadId(uint8_t *id)
00113 {
00114   *id = device_instance;
00115 
00116   return COMPONENT_OK;
00117 }
00118 
00119 /**********************************************************
00120  * @brief Attach a user callback to the error Handler.
00121  * The call back will be then called each time the library 
00122  * detects an error
00123  * @param[in] callback Name of the callback to attach 
00124  * to the error Hanlder
00125  * @retval None
00126  **********************************************************/
00127 void STSPIN820::STSPIN820_AttachErrorHandler(void (*callback)(uint16_t error))
00128 {
00129   error_handler_callback = (void (*)(uint16_t error))callback;
00130 }
00131 
00132 /**********************************************************
00133  * @brief Attach a user callback to the flag Interrupt
00134  * The call back will be then called each time the status 
00135  * flag pin will be pulled down due to the occurrence of 
00136  * a programmed alarms ( OCD, thermal pre-warning or 
00137  * shutdown, UVLO, wrong command, non-performable command)
00138  * @param[in] callback Name of the callback to attach 
00139  * to the Flag Interrupt
00140  * @retval None
00141  **********************************************************/
00142 void STSPIN820::STSPIN820_AttachFlagInterrupt(void (*callback)(void))
00143 {
00144   flag_interrupt_callback = (void (*)(void))callback;
00145 }
00146 
00147 /**********************************************************
00148  * @brief Check if STSPIN820 has a fault by reading EN pin position.
00149  * @retval One if STSPIN820 has EN pin down, otherwise zero
00150  **********************************************************/
00151 uint8_t STSPIN820::STSPIN820_CheckStatusHw(void)
00152 {
00153   if(!STSPIN820_EN_FAULT_PIN_GetState())
00154   {
00155     return 0x01;
00156   }
00157   else
00158   {
00159     return 0x00;
00160   }
00161 }
00162 
00163 /**********************************************************
00164  * @brief Disable the power bridges (leave the output bridges HiZ)
00165  * @retval None
00166  **********************************************************/
00167 void STSPIN820::STSPIN820_Disable(void)
00168 {
00169   STSPIN820_Disable_Power_Bridges();
00170 }
00171 
00172 /**********************************************************
00173  * @brief Enable the power bridges
00174  * @retval None
00175  **********************************************************/
00176 void STSPIN820::STSPIN820_Enable(void)
00177 {
00178   STSPIN820_Enable_Power_Bridges();
00179 }
00180 
00181 /**********************************************************
00182  * @brief Error handler which calls the user callback (if defined)
00183  * @param[in] error Number of the error
00184  * @retval None
00185  **********************************************************/
00186 void STSPIN820::STSPIN820_ErrorHandler(uint16_t error)
00187 {
00188   if (error_handler_callback != 0)
00189   {
00190     (void) error_handler_callback(error);
00191   }
00192   else   
00193   {
00194     while(1)
00195     {
00196       /* Infinite loop */
00197     }
00198   }
00199 }
00200 
00201 /**********************************************************
00202  * @brief Exit STSPIN820 device from standby (low power consumption)
00203  * @retval None
00204  **********************************************************/
00205 void STSPIN820::STSPIN820_ExitDeviceFromStandby(void)
00206 {
00207   uint32_t sequencerPosition = device_prm.sequencerPosition;
00208   
00209   /* Exit standby and set step mode */
00210   /* Disable step clock */
00211   if (STSPIN820_TimStckStop(&toggle_odd) == 0)
00212   {
00213     STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
00214   }
00215 
00216   /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */
00217   /* after the last step clock rising edge triggering the last step */
00218   STSPIN820_Delay(DISABLE_DELAY);
00219     /* Set reference voltage to 0 */
00220   STSPIN820_SetTorque( CURRENT_TORQUE, 0);
00221 
00222   /* Disable power bridges */
00223   STSPIN820_Disable_Power_Bridges();
00224   
00225   device_prm.commandExecuted = NO_CMD;
00226   device_prm.stepsToTake = 0;  
00227   device_prm.speed = 0;
00228 
00229   /* Reset the microstepping sequencer position */
00230   device_prm.sequencerPosition = 0;
00231 
00232   /* Reset current and mark positions */
00233   device_prm.currentPosition = 0; 
00234   device_prm.markPosition = 0;
00235   
00236   STSPIN820_SetStepMode(device_prm.stepMode);
00237   STSPIN820_ReleaseReset();
00238   STSPIN820_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
00239   
00240   STSPIN820_SetHome();
00241   STSPIN820_SetMark();
00242   
00243   if (device_prm.sequencerPosition != 0)
00244   {
00245     /* Set direction to FORWARD to ensure the HW sequencer is increased at */
00246     /* each step clock rising edge */
00247     STSPIN820_SetDirection(FORWARD);
00248     /* Going out of standby */
00249     device_prm.motionState = STANDBYTOINACTIVE;
00250     /* Initialize the step clock timer */
00251     STSPIN820_TimStckInit();
00252     /* Program the step clock */    
00253     STSPIN820_TimStckCompareInit();
00254     STSPIN820_TimStckSetFreq(STSPIN820_MAX_STCK_FREQ);
00255     toggle_odd = 0;
00256     STSPIN820_TimStckStart();
00257     while (device_prm.sequencerPosition != 0);
00258     while (toggle_odd!=0);
00259     device_prm.sequencerPosition = sequencerPosition;    
00260   }
00261   
00262   device_prm.motionState = INACTIVE;
00263 }
00264 
00265 /**********************************************************
00266  * @brief Return the acceleration of the specified device
00267  * @retval Acceleration in pps^2
00268  **********************************************************/
00269 uint16_t STSPIN820::STSPIN820_GetAcceleration(void)
00270 {
00271   return (device_prm.acceleration);
00272 }            
00273 
00274 /**********************************************************
00275  * @brief Return the current speed of the specified device
00276  * @retval Speed in pps
00277  **********************************************************/
00278 uint16_t STSPIN820::STSPIN820_GetCurrentSpeed(void)
00279 {
00280   return device_prm.speed;
00281 }
00282 
00283 /**********************************************************
00284  * @brief Return the decay mode of the specified device
00285  * @retval Decay Mode State (SLOW or MIXED)
00286  **********************************************************/
00287 motor_decay_mode_t STSPIN820::STSPIN820_GetDecayMode(void)
00288 {
00289   motor_decay_mode_t status;
00290   if (STSPIN820_GetDecayGpio() != 0) 
00291   {
00292     status = SLOW_DECAY;
00293   }
00294   else
00295   {
00296     status = MIXED_DECAY;
00297   }
00298   
00299   return status;
00300 }
00301 
00302 /**********************************************************
00303  * @brief Return the deceleration of the specified device
00304  * @retval Deceleration in pps^2
00305  **********************************************************/
00306 uint16_t STSPIN820::STSPIN820_GetDeceleration(void)
00307 {
00308   return (device_prm.deceleration);
00309 }          
00310 
00311 /**********************************************************
00312  * @brief Return the device state
00313  * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE)
00314  **********************************************************/
00315 motor_state_t STSPIN820::STSPIN820_GetDeviceState(void)
00316 {
00317   return device_prm.motionState;
00318 }
00319 
00320 /**********************************************************
00321  * @brief Get the motor current direction
00322  * @retval direction
00323  **********************************************************/
00324 motor_direction_t STSPIN820::STSPIN820_GetDirection(void)
00325 {
00326   return device_prm.direction;
00327 }
00328 
00329 /**********************************************************
00330  * @brief Return the FW version of the library
00331  * @retval Stspin220_FW_VERSION
00332  **********************************************************/
00333 uint32_t STSPIN820::STSPIN820_GetFwVersion(void)
00334 {
00335   return (STSPIN820_FW_VERSION);
00336 }
00337 
00338 /**********************************************************
00339  * @brief Return the mark position of the specified device
00340  * @retval mark position value
00341  **********************************************************/
00342 int32_t STSPIN820::STSPIN820_GetMark(void)
00343 {
00344   return device_prm.markPosition;
00345 }
00346 
00347 /**********************************************************
00348  * @brief Return the max speed of the specified device
00349  * @retval maxSpeed in pps
00350  **********************************************************/
00351 uint16_t STSPIN820::STSPIN820_GetMaxSpeed(void)
00352 {
00353   return (device_prm.maxSpeed);
00354 }
00355 
00356 /**********************************************************
00357  * @brief Return the min speed of the specified device
00358  * @retval minSpeed in pps
00359  **********************************************************/
00360 uint16_t STSPIN820::STSPIN820_GetMinSpeed(void)
00361 {
00362   return (device_prm.minSpeed);
00363 }                                                     
00364 
00365 /**********************************************************
00366  * @brief  Returns the number of devices
00367  * @retval number of devices
00368  **********************************************************/
00369 uint8_t STSPIN820::STSPIN820_GetNbDevices(void)
00370 {
00371   return (number_of_devices);
00372 }
00373 
00374 /**********************************************************
00375  * @brief Return the current position value of the specified device
00376  * @retval current position value
00377  **********************************************************/
00378 int32_t STSPIN820::STSPIN820_GetPosition(void)
00379 {
00380   return device_prm.currentPosition;
00381 }
00382 
00383 /**********************************************************
00384  * @brief Get the motor step mode
00385  * @retval step mode
00386  **********************************************************/
00387 motor_step_mode_t STSPIN820::STSPIN820_GetStepMode(void)
00388 {
00389   return device_prm.stepMode;
00390 }
00391 
00392 /**********************************************************
00393  * @brief Get the selected stop mode
00394  * @retval the selected stop mode
00395  **********************************************************/
00396 motor_stop_mode_t STSPIN820::STSPIN820_GetStopMode(void)
00397 {
00398   return device_prm.stopMode;
00399 }
00400 
00401 /**********************************************************
00402  * @brief Get the torque of the specified device
00403  * @param[in] torqueMode torque mode
00404  * @retval the torqueValue in % (from 0 to 100)
00405  * @note
00406  **********************************************************/
00407 uint8_t STSPIN820::STSPIN820_GetTorque(motor_torque_mode_t torqueMode)
00408 {
00409   uint8_t torqueValue = 0;
00410   switch(torqueMode)
00411   {
00412     case ACC_TORQUE:
00413       torqueValue = device_prm.accelTorque;
00414       break;
00415     case DEC_TORQUE:
00416       torqueValue = device_prm.decelTorque;
00417       break;
00418     case RUN_TORQUE:
00419       torqueValue = device_prm.runTorque;
00420       break;
00421     case HOLD_TORQUE:
00422       torqueValue = device_prm.holdTorque;
00423       break;
00424     case CURRENT_TORQUE:
00425       torqueValue = device_prm.currentTorque;
00426       break;
00427     default:
00428       break;
00429   }
00430   return torqueValue;
00431 }
00432 
00433 /**********************************************************
00434  * @brief Get the torque boost feature status
00435  * @retval TRUE if enabled, FALSE if disabled
00436  **********************************************************/
00437 bool STSPIN820::STSPIN820_GetTorqueBoostEnable(void)
00438 {
00439   return device_prm.torqueBoostEnable;
00440 }
00441 
00442 /**********************************************************
00443  * @brief Get the torque boost threshold
00444  * @param[in]  (from 0 to MAX_NUMBER_OF_DEVICES - 1)
00445  * @retval the torque boost threshold above which the step mode is
00446  * changed to full step
00447  **********************************************************/
00448 uint16_t STSPIN820::STSPIN820_GetTorqueBoostThreshold(void)
00449 {
00450   return device_prm.torqueBoostSpeedThreshold;
00451 }
00452 
00453 /**********************************************************
00454  * @brief Request the motor to move to the home position (ABS_POSITION = 0)
00455  * @retval None
00456  **********************************************************/
00457 void STSPIN820::STSPIN820_GoHome(void)
00458 {
00459   STSPIN820_GoTo(0);
00460 } 
00461   
00462 /**********************************************************
00463  * @brief Request the motor to move to the mark position 
00464  * @retval None
00465  **********************************************************/
00466 void STSPIN820::STSPIN820_GoMark(void)
00467 {
00468     STSPIN820_GoTo(device_prm.markPosition);
00469 }
00470 
00471 /**********************************************************
00472  * @brief Request the motor to move to the specified position 
00473  * @param[in] targetPosition absolute position in steps
00474  * @retval None 
00475  * @note The position is at the resolution corresponding to the
00476  * selected step mode.
00477  * STEP_MODE_FULL                   : Full step
00478  * STEP_MODE_HALF                   : 1/2 step
00479  * STEP_MODE_1_4                    : 1/4 step
00480  * STEP_MODE_1_8                    : 1/8 step
00481  * STEP_MODE_1_16                   : 1/16 step
00482  * STEP_MODE_1_32                   : 1/32 step
00483  * STEP_MODE_1_128                  : 1/128 step
00484  * STEP_MODE_1_256                  : 1/256 step
00485  * @note The 1/64 step mode is not allowed
00486  **********************************************************/
00487 void STSPIN820::STSPIN820_GoTo(int32_t targetPosition)
00488 {
00489   motor_direction_t direction;
00490   
00491   /* Exit from standby if needed */
00492   if (device_prm.motionState == STANDBY)
00493   {
00494     STSPIN820_ExitDeviceFromStandby();
00495   }
00496   /* Deactivate motor if needed */
00497   else if (device_prm.motionState != INACTIVE)
00498   { 
00499     STSPIN820_HardHiZ();
00500   }
00501   
00502   if (targetPosition > device_prm.currentPosition)
00503   {
00504     device_prm.stepsToTake = targetPosition -\
00505                                       device_prm.currentPosition;
00506     if (device_prm.stepsToTake < (STSPIN820_POSITION_RANGE>>1))
00507     {
00508       direction = FORWARD;
00509     }
00510     else
00511     {
00512       direction = BACKWARD;
00513       device_prm.stepsToTake = STSPIN820_POSITION_RANGE -\
00514                                         device_prm.stepsToTake;
00515     }
00516   }
00517   else
00518   {
00519     device_prm.stepsToTake = device_prm.currentPosition -\
00520                                       targetPosition;
00521     if (device_prm.stepsToTake < (STSPIN820_POSITION_RANGE>>1))
00522     {
00523       direction = BACKWARD;
00524     }
00525     else
00526     {
00527       direction = FORWARD; 
00528       device_prm.stepsToTake = STSPIN820_POSITION_RANGE -\
00529                                         device_prm.stepsToTake;
00530     }
00531   }
00532   
00533   if (device_prm.stepsToTake != 0) 
00534   {
00535     device_prm.commandExecuted = MOVE_CMD;
00536     
00537     /* Direction setup */
00538     STSPIN820_SetDirection(direction);
00539     
00540     STSPIN820_ComputeSpeedProfile(device_prm.stepsToTake);
00541     
00542     /* Motor activation */
00543     STSPIN820_StartMovement();
00544   }
00545 }
00546 
00547 /**********************************************************
00548  * @brief Move the motor to the absolute position
00549  * @param[in] direction FORWARD or BACKWARD
00550  * @param[in] targetPosition 32 bit signed value position
00551  * @retval None
00552  * @note The position is at the resolution corresponding to the
00553  * selected step mode.
00554  * STEP_MODE_FULL                   : step
00555  * STEP_MODE_HALF                   : 1/2 step
00556  * STEP_MODE_1_4                    : 1/4 step
00557  * STEP_MODE_1_8                    : 1/8 step
00558  * STEP_MODE_1_16                   : 1/16 step
00559  * STEP_MODE_1_32                   : 1/32 step
00560  * STEP_MODE_1_128                  : 1/128 step
00561  * STEP_MODE_1_256                  : 1/256 step
00562  * @note The 1/64 step mode is not allowed
00563  **********************************************************/
00564 void STSPIN820::STSPIN820_GoToDir(motor_direction_t direction, int32_t targetPosition)
00565 {
00566   /* Exit from standby if needed */
00567   if (device_prm.motionState == STANDBY)
00568   {
00569     STSPIN820_ExitDeviceFromStandby();
00570   }
00571   /* Deactivate motor if needed */
00572   else if (device_prm.motionState != INACTIVE)
00573   { 
00574     STSPIN820_HardHiZ();
00575   }
00576   
00577   if (direction != BACKWARD)
00578   {
00579     if (targetPosition > device_prm.currentPosition)
00580     {
00581       device_prm.stepsToTake = targetPosition -\
00582                                         device_prm.currentPosition;
00583     }
00584     else
00585     {
00586       device_prm.stepsToTake = STSPIN820_POSITION_RANGE +\
00587                                        (targetPosition -\
00588                                         device_prm.currentPosition);
00589     }
00590   }
00591   else
00592   {
00593     if (targetPosition > device_prm.currentPosition)
00594     {
00595       device_prm.stepsToTake = STSPIN820_POSITION_RANGE +\
00596                                         (device_prm.currentPosition -\
00597                                          targetPosition);
00598     }
00599     else
00600     {
00601       device_prm.stepsToTake = device_prm.currentPosition -\
00602                                         targetPosition;
00603     }
00604   }
00605 
00606   if (device_prm.stepsToTake != 0) 
00607   {
00608     device_prm.commandExecuted = MOVE_CMD;
00609     
00610     /* Direction setup */
00611     STSPIN820_SetDirection(direction);
00612     
00613     STSPIN820_ComputeSpeedProfile(device_prm.stepsToTake);
00614     
00615     /* Motor activation */
00616     STSPIN820_StartMovement();
00617   }  
00618 }
00619 
00620 /**********************************************************
00621  * @brief  Immediatly stop the motor and disable the power bridge
00622  * @retval None
00623  **********************************************************/
00624 void STSPIN820::STSPIN820_HardHiZ(void) 
00625 {
00626   /* Set inactive state */
00627   device_prm.motionState = INACTIVE;
00628   
00629   /* Disable step clock */
00630   if (STSPIN820_TimStckStop(&toggle_odd) == 0)
00631   {
00632     STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
00633   }
00634 
00635   /* Let the PWM REF and bridges enabled at least for DISABLE_DELAY time */
00636   /* after the last step clock rising edge triggering the last step */
00637   STSPIN820_Delay(DISABLE_DELAY);
00638   
00639   /* Set reference voltage to 0 */
00640   STSPIN820_SetTorque(CURRENT_TORQUE, 0);
00641 
00642   /* Disable power bridges */
00643   STSPIN820_Disable_Power_Bridges();
00644   
00645   /* Comeback to nominal step mode */
00646   if (device_prm.stepModeLatched != device_prm.stepMode)
00647   {
00648     motor_step_mode_t StepMode = device_prm.stepModeLatched;
00649     STSPIN820_SetStepMode(StepMode);
00650     device_prm.stepMode = device_prm.stepModeLatched;
00651   }
00652 
00653   device_prm.commandExecuted = NO_CMD;
00654   device_prm.stepsToTake = 0;  
00655   device_prm.speed = 0;
00656 }
00657 
00658 /**********************************************************
00659  * @brief  Immediatly stop the motor
00660  * and either set holding torque when stop mode is HOLD_MODE,
00661  * or call STSPIN820_HardHiz function when stop mode is HIZ_MODE,
00662  * or call STSPIN820_PutDeviceInStandby function when stop mode is STANDBY_MODE
00663  * @retval None
00664  **********************************************************/
00665 void STSPIN820::STSPIN820_HardStop(void) 
00666 {
00667   if (device_prm.stopMode == HOLD_MODE)
00668   {
00669     /* Set inactive state */
00670     device_prm.motionState = INACTIVE;
00671 
00672     /* Disable step clock */
00673     if (STSPIN820_TimStckStop(&toggle_odd) == 0)
00674     {
00675       STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
00676     }
00677     
00678     /* Set holding torque */
00679     STSPIN820_ApplyTorque(HOLD_TORQUE);
00680  
00681     /* Comeback to nominal step mode */
00682     if (device_prm.stepModeLatched != device_prm.stepMode)
00683     {
00684       motor_step_mode_t StepMode = device_prm.stepModeLatched;
00685       STSPIN820_SetStepMode(StepMode);
00686       device_prm.stepMode = device_prm.stepModeLatched;
00687     }    
00688     
00689     device_prm.commandExecuted = NO_CMD;
00690     device_prm.stepsToTake = 0;  
00691     device_prm.speed = 0;
00692   }
00693   else if (device_prm.stopMode == HIZ_MODE)
00694   {
00695     STSPIN820_HardHiZ();
00696   }
00697   else if (device_prm.stopMode == STANDBY_MODE)
00698   {
00699     STSPIN820_PutDeviceInStandby();
00700   }
00701 }
00702 
00703 /**********************************************************
00704  * @brief  Moves the motor of the specified number of steps
00705  * @param[in] direction FORWARD or BACKWARD
00706  * @param[in] stepCount Number of steps to perform
00707  * @retval None
00708  **********************************************************/
00709 void STSPIN820::STSPIN820_Move(motor_direction_t direction, uint32_t stepCount)
00710 {
00711   /* Exit from standby if needed */
00712   if (device_prm.motionState == STANDBY)
00713   {
00714     STSPIN820_ExitDeviceFromStandby();
00715   }
00716   /* Deactivate motor if needed */
00717   else if (device_prm.motionState != INACTIVE)
00718   {
00719     STSPIN820_HardHiZ();
00720   }
00721   
00722   if (stepCount != 0) 
00723   {
00724     device_prm.stepsToTake = stepCount;    
00725     device_prm.commandExecuted = MOVE_CMD;
00726     
00727     /* Direction setup */
00728     STSPIN820_SetDirection(direction);
00729     
00730     STSPIN820_ComputeSpeedProfile(stepCount);
00731     
00732     /* Motor activation */
00733     STSPIN820_StartMovement();
00734   }  
00735 }
00736 
00737 /**********************************************************
00738  * @brief Put STSPIN820 device in standby (low power consumption)
00739  * @retval None
00740  **********************************************************/
00741 void STSPIN820::STSPIN820_PutDeviceInStandby(void)
00742 {
00743   /* Stop movement */
00744   STSPIN820_HardHiZ();
00745   
00746   /* Enter standby */
00747     STSPIN820_Reset();
00748   
00749   device_prm.motionState = STANDBY;
00750 }
00751 
00752 /**********************************************************
00753  * @brief  Runs the motor. It will accelerate from the min 
00754  * speed up to the max speed by using the device acceleration.
00755  * @param[in] direction FORWARD or BACKWARD
00756  * @retval None
00757  **********************************************************/
00758 void STSPIN820::STSPIN820_Run(motor_direction_t direction)
00759 {
00760   /* Exit from standby if needed */
00761   if (device_prm.motionState == STANDBY)
00762   {
00763     STSPIN820_ExitDeviceFromStandby();
00764   }
00765   /* Deactivate motor if needed */
00766   else if (device_prm.motionState != INACTIVE)
00767   {
00768     STSPIN820_HardHiZ();
00769   }
00770   
00771     /* Direction setup */
00772     STSPIN820_SetDirection(direction);
00773     device_prm.commandExecuted = RUN_CMD;
00774     /* Motor activation */
00775     STSPIN820_StartMovement(); 
00776 }
00777 
00778 /**********************************************************
00779  * @brief  Changes the acceleration of the specified device
00780  * @param[in] newAcc New acceleration to apply in pps^2
00781  * @retval true if the command is successfully executed, else false
00782  * @note The command is not performed if the device is executing 
00783  * a MOVE or GOTO command (but it can be used during a RUN command)
00784  **********************************************************/
00785 bool STSPIN820::STSPIN820_SetAcceleration(uint16_t newAcc)
00786 {
00787   bool cmdExecuted = FALSE;
00788   if ((newAcc != 0)&&
00789       (((device_prm.motionState & INACTIVE) == INACTIVE)||
00790        (device_prm.commandExecuted == RUN_CMD)))
00791   {
00792     device_prm.acceleration = newAcc;
00793     cmdExecuted = TRUE;
00794   }    
00795   return cmdExecuted;
00796 }            
00797 
00798 /**********************************************************
00799  * @brief  Specifies the decay mode 
00800  * @param[in] decay SLOW_DECAY or MIXED_DECAY
00801  * @retval true if the command is successfully executed, else false
00802  **********************************************************/
00803 void STSPIN820::STSPIN820_SetDecayMode(motor_decay_mode_t decay)
00804 { 
00805   if (decay == SLOW_DECAY)
00806   {
00807     STSPIN820_SetDecayGpio(1);
00808   }
00809   else if (decay == MIXED_DECAY)
00810   {
00811     STSPIN820_SetDecayGpio(0);
00812   }
00813 }
00814 
00815 /**********************************************************
00816  * @brief  Changes the deceleration of the specified device
00817  * @param[in] newDec New deceleration to apply in pps^2
00818  * @retval true if the command is successfully executed, else false
00819  * @note The command is not performed if the device is executing 
00820  * a MOVE or GOTO command (but it can be used during a RUN command)
00821  **********************************************************/
00822 bool STSPIN820::STSPIN820_SetDeceleration(uint16_t newDec)
00823 {
00824   bool cmdExecuted = FALSE;
00825   if ((newDec != 0)&& 
00826       (((device_prm.motionState & INACTIVE) == INACTIVE)||
00827        (device_prm.commandExecuted == RUN_CMD)))
00828   {
00829     device_prm.deceleration = newDec;
00830     cmdExecuted = TRUE;
00831   }      
00832   return cmdExecuted;
00833 }
00834 
00835 /**********************************************************
00836  * @brief  Specifies the direction 
00837  * @param[in] dir FORWARD or BACKWARD
00838  * @note The direction change is applied if the device 
00839  * is in INACTIVE or STANDBY state or if the device is 
00840  * executing a run command
00841  * @retval None
00842  **********************************************************/
00843 void STSPIN820::STSPIN820_SetDirection(motor_direction_t dir)
00844 {
00845   if ((device_prm.motionState == INACTIVE)||\
00846       (device_prm.motionState == STANDBY))
00847   {
00848     device_prm.direction = dir;
00849     STSPIN820_SetDirectionGpio(dir);
00850   }
00851   else if ((device_prm.commandExecuted&RUN_CMD)!=0)
00852   {
00853     device_prm.commandExecuted = (device_command_t) (device_prm.commandExecuted | STSPIN820_DIR_CHANGE_BIT_MASK);
00854   }
00855 }
00856 
00857 /**********************************************************
00858  * @brief  Set current position to be the Home position
00859  * (current position set to 0)
00860  * @retval None
00861  **********************************************************/
00862 void STSPIN820::STSPIN820_SetHome(void)
00863 {
00864   device_prm.currentPosition = 0;
00865 }
00866  
00867 /**********************************************************
00868  * @brief  Set current position to be the Mark position 
00869  * @retval None
00870  **********************************************************/
00871 void STSPIN820::STSPIN820_SetMark(void)
00872 {
00873   device_prm.markPosition = device_prm.currentPosition;
00874 }
00875 
00876 /**********************************************************
00877  * @brief  Changes the max speed of the specified device
00878  * @param[in] newMaxSpeed New max speed  to apply in pps
00879  * @retval true if the command is successfully executed, else false
00880  * @note The command is not performed is the device is executing 
00881  * a MOVE or GOTO command (but it can be used during a RUN command).
00882  **********************************************************/
00883 bool STSPIN820::STSPIN820_SetMaxSpeed(uint16_t newMaxSpeed)
00884 {
00885   bool cmdExecuted = FALSE;
00886   if ((newMaxSpeed >= STSPIN820_MIN_STCK_FREQ)&&\
00887       ((newMaxSpeed <= STSPIN820_MAX_STCK_FREQ)||\
00888        ((device_prm.torqueBoostEnable != FALSE)&&\
00889         ((newMaxSpeed>>STSPIN820_GetStepMode())<= STSPIN820_MAX_STCK_FREQ)))&&\
00890       (device_prm.minSpeed <= newMaxSpeed) &&\
00891       (((device_prm.motionState & INACTIVE) == INACTIVE)||\
00892       (device_prm.commandExecuted == RUN_CMD)))
00893   {
00894     device_prm.maxSpeed = newMaxSpeed;
00895     cmdExecuted = TRUE;
00896   }
00897   return cmdExecuted;
00898 }                                                     
00899 
00900 /**********************************************************
00901  * @brief  Changes the min speed of the specified device
00902  * @param[in] newMinSpeed New min speed  to apply in pps
00903  * @retval true if the command is successfully executed, else false
00904  * @note The command is not performed is the device is executing 
00905  * a MOVE or GOTO command (but it can be used during a RUN command).
00906  **********************************************************/
00907 bool STSPIN820::STSPIN820_SetMinSpeed(uint16_t newMinSpeed)
00908 {
00909   bool cmdExecuted = FALSE;
00910   if ((newMinSpeed >= STSPIN820_MIN_STCK_FREQ)&&
00911       (newMinSpeed <= STSPIN820_MAX_STCK_FREQ) &&
00912       (newMinSpeed <= device_prm.maxSpeed) && 
00913       (((device_prm.motionState & INACTIVE) == INACTIVE)||
00914        (device_prm.commandExecuted == RUN_CMD)))
00915   {
00916     device_prm.minSpeed = newMinSpeed;
00917     cmdExecuted = TRUE;
00918   }  
00919   return cmdExecuted;
00920 }
00921 
00922 /**********************************************************
00923  * @brief  Sets the number of devices to be used 
00924  * @param[in] nbDevices (from 1 to MAX_NUMBER_OF_DEVICES)
00925  * @retval TRUE if successfull, FALSE if failure, attempt to set a number of 
00926  * devices greater than MAX_NUMBER_OF_DEVICES
00927  **********************************************************/
00928 bool STSPIN820::STSPIN820_SetNbDevices(uint8_t nbDevices)
00929 {
00930   if (nbDevices <= MAX_NUMBER_OF_DEVICES)
00931   {
00932     number_of_devices = nbDevices;
00933     return TRUE;
00934   }
00935   else
00936   {
00937     return FALSE;
00938   }
00939 }
00940 
00941 /**********************************************************
00942  * @brief Set the stepping mode 
00943  * @param[in] stepMode from full step to 1/256 microstep
00944  * as specified in enum motor_step_mode_t
00945  * 1/64 microstep mode not supported by STSPIN820
00946  * @retval true if the command is successfully executed, else false
00947  **********************************************************/
00948 bool STSPIN820::STSPIN820_SetStepMode(motor_step_mode_t stepMode)
00949 {
00950   /* Store step mode */
00951   device_prm.stepMode = stepMode;
00952   device_prm.stepModeLatched = stepMode;
00953   
00954   /* Set the mode pins to the levels corresponding to the selected step mode */
00955   switch (stepMode)
00956   {
00957     case STEP_MODE_FULL:
00958       STSPIN820_SetFullStep();
00959       break;
00960     case STEP_MODE_HALF:
00961       STSPIN820_SetModePins(1, 0, 0);
00962       break;    
00963     case STEP_MODE_1_4:
00964       STSPIN820_SetModePins(0, 1, 0);
00965       break;        
00966     case STEP_MODE_1_8:
00967       STSPIN820_SetModePins(1, 1, 0);
00968       break;
00969     case STEP_MODE_1_16:
00970       STSPIN820_SetModePins(0, 0, 1);
00971       break;   
00972     case STEP_MODE_1_32:
00973       STSPIN820_SetModePins(1, 0, 1);
00974       break;   
00975     case STEP_MODE_1_128:
00976       STSPIN820_SetModePins(0, 1, 1);
00977       break;  
00978     case STEP_MODE_1_256:
00979       STSPIN820_SetModePins(1, 1, 1);
00980       break;
00981     default:
00982       return FALSE;
00983   }
00984 
00985   return TRUE;
00986   
00987 }
00988 
00989 /**********************************************************
00990  * @brief Select the mode to stop the motor.
00991  * @param[in] stopMode HOLD_MODE to let power bridge enabled
00992  * @retval None
00993  **********************************************************/
00994 void STSPIN820::STSPIN820_SetStopMode(motor_stop_mode_t stopMode)
00995 {
00996   device_prm.stopMode = stopMode;
00997 }
00998 
00999 /**********************************************************
01000  * @brief Set the torque of the specified device
01001  * @param[in] torqueMode Torque mode as specified in enum motor_torque_mode_t
01002  * @param[in] torqueValue in % (from 0 to 100)
01003  * @retval None
01004  * @note
01005  **********************************************************/
01006 void STSPIN820::STSPIN820_SetTorque(motor_torque_mode_t torqueMode, uint8_t torqueValue)
01007 {
01008   device_prm.updateTorque = TRUE;
01009   if (torqueValue>100)
01010   {
01011     torqueValue = 100;
01012   }
01013   switch(torqueMode)
01014   {
01015     case ACC_TORQUE:
01016       device_prm.accelTorque = torqueValue;
01017       break;
01018     case DEC_TORQUE:
01019       device_prm.decelTorque = torqueValue;
01020       break;
01021     case RUN_TORQUE:
01022       device_prm.runTorque = torqueValue;
01023       break;
01024     case HOLD_TORQUE:
01025       device_prm.holdTorque = torqueValue;
01026       if (device_prm.motionState != INACTIVE)
01027       {
01028         break;
01029       }
01030     case CURRENT_TORQUE:
01031       device_prm.currentTorque = torqueValue;
01032       STSPIN820_PwmRefSetFreqAndDutyCycle(device_prm.refPwmFreq,torqueValue);
01033     default:
01034       device_prm.updateTorque = FALSE;
01035       break; //ignore error
01036   }
01037 }
01038 
01039 /**********************************************************
01040  * @brief Enable or disable the torque boost feature
01041  * @param[in] enable true to enable torque boost, false to disable
01042  * @retval None
01043  **********************************************************/
01044 void STSPIN820::STSPIN820_SetTorqueBoostEnable(bool enable)
01045 {
01046   device_prm.torqueBoostEnable = enable;
01047 }
01048 
01049 /**********************************************************
01050  * @brief Set the torque boost threshold
01051  * @param[in]  (from 0 to MAX_NUMBER_OF_DEVICES - 1)
01052  * @param[in] speedThreshold speed threshold above which the step mode is
01053  * changed to full step
01054  * @retval None
01055  **********************************************************/
01056 void STSPIN820::STSPIN820_SetTorqueBoostThreshold(uint16_t speedThreshold)
01057 {
01058   device_prm.torqueBoostSpeedThreshold = speedThreshold;
01059 }
01060 
01061 /**********************************************************
01062  * @brief  Stops the motor by using the device deceleration
01063  * @retval true if the command is successfully executed, else false
01064  * @note The command is not performed if the device is in INACTIVE,
01065  * STANDBYTOINACTIVE or STANDBY state.
01066  **********************************************************/
01067 bool STSPIN820::STSPIN820_SoftStop(void)
01068 {
01069   bool cmdExecuted = FALSE;
01070   if ((device_prm.motionState & INACTIVE) != INACTIVE)
01071   {
01072     device_prm.commandExecuted = (device_command_t) (device_prm.commandExecuted | STSPIN820_SOFT_STOP_BIT_MASK);
01073     cmdExecuted = TRUE;
01074   }
01075   return (cmdExecuted);
01076 }
01077 
01078 /**********************************************************
01079  * @brief Get the frequency of REF PWM of the specified device
01080  * @retval the frequency of REF PWM in Hz
01081  * @note
01082  **********************************************************/
01083 uint32_t STSPIN820::STSPIN820_VrefPwmGetFreq(void)
01084 {
01085   return device_prm.refPwmFreq;
01086 }
01087 
01088 /**********************************************************
01089  * @brief Set the frequency of REF PWM of the specified device
01090  * @param[in] newFreq in Hz
01091  * @retval None
01092  * @note
01093  **********************************************************/
01094 void STSPIN820::STSPIN820_VrefPwmSetFreq(uint32_t newFreq)
01095 {
01096   device_prm.refPwmFreq = newFreq;
01097   STSPIN820_PwmRefSetFreqAndDutyCycle(newFreq,device_prm.currentTorque);
01098 }
01099 
01100 /**********************************************************
01101  * @brief  Locks until the device state becomes Inactive
01102  * @retval None
01103  **********************************************************/
01104 void STSPIN820::STSPIN820_WaitWhileActive(void)
01105 {
01106   /* Wait while motor is running */
01107   while (((STSPIN820_GetDeviceState()&INACTIVE)!=INACTIVE)||\
01108    (((STSPIN820_GetDeviceState()&INACTIVE)==INACTIVE)&&(toggle_odd!=0)));
01109 }
01110 
01111 /**********************************************************
01112  * @brief Updates the current speed of the device
01113  * @param[in] newSpeed in pps
01114  * @retval None
01115  **********************************************************/
01116 void STSPIN820::STSPIN820_ApplySpeed(uint16_t newSpeed)
01117 {
01118   if (device_prm.torqueBoostEnable != FALSE)
01119   {
01120     if (device_prm.stepMode > STEP_MODE_1_256)
01121     {
01122       STSPIN820_ErrorHandler(STSPIN820_ERROR_APPLY_SPEED);
01123     }
01124     if (device_prm.stepMode != STEP_MODE_FULL)
01125     {
01126       if (((newSpeed>>device_prm.stepModeLatched)>\
01127            device_prm.torqueBoostSpeedThreshold)&&\
01128           (((device_prm.commandExecuted & STSPIN820_MOVE_BIT_MASK) != MOVE_CMD) ||\
01129            ((device_prm.stepsToTake-device_prm.relativePos)>=\
01130             (1<<device_prm.stepModeLatched))))
01131       {         
01132         if ((device_prm.sequencerPosition & 0xFF) == 0x80)
01133        {
01134           STSPIN820_SetFullStep();
01135           device_prm.stepMode = (motor_step_mode_t) STEP_MODE_FULL;
01136           device_prm.accu >>= device_prm.stepModeLatched;
01137           newSpeed >>= device_prm.stepModeLatched;
01138        }
01139       }
01140     }
01141     else if (((newSpeed <= device_prm.torqueBoostSpeedThreshold) &&\
01142               (device_prm.stepModeLatched != STEP_MODE_FULL))||\
01143              (((device_prm.commandExecuted & STSPIN820_MOVE_BIT_MASK) == MOVE_CMD)&&\
01144                ((device_prm.stepsToTake-device_prm.relativePos)<=\
01145                 (1<<device_prm.stepModeLatched))))
01146     {
01147       STSPIN820_SetStepMode(device_prm.stepModeLatched);
01148       device_prm.stepMode = device_prm.stepModeLatched;
01149       device_prm.accu <<= device_prm.stepModeLatched;
01150       newSpeed <<= device_prm.stepModeLatched;
01151     }
01152   }
01153   else if (device_prm.stepMode != device_prm.stepModeLatched)
01154   {
01155     //torqueBoostEnable has just been disabled
01156     STSPIN820_SetStepMode(device_prm.stepModeLatched);
01157     device_prm.stepMode = device_prm.stepModeLatched;
01158     device_prm.accu <<= device_prm.stepModeLatched;
01159     newSpeed <<= device_prm.stepModeLatched;
01160   }
01161   
01162   if (newSpeed < STSPIN820_MIN_STCK_FREQ)
01163   {
01164     newSpeed = STSPIN820_MIN_STCK_FREQ;  
01165   }
01166   if (newSpeed > STSPIN820_MAX_STCK_FREQ)
01167   {
01168     newSpeed = STSPIN820_MAX_STCK_FREQ;
01169   }
01170   
01171   device_prm.speed = newSpeed;
01172   STSPIN820_TimStckSetFreq(newSpeed);
01173 
01174 }
01175 
01176 /**********************************************************
01177  * @brief Apply the set torque to the specified device
01178  * @param[in] torqueMode torque mode
01179  * @retval None
01180  * @note
01181  **********************************************************/
01182 void STSPIN820::STSPIN820_ApplyTorque(motor_torque_mode_t torqueMode)
01183 {
01184   uint8_t torqueValue = 0;
01185   device_prm.updateTorque = FALSE;
01186   switch(torqueMode)
01187   {
01188     case ACC_TORQUE:
01189       device_prm.currentTorque = device_prm.accelTorque;
01190       break;
01191     case DEC_TORQUE:
01192       device_prm.currentTorque = device_prm.decelTorque;
01193       break;
01194     case RUN_TORQUE:
01195       device_prm.currentTorque = device_prm.runTorque;
01196       break;
01197     case HOLD_TORQUE:
01198       device_prm.currentTorque = device_prm.holdTorque;
01199       break;
01200     case CURRENT_TORQUE:
01201       break;
01202     default:
01203       return; //ignore error
01204   }
01205   torqueValue = device_prm.currentTorque;
01206   STSPIN820_PwmRefSetFreqAndDutyCycle(device_prm.refPwmFreq,torqueValue);
01207 }
01208 
01209 /**********************************************************
01210  * @brief  Computes the speed profile according to the number of steps to move
01211  * @param[in] nbSteps number of steps to perform
01212  * @retval None
01213  * @note Using the acceleration and deceleration of the device,
01214  * this function determines the duration in steps of the acceleration,
01215  * steady and deceleration phases.
01216  * If the total number of steps to perform is big enough, a trapezoidal move
01217  * is performed (i.e. there is a steady phase where the motor runs at the maximum
01218  * speed.
01219  * Else, a triangular move is performed (no steady phase: the maximum speed is never
01220  * reached.
01221  **********************************************************/
01222 void STSPIN820::STSPIN820_ComputeSpeedProfile(uint32_t nbSteps)
01223 {
01224   uint32_t reqAccSteps; 
01225     uint32_t reqDecSteps;
01226    
01227   /* compute the number of steps to get the targeted speed */
01228   uint16_t minSpeed = device_prm.minSpeed;
01229   reqAccSteps = (device_prm.maxSpeed - minSpeed);
01230   reqAccSteps *= (device_prm.maxSpeed + minSpeed);
01231   reqDecSteps = reqAccSteps;
01232   reqAccSteps /= (uint32_t)device_prm.acceleration;
01233   reqAccSteps /= 2;
01234 
01235   /* compute the number of steps to stop */
01236   reqDecSteps /= (uint32_t)device_prm.deceleration;
01237   reqDecSteps /= 2;
01238 
01239     if(( reqAccSteps + reqDecSteps ) > nbSteps)
01240     {   
01241     /* Triangular move  */
01242     /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */
01243     uint32_t dec = device_prm.deceleration;
01244     uint32_t acc = device_prm.acceleration;
01245     
01246     reqDecSteps =  ((uint32_t) dec * nbSteps) / (acc + dec);
01247     if (reqDecSteps > 1)
01248     {
01249       reqAccSteps = reqDecSteps - 1;
01250       if(reqAccSteps == 0)
01251       {
01252         reqAccSteps = 1;
01253       }      
01254     }
01255     else
01256     {
01257       reqAccSteps = 0;
01258     }
01259     device_prm.endAccPos = reqAccSteps;
01260     device_prm.startDecPos = reqDecSteps;
01261     }
01262     else
01263     {    
01264     /* Trapezoidal move */
01265     /* accelerating phase to endAccPos */
01266     /* steady phase from  endAccPos to startDecPos */
01267     /* decelerating from startDecPos to stepsToTake*/
01268     device_prm.endAccPos = reqAccSteps;
01269     device_prm.startDecPos = nbSteps - reqDecSteps - 1;
01270     }
01271 }
01272 
01273 /**********************************************************
01274  * @brief  Handlers of the flag interrupt which calls the user callback (if defined)
01275  * @retval None
01276  **********************************************************/
01277 void STSPIN820::STSPIN820_FlagInterruptHandler(void)
01278 {
01279   if (flag_interrupt_callback != 0)
01280   {
01281     flag_interrupt_callback();
01282   }
01283 }
01284 
01285 /**********************************************************
01286  * @brief  Set the parameters of the device whose values are not defined in
01287  * STSPIN820_config.h
01288  * @retval None
01289  **********************************************************/
01290 void STSPIN820::STSPIN820_SetDeviceParamsOtherValues(void)
01291 {
01292   uint16_t tmp;
01293 
01294   device_prm.accu = 0;
01295   device_prm.currentPosition = 0;
01296   device_prm.sequencerPosition = 0;
01297   device_prm.endAccPos = 0;
01298   device_prm.relativePos = 0;
01299   device_prm.startDecPos = 0;
01300   device_prm.stepsToTake = 0;
01301   device_prm.updateTorque = FALSE;
01302   device_prm.speed = 0;
01303   device_prm.commandExecuted = NO_CMD;
01304   device_prm.direction = FORWARD;
01305   tmp = device_prm.minSpeed;
01306   if (((device_prm.torqueBoostEnable != FALSE)&&\
01307        (device_prm.torqueBoostSpeedThreshold>STSPIN820_MAX_STCK_FREQ))||\
01308       (tmp>device_prm.maxSpeed))
01309   {
01310     STSPIN820_ErrorHandler(STSPIN820_ERROR_INIT);
01311   }
01312 }
01313 
01314 /**********************************************************
01315  * @brief  Set the parameters of the device to values of initDevicePrm structure
01316  * @param pInitDevicePrm structure containing values to initialize the device 
01317  * parameters
01318  * @retval None
01319  **********************************************************/
01320 void STSPIN820::STSPIN820_SetDeviceParamsToGivenValues(STSPIN820_init_t* pInitDevicePrm)
01321 {
01322   int i = 100;
01323   printf("%d\r\n", i++);
01324   device_prm.motionState = STANDBY;;
01325 
01326   if (STSPIN820_SetAcceleration(pInitDevicePrm->acceleration)==FALSE)
01327   {
01328     STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_ACCELERATION);
01329   }
01330   printf("%d\r\n", i++);
01331   if (STSPIN820_SetDeceleration(pInitDevicePrm->deceleration)==FALSE)
01332   {
01333     STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_DECELERATION);
01334   }
01335   printf("%d\r\n", i++);
01336   if (STSPIN820_SetMaxSpeed(pInitDevicePrm->maxSpeed)==FALSE)
01337   {
01338     STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MAX_SPEED);
01339   }
01340   printf("%d\r\n", i++);
01341   if (STSPIN820_SetMinSpeed(pInitDevicePrm->minSpeed)==FALSE)
01342   {
01343     STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MIN_SPEED);
01344   }
01345   printf("%d\r\n", i++);
01346   STSPIN820_VrefPwmSetFreq(pInitDevicePrm->vrefPwmFreq);
01347   STSPIN820_SetTorque(ACC_TORQUE,pInitDevicePrm->accelTorque);
01348   STSPIN820_SetTorque(DEC_TORQUE,pInitDevicePrm->decelTorque);
01349   STSPIN820_SetTorque(RUN_TORQUE,pInitDevicePrm->runTorque);
01350   STSPIN820_SetTorque(HOLD_TORQUE,pInitDevicePrm->holdTorque);
01351   device_prm.torqueBoostEnable = pInitDevicePrm->torqueBoostEnable;
01352   device_prm.torqueBoostSpeedThreshold = pInitDevicePrm->torqueBoostSpeedThreshold;
01353   STSPIN820_SetStopMode(pInitDevicePrm->stopMode);
01354   printf("%d\r\n", i++);
01355   STSPIN820_SetDeviceParamsOtherValues();
01356   printf("%d\r\n", i++);
01357   /* Eventually deactivate motor */
01358   if ((device_prm.motionState != INACTIVE)&&\
01359       (device_prm.motionState != STANDBY))
01360   {
01361     STSPIN820_HardHiZ();
01362   }
01363 
01364   /* Enter standby */
01365   STSPIN820_Reset();
01366   
01367   /* Reset the microstepping sequencer position */
01368   device_prm.sequencerPosition = 0;
01369 
01370   /* Reset current and mark positions */
01371   device_prm.currentPosition = 0; 
01372   device_prm.markPosition = 0;
01373   
01374   /* Set predefined step mode */
01375   STSPIN820_SetStepMode(pInitDevicePrm->stepMode);
01376   
01377   /* Wait */
01378   STSPIN820_Delay(SELECT_STEP_MODE_DELAY);
01379   
01380   /* Exit standby */
01381   STSPIN820_ReleaseReset();
01382   
01383     /* Let a delay after reset release*/
01384   STSPIN820_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
01385   
01386 }
01387 
01388 /**********************************************************
01389  * @brief  Sets the parameters of the device to predefined values
01390  * from STSPIN820_config.h
01391  * @retval None
01392  **********************************************************/
01393 void STSPIN820::STSPIN820_SetDeviceParamsToPredefinedValues(void)
01394 {
01395   device_prm.motionState = STANDBY;
01396 
01397   if (STSPIN820_SetAcceleration(STSPIN820_CONF_PARAM_ACC)==FALSE)
01398   {
01399     STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_ACCELERATION);
01400   }
01401   if (STSPIN820_SetDeceleration(STSPIN820_CONF_PARAM_DEC)==FALSE)
01402   {
01403     STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_DECELERATION);
01404   }
01405   if (STSPIN820_SetMaxSpeed(STSPIN820_CONF_PARAM_RUNNING_SPEED)==FALSE)
01406   {
01407     STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MAX_SPEED);
01408   }
01409   if (STSPIN820_SetMinSpeed(STSPIN820_CONF_PARAM_MIN_SPEED)==FALSE)
01410   {
01411     STSPIN820_ErrorHandler(STSPIN820_ERROR_SET_MIN_SPEED);
01412   }
01413 
01414   STSPIN820_VrefPwmSetFreq(STSPIN820_CONF_PARAM_REF_PWM_FREQUENCY);
01415   STSPIN820_SetTorque(ACC_TORQUE,STSPIN820_CONF_PARAM_ACC_TORQUE);
01416   STSPIN820_SetTorque(DEC_TORQUE,STSPIN820_CONF_PARAM_DEC_TORQUE);
01417   STSPIN820_SetTorque(RUN_TORQUE,STSPIN820_CONF_PARAM_RUNNING_TORQUE);
01418   STSPIN820_SetTorque(HOLD_TORQUE,STSPIN820_CONF_PARAM_HOLDING_TORQUE);
01419   device_prm.torqueBoostEnable = STSPIN820_CONF_PARAM_TORQUE_BOOST_EN;
01420   device_prm.torqueBoostSpeedThreshold = STSPIN820_CONF_PARAM_TORQUE_BOOST_TH;
01421   STSPIN820_SetStopMode(STSPIN820_CONF_PARAM_AUTO_HIZ_STOP);
01422 
01423   STSPIN820_SetDeviceParamsOtherValues();
01424   
01425     /* Eventually deactivate motor */
01426   if ((device_prm.motionState != INACTIVE)&&\
01427       (device_prm.motionState != STANDBY))
01428   {
01429     STSPIN820_HardHiZ();
01430   }
01431 
01432   /* Enter standby */
01433   STSPIN820_Reset();
01434   
01435   /* Reset the microstepping sequencer position */
01436   device_prm.sequencerPosition = 0;
01437 
01438   /* Reset current and mark positions */
01439   device_prm.currentPosition = 0; 
01440   device_prm.markPosition = 0;
01441   
01442   /* Set predefined step mode */
01443   STSPIN820_SetStepMode((motor_step_mode_t) STSPIN820_CONF_PARAM_STEP_MODE);
01444     
01445   /* Wait */
01446   STSPIN820_Delay(SELECT_STEP_MODE_DELAY);
01447   
01448   /* Exit standby */
01449   STSPIN820_ReleaseReset();
01450   
01451   /* Let a delay after reset release*/
01452   STSPIN820_Delay(AFTER_STANDBY_EXIT_DEAD_TIME);
01453 }
01454 
01455 /**********************************************************
01456  * @brief Initialises the bridge parameters to start the movement
01457  * and enable the power bridge
01458  * @retval None
01459  **********************************************************/
01460 void STSPIN820::STSPIN820_StartMovement(void)  
01461 {
01462   /* Enable STSPIN820 powerstage */
01463   STSPIN820_Enable();
01464   toggle_odd = 0;
01465   device_prm.accu = 0;
01466   device_prm.relativePos = 0;  
01467   if ((device_prm.endAccPos == 0)&&\
01468       (device_prm.commandExecuted != RUN_CMD))
01469   {
01470     device_prm.motionState = DECELERATING;
01471     STSPIN820_ApplyTorque(DEC_TORQUE);
01472   }
01473   else
01474   {
01475     device_prm.motionState = ACCELERATING;
01476     STSPIN820_ApplyTorque(ACC_TORQUE);
01477   }
01478   STSPIN820_PwmRefStart();
01479   /* Initialize the step clock timer */
01480   STSPIN820_TimStckInit();
01481   /* Program the step clock */
01482   STSPIN820_TimStckCompareInit();
01483   STSPIN820_ApplySpeed(device_prm.minSpeed);
01484   STSPIN820_TimStckStart();
01485 }
01486 
01487 /**********************************************************
01488  * @brief  Handles the device state machine at each pulse
01489  * @retval None
01490  * @note Must only be called by the timer ISR
01491  **********************************************************/
01492 void STSPIN820::STSPIN820_StepClockHandler(void)
01493 {
01494   uint32_t stepModeShift = device_prm.stepModeLatched - device_prm.stepMode;
01495   uint16_t tmp;
01496 
01497   if (device_prm.motionState == STANDBYTOINACTIVE)
01498   {
01499     if (toggle_odd != 0)
01500     {
01501       toggle_odd = 0;
01502       if (device_prm.sequencerPosition == 0)
01503       {
01504         if (STSPIN820_TimStckStop(&toggle_odd) == 0)
01505         {
01506           STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
01507         }
01508         return;
01509       }      
01510     }
01511     else
01512     {
01513       toggle_odd = 1;
01514       tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode));
01515       device_prm.sequencerPosition -= tmp;
01516     }
01517     STSPIN820_TimStckSetFreq(STSPIN820_MAX_STCK_FREQ);
01518     return;
01519   }  
01520   
01521   if (toggle_odd == 0)
01522   {
01523     toggle_odd = 1;
01524   }
01525   else
01526   {
01527     toggle_odd = 0;
01528     /* Incrementation of the relative position */
01529     device_prm.relativePos += (1 << stepModeShift);
01530 
01531     /* Incrementation of the current position */
01532     if (device_prm.direction != BACKWARD)
01533     {
01534       device_prm.currentPosition += (1 << stepModeShift);
01535       tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode));
01536       device_prm.sequencerPosition += tmp;
01537       if (device_prm.sequencerPosition >= (SEQUENCER_MAX_VALUE+1))
01538       {
01539         device_prm.sequencerPosition -= (SEQUENCER_MAX_VALUE+1);
01540       }
01541     }
01542     else
01543     {
01544       device_prm.currentPosition -= (1 << stepModeShift);
01545       tmp = (1 << (STEP_MODE_1_256-device_prm.stepMode));
01546       device_prm.sequencerPosition -= tmp;
01547       if (device_prm.sequencerPosition < 0)
01548       {
01549         device_prm.sequencerPosition += (SEQUENCER_MAX_VALUE+1);
01550       }
01551     }
01552 
01553     switch (device_prm.motionState) 
01554     {
01555       case ACCELERATING: 
01556       {
01557           uint32_t relPos = device_prm.relativePos;
01558           uint32_t endAccPos = device_prm.endAccPos;
01559           uint16_t speed = device_prm.speed;
01560           uint32_t acc = ((uint32_t)device_prm.acceleration << 16)>>stepModeShift;
01561         
01562           if (((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)||\
01563               ((device_prm.commandExecuted==MOVE_CMD)&&(relPos>=device_prm.startDecPos)))
01564           {
01565             device_prm.motionState = DECELERATING;
01566             device_prm.accu = 0;
01567             /* Apply decelerating torque */
01568             STSPIN820_ApplyTorque(DEC_TORQUE);
01569           }
01570           else if ((speed>=(device_prm.maxSpeed>>stepModeShift))||\
01571                    ((device_prm.commandExecuted==MOVE_CMD)&&(relPos >= endAccPos)))
01572           {
01573             device_prm.motionState = STEADY;
01574             /* Apply running torque */
01575             STSPIN820_ApplyTorque(RUN_TORQUE);
01576           }
01577           else
01578           {
01579             bool speedUpdated = FALSE;
01580             /* Go on accelerating */
01581             if (speed==0)
01582             {
01583               speed =1;
01584             }
01585             device_prm.accu += acc / speed;
01586             while (device_prm.accu>=(0X10000L))
01587             {
01588               device_prm.accu -= (0X10000L);
01589               speed +=1;
01590               speedUpdated = TRUE;
01591             }
01592           
01593             if (speedUpdated)
01594             {
01595               if (speed>(device_prm.maxSpeed>>stepModeShift))
01596               {
01597                 speed = device_prm.maxSpeed>>stepModeShift;
01598               }    
01599               device_prm.speed = speed;
01600             }
01601             
01602             if (device_prm.updateTorque!=FALSE)
01603             {
01604               /* Apply accelerating torque */
01605               STSPIN820_ApplyTorque(ACC_TORQUE);              
01606             }
01607           }
01608           break;
01609       }
01610       case STEADY: 
01611       {
01612         uint16_t maxSpeed = device_prm.maxSpeed>>stepModeShift;
01613         uint32_t relativePos = device_prm.relativePos;
01614         if (device_prm.updateTorque!=FALSE)
01615         {
01616           /* Apply accelerating torque */
01617           STSPIN820_ApplyTorque(RUN_TORQUE);
01618         }
01619         if  (((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)||\
01620              ((device_prm.commandExecuted==MOVE_CMD)&&\
01621               (relativePos>=(device_prm.startDecPos)))||\
01622              ((device_prm.commandExecuted==RUN_CMD)&&\
01623               (device_prm.speed>maxSpeed)))
01624         {
01625           device_prm.motionState = DECELERATING;
01626           device_prm.accu = 0;
01627           /* Apply decelerating torque */
01628           STSPIN820_ApplyTorque(DEC_TORQUE);
01629         }
01630         else if ((device_prm.commandExecuted==RUN_CMD)&&(device_prm.speed<maxSpeed))
01631         {
01632           device_prm.motionState = ACCELERATING;
01633           device_prm.accu = 0;
01634           /* Apply accelerating torque */
01635           STSPIN820_ApplyTorque(ACC_TORQUE);
01636         }
01637         break;
01638       }
01639       case DECELERATING: 
01640       {
01641         uint32_t relativePos = device_prm.relativePos;
01642         uint16_t speed = device_prm.speed;
01643         uint32_t dec = ((uint32_t)device_prm.deceleration << 16)>>stepModeShift;
01644         if ((((device_prm.commandExecuted&(STSPIN820_SOFT_STOP_BIT_MASK|STSPIN820_DIR_CHANGE_BIT_MASK))!=0)&&\
01645              (speed<=(device_prm.minSpeed>>stepModeShift)))||\
01646             ((device_prm.commandExecuted==MOVE_CMD)&&(relativePos>=device_prm.stepsToTake)))
01647         {
01648           /* Motion process complete */
01649           if ((device_prm.commandExecuted&STSPIN820_DIR_CHANGE_BIT_MASK)!=0)
01650           {
01651             device_prm.commandExecuted = (device_command_t) (device_prm.commandExecuted & ~STSPIN820_DIR_CHANGE_BIT_MASK);
01652             if (device_prm.direction==BACKWARD)
01653             {
01654               device_prm.direction=FORWARD;
01655             }
01656             else device_prm.direction=BACKWARD;
01657             STSPIN820_SetDirectionGpio(device_prm.direction);
01658             if ((device_prm.commandExecuted&STSPIN820_SOFT_STOP_BIT_MASK)==0)
01659             {
01660               device_prm.motionState = ACCELERATING;
01661               device_prm.accu = 0;
01662               /* Apply accelerating torque */
01663               STSPIN820_ApplyTorque(ACC_TORQUE);
01664               break;
01665             }
01666           }
01667           if (device_prm.stopMode==HOLD_MODE)
01668           {
01669             STSPIN820_HardStop();
01670           }
01671           else if (device_prm.stopMode==STANDBY_MODE)
01672           {
01673             STSPIN820_PutDeviceInStandby();
01674           }
01675           else
01676           {
01677             STSPIN820_HardHiZ();
01678           }
01679         }
01680         else if ((device_prm.commandExecuted==RUN_CMD)&&
01681                  (speed<=(device_prm.maxSpeed>>stepModeShift)))
01682         {
01683           device_prm.motionState = STEADY;
01684           /* Apply running torque */
01685           STSPIN820_ApplyTorque(RUN_TORQUE);
01686         }
01687         else
01688         {
01689           /* Go on decelerating */
01690           if (speed>(device_prm.minSpeed>>stepModeShift))
01691           {
01692             bool speedUpdated = FALSE;
01693             if (speed==0)
01694             {
01695               speed = 1;
01696             }
01697             device_prm.accu += dec / speed;
01698             while (device_prm.accu>=(0X10000L))
01699             {
01700               device_prm.accu -= (0X10000L);
01701               if (speed>1)
01702               {  
01703                 speed -=1;
01704               }
01705               speedUpdated = TRUE;
01706             }
01707           
01708             if (speedUpdated)
01709             {
01710               if (speed<(device_prm.minSpeed>>stepModeShift))
01711               {
01712                 speed = device_prm.minSpeed>>stepModeShift;
01713               }  
01714               device_prm.speed = speed;
01715             }
01716             
01717             if (device_prm.updateTorque!=FALSE)
01718             {
01719               /* Apply decelerating torque */
01720               STSPIN820_ApplyTorque(DEC_TORQUE);
01721             }
01722           }
01723         }
01724         break;
01725       }
01726       default: 
01727       {
01728         break;
01729       }
01730     }
01731   }
01732   if ((device_prm.motionState & INACTIVE) != INACTIVE)
01733   {
01734     STSPIN820_ApplySpeed(device_prm.speed);
01735   }
01736   else
01737   {
01738     if (STSPIN820_TimStckStop(&toggle_odd) == 0)
01739     {
01740       STSPIN820_ErrorHandler(STSPIN820_ERROR_STEP_CLOCK);
01741     }
01742   }
01743 }
01744 
01745 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/