Diff: main.cpp

Revision:

0:36024a9bd220

Child:

1:1d98c151c8eb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 05 15:33:37 2016 +0000
@@ -0,0 +1,645 @@
+/**
+ ******************************************************************************
+ * @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 Boards: control of 3 motors with 3
+ *          expansions boards.
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; COPYRIGHT(c) 2016 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 *motor1;
+POWERSTEP01 *motor2;
+POWERSTEP01 *motor3;
+
+/* 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. */
+  POWERSTEP01::isrFlag = TRUE;
+
+  motor1->FetchAndClearAllStatus();
+  POWERSTEP01 *motor;
+  motor = motor1;
+  unsigned int statusRegister;
+  
+  printf("    WARNING: \"FLAG\" interrupt triggered.\r\n");
+  /* Get the value of the status register. */
+  for (uint8_t loop = 0; loop<POWERSTEP01::GetNbDevices();loop++)
+  {
+    if (loop==1) motor = motor2;
+    if (loop==2) motor = motor3;
+    statusRegister = motor->GetFetchedStatus();
+    printf("    Motor%d:\r\n",loop+1);
+    /* Check HIZ flag: if set, power brigdes are disabled */
+    if ((statusRegister & POWERSTEP01_STATUS_HIZ)==POWERSTEP01_STATUS_HIZ)
+    {
+      // HIZ state
+      printf("    HiZ state.\r\n");
+    }
+    /* Check BUSY flag: if not set, a command is under execution */
+    if ((statusRegister & POWERSTEP01_STATUS_BUSY)==0)
+    {
+      // BUSY
+      printf("    Busy.\r\n");
+    }
+    /* Check SW_F flag: if not set, the SW input is opened */
+    if ((statusRegister & POWERSTEP01_STATUS_SW_F )!=0)
+    {
+       // SW closed (connected to ground)
+       printf("    SW closed (connected to ground).\r\n");
+    }
+    /* Check SW_EN bit */
+    if ((statusRegister & POWERSTEP01_STATUS_SW_EVN)==
+        POWERSTEP01_STATUS_SW_EVN)
+    {
+      // SW turn_on event
+       printf("    SW turn_on event.\r\n");
+    }
+    if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)==
+        POWERSTEP01_STATUS_MOT_STATUS_STOPPED)
+    {
+      // MOTOR STOPPED
+      printf("    Stopped.\r\n");
+    }
+    else
+    {
+      if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)==
+                POWERSTEP01_STATUS_MOT_STATUS_ACCELERATION)
+      {
+        // MOTOR ACCELERATION
+        printf("    Accelerating ");
+      }  
+      else  if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)==
+                POWERSTEP01_STATUS_MOT_STATUS_DECELERATION)
+      {
+        // MOTOR DECELERATION
+        printf("    Decelerating ");
+      }
+      else  if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)==
+                POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD)
+      {
+        // MOTOR RUNNING AT CONSTANT SPEED
+        printf("    Steady running ");        
+      } 
+      /* Check direction bit */
+      if ((statusRegister & POWERSTEP01_STATUS_DIR)==0)
+      {
+      // StepperMotor::BWD
+       printf(" in backward direction.\r\n");
+      }
+      else  
+      {
+        // StepperMotor::FWD
+        printf(" in forward direction.\r\n");
+      }
+    } 
+    /* 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("    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
+      printf("    Step clock mode enabled.\r\n");
+    }  
+    /* Check UVLO flag: if not set, there is an undervoltage lock-out */
+    if ((statusRegister & POWERSTEP01_STATUS_UVLO)==0)
+    {
+       //Undervoltage lock-out
+       printf("    undervoltage lock-out.\r\n"); 
+    }  
+    /* 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
+       printf("    ADC undervoltage lock-out.\r\n");  
+    } 
+    /* 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
+      if (((statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11)==1)
+      {
+        printf("    Thermal status - Warning.\r\n");
+      }
+      else if (((statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11)==2)
+      {
+        printf("    Thermal status - Bridge shutdown.\r\n");
+      }
+      else if (((statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11)==3)
+      {
+        printf("    Thermal status - Device shutdown.\r\n");
+      }
+    }    
+    /* Check OCD  flag: if not set, there is an overcurrent detection */
+    if ((statusRegister & POWERSTEP01_STATUS_OCD)==0)
+    {
+      //Overcurrent detection
+      printf("    Overcurrent detection.\r\n"); 
+    }      
+    /* Check STALL_A flag: if not set, there is a Stall condition on bridge A */
+    if ((statusRegister & POWERSTEP01_STATUS_STALL_A)==0)
+    {
+      //Bridge A stalled
+      printf("    Bridge A stalled.\r\n");   
+    }    
+    /* Check STALL_B flag: if not set, there is a Stall condition on bridge B */
+    if ((statusRegister & POWERSTEP01_STATUS_STALL_B)==0)
+    {
+      //Bridge B stalled
+      printf("    Bridge B stalled.\r\n");  
+    }
+  } 
+  /* Reset ISR flag. */
+  POWERSTEP01::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. */
+  POWERSTEP01::isrFlag = TRUE;
+  
+  /* Reset ISR flag. */
+  POWERSTEP01::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)
+  {
+  }    
+}
+
+void WaitForAllDevicesNotBusy(void)
+{
+  /* Wait while at least one is active */
+  while (motor1->IsDeviceBusy()|motor2->IsDeviceBusy()|motor3->IsDeviceBusy());
+}
+
+/* Main ----------------------------------------------------------------------*/
+
+int main()
+{
+  int32_t pos;
+  uint32_t myMaxSpeed;
+  uint32_t myMinSpeed;
+  uint16_t myAcceleration;
+  uint16_t myDeceleration;
+  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. */
+  motor1 = new POWERSTEP01(D2, D4, D8, D9, D10, dev_spi);
+  motor2 = new POWERSTEP01(D2, D4, D8, D9, D10, dev_spi);
+  motor3 = new POWERSTEP01(D2, D4, D8, D9, D10, dev_spi);
+  if (motor1->Init(&initDeviceParameters) != COMPONENT_OK) exit(EXIT_FAILURE);
+  if (motor2->Init(&initDeviceParameters) != COMPONENT_OK) exit(EXIT_FAILURE);
+  if (motor3->Init(&initDeviceParameters) != COMPONENT_OK) exit(EXIT_FAILURE);
+
+  /* Attaching and enabling interrupt handlers. */
+  motor1->AttachFlagIRQ(&myFlagIRQHandler);
+  motor1->EnableFlagIRQ();
+  motor1->AttachBusyIRQ(&myBusyIRQHandler);
+  motor1->EnableBusyIRQ();
+  motor2->AttachFlagIRQ(&myFlagIRQHandler);
+  motor2->EnableFlagIRQ();
+  motor2->AttachBusyIRQ(&myBusyIRQHandler);
+  motor2->EnableBusyIRQ();
+  motor3->AttachFlagIRQ(&myFlagIRQHandler);
+  motor3->EnableFlagIRQ();
+  motor3->AttachBusyIRQ(&myBusyIRQHandler);
+  motor3->EnableBusyIRQ();
+  
+  /* Attaching an error handler */
+  motor1->AttachErrorHandler(&myErrorHandler);
+  motor2->AttachErrorHandler(&myErrorHandler);
+  motor3->AttachErrorHandler(&myErrorHandler);
+  
+  /* Printing to the console. */
+  printf("Motor Control Application Example for 3 Motors\r\n");
+  
+  /* Request motor 1 to go to position 3200 and print to the console */
+  printf("--> Request motor1 to go to position 3200.\r\n");
+  motor1->GoTo(3200);
+
+  /* Wait for motor 2 ends moving */  
+  motor1->WaitWhileActive();
+
+  /* Get current position of motor 1 and print to the console */
+  pos = motor1->GetPosition();
+  printf("    Motor1 position: %d.\r\n", pos);
+  
+  /* Wait for 2 seconds */  
+  wait_ms(2000);
+  
+  /* If the read position of motor 1 is 3200 */
+  /* Request motor 2 to go to the same position */
+  if (pos == 3200)
+  {  
+    /* Set current position of motor 1 to be its mark position*/
+    printf("    Set mark to current position of motor1.\r\n");     
+    motor1->SetMark();
+  
+    /* Request motor 2 to Go to the same position and print to the console */
+    printf("--> Request motor2 to go to position 3200.\r\n"); 
+    motor2->GoTo(pos); 
+
+    /* Wait for  motor 2 ends moving */  
+    motor2->WaitWhileActive();
+  }
+  
+  /* Get current position of motor 2 and print to the console */
+  pos = motor2->GetPosition();
+  printf("    Motor2 position: %d.\r\n", pos);
+
+  /* If the read position of motor 2 is 3200 */
+  /* Request motor 3 to go to the same position */
+  if (pos == 3200)
+  {
+    /* Request motor 3 to Go to the same position and print to the console */
+    printf("--> Request motor3 to go to position 3200.\r\n");
+    motor3->GoTo(pos); 
+  
+    /* Wait for motor 3 ends moving */  
+    motor3->WaitWhileActive();
+  }
+
+  /* Get current position of motor 3 and print to the console */
+  pos = motor3->GetPosition();
+  printf("    Motor3 position: %d.\r\n", pos);
+
+  /* Wait for 1s */
+  wait_ms(1000);
+  
+  if (pos == 3200)
+  {
+    /* Request all motors to go home and print to the console */
+    printf("    Request all motors to go home.\r\n");
+    motor1->QueueCommands(POWERSTEP01_GO_HOME,0);
+    motor2->QueueCommands(POWERSTEP01_GO_HOME,0);
+    motor3->QueueCommands(POWERSTEP01_GO_HOME,0);
+    motor1->SendQueuedCommands();
+
+    /* Wait for all motors ends moving */ 
+    WaitForAllDevicesNotBusy();
+  }
+  
+  /* Wait for 1s */
+  wait_ms(1000);
+  
+  /* Request motor 1 to Goto position -3200 and print to the console */
+  printf("--> Request motor1 to go to position -3200.\r\n"); 
+  motor1->GoTo(-3200);  
+
+  /* Wait for motor 1 ends moving */  
+  motor1->WaitWhileActive();
+
+  /* Get current position of motor 1 and print to the console */
+  pos = motor1->GetPosition();
+  printf("    Motor1 position: %d.\r\n", pos);
+    
+  /* If the read position of motor 1 is -3200 */
+  /* Request motor 2 to go to the same position */
+  if (pos == -3200)
+  {
+    /* Request motor 2 to go to the same position and print to the console */
+    printf("--> Request motor2 to go to position -3200.\r\n"); 
+    motor2->GoTo(pos); 
+
+    /* Wait for  motor 2 ends moving */  
+    motor2->WaitWhileActive();
+  }
+  
+  /* Get current position of motor 2 and print to the console */
+  pos = motor2->GetPosition();
+  printf("    Motor2 position: %d.\r\n", pos);
+
+  /* If the read position of motor 2 is -3200 */
+  /* Request motor 3 to go to the same position */
+  if (pos == -3200)
+  {
+    /* Request motor 3 to go to the same position and print to the console */
+    printf("--> Request motor3 to go to position -3200.\r\n"); 
+    motor3->GoTo(pos); 
+  
+    /* Wait for motor 3 ends moving */  
+    motor3->WaitWhileActive();
+  }
+
+  /* Get current position of motor 3 and print to the console */
+  pos = motor3->GetPosition();
+  printf("    Motor3 position: %d.\r\n", pos);
+
+  /* Wait for 1s */
+  wait_ms(1000);
+  
+  if (pos == -3200)
+  {
+    /* Set current position of motor 3 to be its mark position*/
+    printf("    Set mark to current position of motor3.\r\n");
+    motor3->SetMark();
+    
+    /* Request all motors to go home and print to the console */
+    printf("--> Request all motors to go home.\r\n");
+    motor1->QueueCommands(POWERSTEP01_GO_HOME,0);
+    motor2->QueueCommands(POWERSTEP01_GO_HOME,0);
+    motor3->QueueCommands(POWERSTEP01_GO_HOME,0);
+    motor1->SendQueuedCommands();
+    
+    /* Wait for all device ends moving */ 
+    WaitForAllDevicesNotBusy();
+  }
+
+  /* Wait for 1s */
+  wait_ms(1000);
+  
+  /* Request motor 1 and motor 3 to go their mark position */
+  printf("--> Request motor1 and motor3 to go to their marked position.\r\n");
+  motor1->QueueCommands(POWERSTEP01_GO_MARK,0);
+  motor2->QueueCommands(POWERSTEP01_NOP,0);
+  motor3->QueueCommands(POWERSTEP01_GO_MARK,0);
+  motor1->SendQueuedCommands();
+ 
+  /* Wait for motor 1 and 2 ends moving */ 
+  WaitForAllDevicesNotBusy();
+  
+  /* Wait for 1s */
+  wait_ms(1000);
+
+  /* Request motor 1 to run in StepperMotor::FWD direction at 400 steps/s*/
+  printf("--> Request motor1 to run at 400 steps/s in forward direction.\r\n");
+  motor1->Run(StepperMotor::FWD, 400);
+
+  /* Wait for device to reach the targeted speed */
+  while((motor1->ReadStatusRegister() & POWERSTEP01_STATUS_MOT_STATUS)!=
+        POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD)
+  {
+    /* Record the reached speed in step/s rounded to integer */
+    unsignedIntegerValue = motor1->GetSpeed();
+    /* Print reached speed to the console in step/s */
+    printf("    motor1 reached Speed: %d step/s.\r\n", unsignedIntegerValue);
+    wait_ms(50);
+  }
+ 
+  /* Record the reached speed in step/s */
+  floatValue = motor1->GetAnalogValue(POWERSTEP01_SPEED); 
+  /* Print reached speed to the console in step/s */
+  printf("    motor1 reached Speed: %f step/s.\r\n", floatValue);
+
+  /* Request motor 2 to run in StepperMotor::FWD direction at 300 steps/s*/
+  /* Request motor 3 to run in StepperMotor::FWD direction at 200 steps/s*/
+  /* and start at same time. */
+  printf("--> Request motor2 and motor3 to run respectively in forward direction\r\n");
+  printf("    at 300 steps/s and 200 steps/s and start at same time.\r\n");
+  motor1->QueueCommands(POWERSTEP01_NOP,0);
+  motor2->QueueCommands((uint8_t)POWERSTEP01_RUN|(uint8_t)StepperMotor::FWD,POWERSTEP01::Speed_Steps_s_to_RegVal(300));
+  motor3->QueueCommands((uint8_t)POWERSTEP01_RUN|(uint8_t)StepperMotor::FWD,POWERSTEP01::Speed_Steps_s_to_RegVal(200));
+  motor1->SendQueuedCommands();  
+ 
+  /* Wait for device to reach the targeted speed */
+  while(((motor2->ReadStatusRegister() & POWERSTEP01_STATUS_MOT_STATUS)!=
+        POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD)||
+        ((motor3->ReadStatusRegister() & POWERSTEP01_STATUS_MOT_STATUS)!=
+        POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD));
+ 
+  /* Record the reached speed in step/s */
+  floatValue = motor2->GetAnalogValue(POWERSTEP01_SPEED); 
+  /* Print reached speed to the console in step/s */
+  printf("    motor2 reached Speed: %f step/s.\r\n", floatValue);
+  
+   /* Record the reached speed in step/s */
+  floatValue = motor3->GetAnalogValue(POWERSTEP01_SPEED); 
+  /* Print reached speed to the console in step/s */
+  printf("    motor3 reached Speed: %f step/s.\r\n", floatValue); 
+  
+  /* Wait for 3s */
+  wait_ms(3000);
+
+  /* Request motor 2 to make a soft stop */
+  printf("--> Request motor2 to stop softly\r\n");
+  motor2->SoftStop();
+  
+  /* Wait for motor 2 end moving */
+  motor2->WaitWhileActive();  
+
+  /* Request motor 1 and 3 to make a hard stop */
+  printf("--> Request motor1 and motor3 to stop immediately\r\n");
+  motor1->QueueCommands(POWERSTEP01_HARD_STOP,0);
+  motor2->QueueCommands(POWERSTEP01_NOP,0);
+  motor3->QueueCommands(POWERSTEP01_HARD_STOP,0);
+  motor1->SendQueuedCommands();
+
+  /* Wait for both motors end moving */  
+  WaitForAllDevicesNotBusy();
+  
+  /* Request all motors to go home and print to the console */
+  printf("--> Request all motors to go home.\r\n");
+  motor1->QueueCommands(POWERSTEP01_GO_HOME,0);
+  motor2->QueueCommands(POWERSTEP01_GO_HOME,0);
+  motor3->QueueCommands(POWERSTEP01_GO_HOME,0);
+  motor1->SendQueuedCommands();
+    
+  /* Wait for all device ends moving */ 
+  WaitForAllDevicesNotBusy();
+
+  /* Get acceleration, deceleration, Maxspeed and MinSpeed of motor 1*/
+  myMaxSpeed= motor1->GetParameter(POWERSTEP01_MAX_SPEED);
+  myAcceleration = motor1->GetParameter(POWERSTEP01_ACC);
+  myDeceleration = motor1->GetParameter(POWERSTEP01_DEC);
+  myMinSpeed  = motor1->GetParameter(POWERSTEP01_MIN_SPEED); 
+  
+  /* Select 1/16 microstepping mode for motor 1 */
+  printf("    Set 1/16 microstepping mode for motor1.\r\n"); 
+  motor1->SetStepMode(STEP_MODE_1_16);
+  
+  /* Select 1/8 microstepping mode for motor 2 */
+  printf("    Set 1/8 microstepping mode for motor2.\r\n"); 
+  motor2->SetStepMode(STEP_MODE_1_8);
+  
+  /* Set speed and acceleration of motor 2 */
+  /* Do not scale with microstepping mode */
+  motor2->SetParameter(POWERSTEP01_ACC, myAcceleration);
+  motor2->SetParameter(POWERSTEP01_DEC, myDeceleration);
+  motor2->SetParameter(POWERSTEP01_MIN_SPEED, myMinSpeed);
+  motor2->SetParameter(POWERSTEP01_MAX_SPEED, myMaxSpeed);
+  
+  /* Select ful step mode for motor 3 */
+  printf("    Set ful step mode for motor3.\r\n"); 
+  motor3->SetStepMode(STEP_MODE_FULL);
+
+  /* Set speed and acceleration of motor 3 */
+  /* Do not scale with microstepping mode */
+  motor3->SetParameter(POWERSTEP01_ACC, myAcceleration);
+  motor3->SetParameter(POWERSTEP01_DEC, myDeceleration);
+  motor3->SetParameter(POWERSTEP01_MIN_SPEED, myMinSpeed);
+  motor3->SetParameter(POWERSTEP01_MAX_SPEED, myMaxSpeed);
+
+  /* Printing to the console. */
+  printf("--> Infinite Loop...\r\n"); 
+  /* Infinite loop */
+  while(1)
+  {
+    /* motor 1 is using 1/16 microstepping mode */ 
+    /* motor 2 is using 1/8 microstepping mode */
+    /* motor 3 is using full step mode */
+    /* position is in microsteps */
+    motor1->QueueCommands(POWERSTEP01_GO_TO,-3200);
+    motor2->QueueCommands(POWERSTEP01_GO_TO,1600);
+    motor3->QueueCommands(POWERSTEP01_GO_TO,-200);
+    motor1->SendQueuedCommands();
+    
+    /* Wait for all device ends moving */ 
+    WaitForAllDevicesNotBusy();
+
+    motor1->QueueCommands(POWERSTEP01_GO_TO,3200);
+    motor2->QueueCommands(POWERSTEP01_GO_TO,-1600);
+    motor3->QueueCommands(POWERSTEP01_GO_TO,200);
+    motor1->SendQueuedCommands();
+    
+    /* Wait for all device ends moving */ 
+    WaitForAllDevicesNotBusy();
+  }
+}
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/