Diff: main.cpp

Revision:

0:cd62cd5b2c77

Child:

1:6b179be4937e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 05 15:40:30 2016 +0000
@@ -0,0 +1,644 @@
+/**
+ ******************************************************************************
+ * @file    main.cpp
+ * @author  IPC Rennes
+ * @version V1.0.0
+ * @date    March 18th, 2016
+ * @brief   mbed test application for the STMicroelectronics X-NUCLEO-IHM03A1
+ *          Motor Control Expansion Board: control of 1 motor.
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; COPYRIGHT(c) 2015 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.
+ *
+ ******************************************************************************
+ */
+
+
+/* Includes ------------------------------------------------------------------*/
+
+/* mbed specific header files. */
+#include "mbed.h"
+
+/* Helper header files. */
+#include "DevSPI.h"
+
+/* Component specific header files. */
+#include "powerstep01_class.h"
+
+/* Variables -----------------------------------------------------------------*/
+
+/* Initialization parameters of the motor connected to the expansion board. */
+/* Current mode. */
+powerstep01_Init_u_t initDeviceParameters =
+{
+  /* common parameters */
+  .cm.cp.cmVmSelection = POWERSTEP01_CM_VM_CURRENT, // enum powerstep01_CmVm_t
+  582, // Acceleration rate in step/s2, range 14.55 to 59590 steps/s^2
+  582, // Deceleration rate in step/s2, range 14.55 to 59590 steps/s^2
+  488, // Maximum speed in step/s, range 15.25 to 15610 steps/s
+  0, // Minimum speed in step/s, range 0 to 976.3 steps/s
+  POWERSTEP01_LSPD_OPT_OFF, // Low speed optimization bit, enum powerstep01_LspdOpt_t
+  244.16, // Full step speed in step/s, range 7.63 to 15625 steps/s
+  POWERSTEP01_BOOST_MODE_OFF, // Boost of the amplitude square wave, enum powerstep01_BoostMode_t
+  281.25, // Overcurrent threshold settings via enum powerstep01_OcdTh_t
+  STEP_MODE_1_16, // Step mode settings via enum motorStepMode_t
+  POWERSTEP01_SYNC_SEL_DISABLED, // Synch. Mode settings via enum powerstep01_SyncSel_t
+  (POWERSTEP01_ALARM_EN_OVERCURRENT|
+   POWERSTEP01_ALARM_EN_THERMAL_SHUTDOWN|
+   POWERSTEP01_ALARM_EN_THERMAL_WARNING|
+   POWERSTEP01_ALARM_EN_UVLO|
+   POWERSTEP01_ALARM_EN_STALL_DETECTION|
+   POWERSTEP01_ALARM_EN_SW_TURN_ON|
+   POWERSTEP01_ALARM_EN_WRONG_NPERF_CMD), // Alarm settings via bitmap enum powerstep01_AlarmEn_t
+  POWERSTEP01_IGATE_64mA, // Gate sink/source current via enum powerstep01_Igate_t 
+  POWERSTEP01_TBOOST_0ns, // Duration of the overboost phase during gate turn-off via enum powerstep01_Tboost_t
+  POWERSTEP01_TCC_500ns, // Controlled current time via enum powerstep01_Tcc_t
+  POWERSTEP01_WD_EN_DISABLE, // External clock watchdog, enum powerstep01_WdEn_t  
+  POWERSTEP01_TBLANK_375ns, // Duration of the blanking time via enum powerstep01_TBlank_t
+  POWERSTEP01_TDT_125ns, // Duration of the dead time via enum powerstep01_Tdt_t
+  /* current mode parameters */
+  328.12, // Hold torque in mV, range from 7.8mV to 1000 mV
+  328.12, // Running torque in mV, range from 7.8mV to 1000 mV 
+  328.12, // Acceleration torque in mV, range from 7.8mV to 1000 mV
+  328.12, // Deceleration torque in mV, range from 7.8mV to 1000 mV
+  POWERSTEP01_TOFF_FAST_8us, //Maximum fast decay time , enum powerstep01_ToffFast_t 
+  POWERSTEP01_FAST_STEP_12us, //Maximum fall step time , enum powerstep01_FastStep_t 
+  3.0, // Minimum on-time in us, range 0.5us to 64us
+  21.0, // Minimum off-time in us, range 0.5us to 64us 
+  POWERSTEP01_CONFIG_INT_16MHZ_OSCOUT_2MHZ, // Clock setting , enum powerstep01_ConfigOscMgmt_t
+  POWERSTEP01_CONFIG_SW_HARD_STOP, // External switch hard stop interrupt mode, enum powerstep01_ConfigSwMode_t
+  POWERSTEP01_CONFIG_TQ_REG_TVAL_USED, // External torque regulation enabling , enum powerstep01_ConfigEnTqReg_t
+  POWERSTEP01_CONFIG_VS_COMP_DISABLE, // Motor Supply Voltage Compensation enabling , enum powerstep01_ConfigEnVscomp_t 
+  POWERSTEP01_CONFIG_OC_SD_DISABLE, // Over current shutwdown enabling, enum powerstep01_ConfigOcSd_t
+  POWERSTEP01_CONFIG_UVLOVAL_LOW, // UVLO Threshold via powerstep01_ConfigUvLoVal_t
+  POWERSTEP01_CONFIG_VCCVAL_15V, // VCC Val, enum powerstep01_ConfigVccVal_t
+  POWERSTEP01_CONFIG_TSW_048us, // Switching period, enum powerstep01_ConfigTsw_t
+  POWERSTEP01_CONFIG_PRED_DISABLE, // Predictive current enabling , enum powerstep01_ConfigPredEn_t 
+};
+
+/* Motor Control Component. */
+POWERSTEP01 *motor;
+
+
+/* Functions -----------------------------------------------------------------*/
+
+/**
+ * @brief  This is an example of user handler for the flag interrupt.
+ * @param  None
+ * @retval None
+ * @note   If needed, implement it, and then attach and enable it:
+ *           + motor->AttachFlagIRQ(&FlagIRQHandler);
+ *           + motor->EnableFlagIRQ();
+ *         To disable it:
+ *           + motor->DisbleFlagIRQ();
+ */
+void myFlagIRQHandler(void)
+{
+  /* Set ISR flag. */
+  motor->isrFlag = TRUE;
+
+  /* Get the value of the status register. */
+  unsigned int statusRegister = motor->GetStatus();
+ 
+  /* Check HIZ flag: if set, power brigdes are disabled */
+  if ((statusRegister & POWERSTEP01_STATUS_HIZ)==POWERSTEP01_STATUS_HIZ)
+  {
+    // HIZ state
+    printf("    WARNING: \"FLAG\" interrupt triggered. HiZ state.\r\n");
+  }
+  /* Check BUSY flag: if not set, a command is under execution */
+  if ((statusRegister & POWERSTEP01_STATUS_BUSY)==0)
+  {
+    // BUSY
+  }
+  /* Check SW_F flag: if not set, the SW input is opened */
+  if ((statusRegister & POWERSTEP01_STATUS_SW_F )==0)
+  {
+     // SW OPEN
+  }
+  else
+  {
+    // SW CLOSED   
+  }  
+  /* Check SW_EN bit */
+  if ((statusRegister & POWERSTEP01_STATUS_SW_EVN)==POWERSTEP01_STATUS_SW_EVN)
+  {
+    // switch turn_on event
+  }  
+  /* Check direction bit */
+  if ((statusRegister & POWERSTEP01_STATUS_DIR)==0)
+  {
+    // BACKWARD
+  }
+  else  
+  {
+    // FORWARD 
+  }
+  if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)==
+      POWERSTEP01_STATUS_MOT_STATUS_STOPPED)
+  {
+    // MOTOR STOPPED
+  }
+  else  if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)==
+            POWERSTEP01_STATUS_MOT_STATUS_ACCELERATION)
+  {
+    // MOTOR ACCELERATION
+  }  
+  else  if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)==
+            POWERSTEP01_STATUS_MOT_STATUS_DECELERATION)
+  {
+    // MOTOR DECELERATION
+  }
+  else  if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)==
+            POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD)
+  {
+    // MOTOR RUNNING AT CONSTANT SPEED
+  }  
+  /* Check Command Error flag: if set, the command received by SPI can't be */
+  /* performed. This occurs for instance when a move command is sent to the */
+  /* Powerstep01 while it is already running */
+  if ((statusRegister & POWERSTEP01_STATUS_CMD_ERROR)==
+      POWERSTEP01_STATUS_CMD_ERROR)
+  {
+    // Command Error
+    printf("    WARNING: \"FLAG\" interrupt triggered. Non-performable command detected.\r\n");
+  }  
+  /* Check Step mode clock flag: if set, the device is working in step clock mode */
+  if ((statusRegister & POWERSTEP01_STATUS_STCK_MOD)==
+      POWERSTEP01_STATUS_STCK_MOD)
+  {
+     //Step clock mode enabled
+  }  
+  /* Check UVLO flag: if not set, there is an undervoltage lock-out */
+  if ((statusRegister & POWERSTEP01_STATUS_UVLO)==0)
+  {
+     //undervoltage lock-out 
+  }  
+  /* Check UVLO ADC flag: if not set, there is an ADC undervoltage lock-out */
+  if ((statusRegister & POWERSTEP01_STATUS_UVLO_ADC)==0)
+  {
+     //ADC undervoltage lock-out 
+  } 
+  /* Check thermal STATUS flags: if  set, the thermal status is not normal */
+  if ((statusRegister & POWERSTEP01_STATUS_TH_STATUS)!=0)
+  {
+    //thermal status: 1: Warning, 2: Bridge shutdown, 3: Device shutdown
+  }    
+  /* Check OCD  flag: if not set, there is an overcurrent detection */
+  if ((statusRegister & POWERSTEP01_STATUS_OCD)==0)
+  {
+    //overcurrent detection 
+  }      
+  /* Check STALL_A flag: if not set, there is a Stall condition on bridge A */
+  if ((statusRegister & POWERSTEP01_STATUS_STALL_A)==0)
+  {
+    //overcurrent detection 
+  }    
+  /* Check STALL_B flag: if not set, there is a Stall condition on bridge B */
+  if ((statusRegister & POWERSTEP01_STATUS_STALL_B)==0)
+  {
+    //overcurrent detection 
+  }      
+    
+  /* Reset ISR flag. */
+  motor->isrFlag = FALSE;
+}
+
+/**
+ * @brief  This is an example of user handler for the busy interrupt.
+ * @param  None
+ * @retval None
+ * @note   If needed, implement it, and then attach and enable it:
+ *           + motor->AttachBusyIRQ(&FlagIRQHandler);
+ *           + motor->EnableBusyIRQ();
+ *         To disable it:
+ *           + motor->DisbleBusyIRQ();
+ */
+void myBusyIRQHandler(void)
+{
+  /* Set ISR flag. */
+  motor->isrFlag = TRUE;
+
+  if (motor->CheckBusyHw())
+  {
+    /* Busy pin is low, so at list one Powerstep01 chip is busy */
+    /* To be customized (for example Switch on a LED) */
+  }
+  else
+  {
+    /* To be customized (for example Switch off a LED) */        
+  }
+  
+  /* Reset ISR flag. */
+  motor->isrFlag = FALSE;
+}
+
+/**
+ * @brief  This is an example of error handler.
+ * @param[in] error Number of the error
+ * @retval None
+ */
+void myErrorHandler(uint16_t error)
+{
+  /* Printing to the console. */
+  printf("Error %d detected\r\n\n", error);
+  
+  /* Infinite loop */
+  while(1)
+  {
+  }    
+}
+
+/* Main ----------------------------------------------------------------------*/
+
+int main()
+{
+  int32_t pos;
+  uint32_t unsignedIntegerValue;
+  float floatValue;
+  
+  /* Printing to the console. */
+  printf("STARTING MAIN PROGRAM\r\n");
+  printf("    Reminder:\r\n");
+  printf("    The position unit is in agreement to the step mode.\r\n");
+  printf("    The speed, acceleration or deceleration unit\r\n");
+  printf("    do not depend on the step mode and the step unit is a full step.\r\n");
+    
+//----- Initialization 
+  /* Initializing SPI bus. */
+  DevSPI dev_spi(D11, D12, D13);
+
+  /* Initializing Motor Control Component. */
+  motor = new POWERSTEP01(D2, D4, D8, D9, D10, dev_spi);
+  if (motor->Init(&initDeviceParameters) != COMPONENT_OK) exit(EXIT_FAILURE);
+
+  /* Attaching and enabling interrupt handlers. */
+  motor->AttachFlagIRQ(&myFlagIRQHandler);
+  motor->EnableFlagIRQ();
+  motor->AttachBusyIRQ(&myBusyIRQHandler);
+  motor->EnableBusyIRQ();
+    
+  /* Attaching an error handler */
+  motor->AttachErrorHandler(&myErrorHandler);
+
+  /* Printing to the console. */
+  printf("Motor Control Application Example for 1 Motor\r\n");
+
+//----- Move of 16000 steps in the FW direction
+  /* Printing to the console. */
+  printf("--> Moving forward 16000 steps.\r\n");
+
+  /* Move 16000 steps in the FORWARD direction*/
+  motor->Move(StepperMotor::FWD, 16000);
+
+  /* Waiting while the motor is active. */
+  motor->WaitWhileActive();
+
+  /* Wait for 2 seconds */  
+  wait_ms(2000);
+
+//----- Move of 16000 steps in the BW direction
+  /* Printing to the console. */
+  printf("--> Moving backward 16000 steps.\r\n");
+
+  /* Move 16000 steps in the BACKWARD direction*/
+  motor->Move(StepperMotor::BWD, 16000);
+
+  /* Waiting while the motor is active. */
+  motor->WaitWhileActive();
+
+  /* Wait for 2 seconds */  
+  wait_ms(2000);
+
+//----- Go to position -6400
+  /* Printing to the console. */
+  printf("--> Go to position -6400 steps.\r\n");
+
+  /* Request device to go to position -6400 */
+  motor->GoTo(-6400);  
+  
+  /* Wait for the motor ends moving */
+  motor->WaitWhileActive();
+
+  /* Get current position of device and print to the console */
+  pos = motor->GetPosition();
+  printf("    Position: %d.\r\n", pos);
+
+  /* Mark the current position and print to the console */
+  motor->SetMark();
+  printf("    Position marked: -6400.\r\n");
+  
+  /* Wait for 2 seconds */  
+  wait_ms(2000);
+
+//----- Go Home
+  /* Printing to the console. */
+  printf("--> Go to home position.\r\n");
+  
+  /* Request device to go to Home */
+  motor->GoHome();  
+  motor->WaitWhileActive();
+
+  /* Get current position of device and print to the console */
+  pos = motor->GetPosition();
+  printf("    Position: %d.\r\n", pos);
+  
+  /* Wait for 2 seconds */
+  wait_ms(2000);
+
+//----- Go to position 6400
+  /* Printing to the console. */
+  printf("--> Go to position 6400 steps.\r\n");
+  
+  /* Request device to go to position 6400 */
+  motor->GoTo(6400);  
+  
+  /* Wait for the motor of device ends moving */
+  motor->WaitWhileActive();
+
+  /* Get current position of device and print to the console */
+  pos = motor->GetPosition();
+  printf("    Position: %d.\r\n", pos);
+
+  /* Wait for 2 seconds */
+  wait_ms(2000);
+  
+//----- Go Mark which was set previously after go to -6400
+  /* Printing to the console. */
+  printf("--> Go to the marked position.\r\n");
+  
+  /* Request device to go to Mark position */
+  motor->GoMark();  
+  
+  /* Wait for the motor of device ends moving */
+  motor->WaitWhileActive();
+
+  /* Get current position of device and print to the console */
+  pos = motor->GetPosition();
+  printf("    Position: %d.\r\n", pos);
+
+  /* Wait for 2 seconds */
+  wait_ms(2000);
+
+//----- Run the motor BACKWARD
+  /* Printing to the console. */
+  printf("--> Run the motor backward at 400 step/s.\r\n");
+  
+  /* Request device to run BACKWARD at 400 step/s */
+  motor->Run(StepperMotor::BWD,400);
+
+//----- Get parameter example   
+  /* Wait for device reaches the targeted speed */
+  while((motor->ReadStatusRegister() & POWERSTEP01_STATUS_MOT_STATUS)!=
+        POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD);
+ 
+  /* Record the reached speed in step/s */
+  floatValue = motor->GetAnalogValue(POWERSTEP01_SPEED); 
+  /* Print reached speed to the console in step/s */
+  printf("    Reached Speed: %f step/s.\r\n", floatValue);
+
+  /* Record the reached speed in step/s rounded to integer */
+  unsignedIntegerValue = motor->GetSpeed();
+  /* Print reached speed to the console in step/s */
+  printf("    Reached Speed: %d step/s.\r\n", unsignedIntegerValue);
+  
+  /* Record the reached speed in step/tick */
+  unsignedIntegerValue = motor->GetParameter(POWERSTEP01_SPEED); 
+  /* Print reached speed to the console in step/tick */
+  printf("    Reached Speed: %d step/tick.\r\n", unsignedIntegerValue);
+
+//----- Soft stopped required while running
+  /* Printing to the console. */
+  printf("--> Soft stop requested.\r\n");
+  
+  /* Request a soft stop of device and keep the power bridges enabled */
+  motor->SoftHiZ();
+
+  /* Wait for the motor of device ends moving */  
+  motor->WaitWhileActive();
+
+  /* Wait for 2 seconds */
+  wait_ms(2000);
+
+//----- Run stopped by hardstop
+  /* Printing to the console. */
+  printf("--> Run the motor forward at 300 step/s.\r\n");
+  
+  /* Request device to run in FORWARD direction at 300 step/s */
+  motor->Run(StepperMotor::FWD,300);         
+  wait_ms(5000);
+  
+  /* Printing to the console. */
+  printf("--> Hard stop requested.\r\n");
+  
+  /* Request device to immediatly stop */
+  motor->HardStop();
+  motor->WaitWhileActive();
+
+  /* Wait for 2 seconds */
+  wait_ms(2000);
+
+//----- GOTO stopped by soft stop
+  /* Printing to the console. */
+  printf("--> Go to position 200000 steps.\r\n");
+  
+  /* Request device to go to position 200000  */
+  motor->GoTo(200000);  
+  wait_ms(2000);
+
+  /* Printing to the console. */
+  printf("--> Soft stop requested.\r\n");
+
+  /* Request device to perform a soft stop */
+  motor->SoftStop();
+  motor->WaitWhileActive();
+  
+  /* Get current position of device and print to the console */
+  pos = motor->GetPosition();
+  printf("    Position: %d.\r\n", pos);
+
+  /* Wait for 2 seconds */
+  wait_ms(2000);
+
+//----- Read inexistent register to test MyFlagInterruptHandler
+  /* Printing to the console. */
+  printf("--> Try to read an inexistent register.\r\n");
+
+  /* Try to read an inexistent register */
+  /* the flag interrupt should be raised */
+  /* and the MyFlagInterruptHandler function called */
+  motor->GetParameter(0x1F);
+  
+//----- Put the bridges in high impedance
+  /* Printing to the console. */
+  printf("--> Request disabling of device power bridges.\r\n");
+    
+  /* Request disabling of device power bridges */
+  motor->HardHiZ();
+  wait_ms(5000);
+  
+//----- Step clock mode example
+  /* Printing to the console. */
+  printf("--> Enable Step Clock Mode of the Powerstep01, enabling power bridges.\r\n");
+  
+  /* Enable Step Clock Mode of the Powerstep01, enabling power bridges */
+  motor->StepClockModeEnable(StepperMotor::FWD);
+  
+  /* Printing to the console. */ 
+  printf("--> Start step clock.\r\n");  
+  
+  /* Enable the step clock at 333 Hz */
+  motor->StepClockStart(333);
+  
+  /* Let the motor runs for 5 second at 333 step/s */
+  wait_ms(5000);
+
+  /* Printing to the console. */ 
+  printf("--> Stop step clock.\r\n"); 
+  
+  /* Stop the step clock */
+  motor->StepClockStop();
+  
+  /* Wait for 2 seconds */
+  wait_ms(2000);
+
+//----- Set parameter example
+  /* Printing to the console. */
+  printf("--> Set POWERSTEP01_MAX_SPEED parameter to 3.\r\n");
+  printf("    This is corresponding to 45.75 full step/s.\r\n");
+
+  /* Change the maximum speed to 3*2^18 step/tick */
+  motor->SetParameter(POWERSTEP01_MAX_SPEED, 3);
+  
+  /* Printing to the console. */
+  printf("--> Move the motor 3200 steps in forward direction.\r\n");
+    
+  /* Request device to move 3200 microsteps */
+  motor->Move(StepperMotor::FWD,3200);
+    
+  /* Wait for the motor ends moving */
+  motor->WaitWhileActive();
+  
+  /* Wait for 2 seconds */
+  wait_ms(2000);
+  
+//----- Change step mode to full step mode
+  /* Printing to the console. */
+  printf("--> Select full step mode.\r\n");
+
+  /* Select full step mode for device */
+  motor->SetStepMode(STEP_MODE_FULL);
+
+  /* Printing to the console. */
+  printf("--> Set the device position to POWERSTEP01_MIN_POSITION + 199 (-2^21+199).\r\n");
+
+  /* Set the device position to POWERSTEP01_MIN_POSITION + 199 */
+  motor->SetParameter(POWERSTEP01_ABS_POS,(uint32_t)(POWERSTEP01_MIN_POSITION+199));
+  
+  /* Get current position of device and print to the console */
+  pos = motor->GetPosition();
+  printf("    Position: %d.\r\n", pos);
+
+  /* Printing to the console. */
+  printf("--> Go to POWERSTEP01_MAX_POSITION (2^21-1) using the shortest path.\r\n");
+
+  /* Request device to go to the POWERSTEP01_MAX_POSITION using the shortest path */
+  motor->GoTo(POWERSTEP01_MAX_POSITION);
+
+  /* Wait for the motor of device ends moving */
+  motor->WaitWhileActive();
+
+  /* Get current position of device and print to the console */
+  pos = motor->GetPosition();
+  printf("    Position: %d.\r\n", pos);
+
+  /* Wait for 2 seconds */
+  wait_ms(2000);
+  
+  /* Printing to the console. */
+  printf("--> Set the device position to POWERSTEP01_MIN_POSITION + 199 (-2^21+199).\r\n");
+
+  /* Set the device position to POWERSTEP01_MIN_POSITION + 199 */
+  motor->SetParameter(POWERSTEP01_ABS_POS,(uint32_t)(POWERSTEP01_MIN_POSITION+199));
+  
+  /* Get current position of device and print to the console */
+  pos = motor->GetPosition();
+  printf("    Position: %d.\r\n", pos);
+
+//----- GoTo_DIR example
+  /* Printing to the console. */
+  printf("--> Go to the POWERSTEP01_MAX_POSITION using backward direction.\r\n");
+
+  /* Request device to go to the POWERSTEP01_MAX_POSITION using backward direction */
+  motor->GoTo(StepperMotor::BWD,POWERSTEP01_MAX_POSITION);
+
+  /* Wait for the motor of device ends moving */
+  motor->WaitWhileActive();
+
+  /* Get current position of device and print to the console */
+  pos = motor->GetPosition();
+  printf("    Position: %d.\r\n", pos); 
+ 
+   /* Printing to the console. */
+  printf("--> Restore device initial max speed.\r\n");
+  
+  /* Restore device initial max speed */
+  motor->SetAnalogValue(POWERSTEP01_MAX_SPEED, POWERSTEP01_CONF_PARAM_MAX_SPEED_DEVICE_0);
+  
+  /* Get the maximum speed in step/s */
+  floatValue = motor->GetAnalogValue(POWERSTEP01_MAX_SPEED); 
+  /* Print maximum speed to the console in step/s */
+  printf("    Maximum Speed: %f step/s.\r\n", floatValue); 
+  
+//----- Restore initial microstepping mode
+   /* Printing to the console. */
+  printf("--> Restore device initial step mode.\r\n");
+
+  /* Reset device to its initial  microstepping mode */
+  motor->SetStepMode(POWERSTEP01_CONF_PARAM_STEP_MODE_DEVICE_0);
+
+  /* Wait for 2 seconds */  
+  wait_ms(2000);
+
+  /* Printing to the console. */
+  printf("--> Infinite Loop...\r\n");
+  /* Infinite Loop. */
+  while (1)
+  {
+    /* Request device to go position -6400 */
+    motor->GoTo(-6400);
+
+    /* Waiting while the motor is active. */
+    motor->WaitWhileActive();
+
+    /* Request device to go position 6400 */
+    motor->GoTo(6400);
+ 
+    /* Waiting while the motor is active. */
+    motor->WaitWhileActive();
+  } 
+}
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/