File content as of revision 5:05b73855a2e1:

/**
 ******************************************************************************
 * @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.h"

/* Variables -----------------------------------------------------------------*/

/* Initialization parameters of the motor connected to the expansion board. */
/* Current mode. */
powerstep01_init_u_t init_device_parameters =
{
  /* 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->attach_flag_irq(&myFlagIRQHandler);
 *           + motor->enable_flag_irq();
 *         To disable it:
 *           + motor->DisbleFlagIRQ();
 */
void myFlagIRQHandler(void)
{
  /* Set ISR flag. */
  PowerStep01::isrFlag = TRUE;

  motor1->fetch_and_clear_all_status();
  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::get_nb_devices();loop++)
  {
    if (loop==1) motor = motor2;
    if (loop==2) motor = motor3;
    statusRegister = motor->get_fetched_status();
    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");
       printf("    Expected with default IHM03A1 HW configuration.\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->attach_busy_irq(&myBusyIRQHandler);
 *           + motor->enable_busy_irq();
 *         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
 * @note   If needed, implement it, and then attach it:
 *           + motor->attach_error_handler(&myErrorHandler);
 */
void myErrorHandler(uint16_t error)
{
  /* Printing to the console. */
  printf("Error %d detected\r\n\n", error);
  
  /* Infinite loop */
  while(1)
  {
  }    
}

void wait_for_all_devices_not_busy(void)
{
  /* Wait while at least one is active */
  while (motor1->is_device_busy()|motor2->is_device_busy()|motor3->is_device_busy());
}

/* 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(&init_device_parameters) != COMPONENT_OK) exit(EXIT_FAILURE);
  if (motor2->init(&init_device_parameters) != COMPONENT_OK) exit(EXIT_FAILURE);
  if (motor3->init(&init_device_parameters) != COMPONENT_OK) exit(EXIT_FAILURE);

  /* Attaching and enabling interrupt handlers. */
  motor1->attach_flag_irq(&myFlagIRQHandler);
  motor1->enable_flag_irq();
  motor1->attach_busy_irq(&myBusyIRQHandler);
  motor1->enable_busy_irq();
  motor2->attach_flag_irq(&myFlagIRQHandler);
  motor2->enable_flag_irq();
  motor2->attach_busy_irq(&myBusyIRQHandler);
  motor2->enable_busy_irq();
  motor3->attach_flag_irq(&myFlagIRQHandler);
  motor3->enable_flag_irq();
  motor3->attach_busy_irq(&myBusyIRQHandler);
  motor3->enable_busy_irq();
  
  /* Attaching an error handler */
  motor1->attach_error_handler(&myErrorHandler);
  motor2->attach_error_handler(&myErrorHandler);
  motor3->attach_error_handler(&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->go_to(3200);

  /* Wait for motor 2 ends moving */  
  motor1->wait_while_active();

  /* Get current position of motor 1 and print to the console */
  pos = motor1->get_position();
  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->set_mark();
  
    /* 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->go_to(pos); 

    /* Wait for  motor 2 ends moving */  
    motor2->wait_while_active();
  }
  
  /* Get current position of motor 2 and print to the console */
  pos = motor2->get_position();
  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->go_to(pos); 
  
    /* Wait for motor 3 ends moving */  
    motor3->wait_while_active();
  }

  /* Get current position of motor 3 and print to the console */
  pos = motor3->get_position();
  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->queue_commands(POWERSTEP01_GO_HOME,0);
    motor2->queue_commands(POWERSTEP01_GO_HOME,0);
    motor3->queue_commands(POWERSTEP01_GO_HOME,0);
    motor1->send_queued_commands();

    /* Wait for all motors ends moving */ 
    wait_for_all_devices_not_busy();
  }
  
  /* 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->go_to(-3200);  

  /* Wait for motor 1 ends moving */  
  motor1->wait_while_active();

  /* Get current position of motor 1 and print to the console */
  pos = motor1->get_position();
  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->go_to(pos); 

    /* Wait for  motor 2 ends moving */  
    motor2->wait_while_active();
  }
  
  /* Get current position of motor 2 and print to the console */
  pos = motor2->get_position();
  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->go_to(pos); 
  
    /* Wait for motor 3 ends moving */  
    motor3->wait_while_active();
  }

  /* Get current position of motor 3 and print to the console */
  pos = motor3->get_position();
  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->set_mark();
    
    /* Request all motors to go home and print to the console */
    printf("--> Request all motors to go home.\r\n");
    motor1->queue_commands(POWERSTEP01_GO_HOME,0);
    motor2->queue_commands(POWERSTEP01_GO_HOME,0);
    motor3->queue_commands(POWERSTEP01_GO_HOME,0);
    motor1->send_queued_commands();
    
    /* Wait for all device ends moving */ 
    wait_for_all_devices_not_busy();
  }

  /* 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->queue_commands(POWERSTEP01_GO_MARK,0);
  motor2->queue_commands(POWERSTEP01_NOP,0);
  motor3->queue_commands(POWERSTEP01_GO_MARK,0);
  motor1->send_queued_commands();
 
  /* Wait for motor 1 and 2 ends moving */ 
  wait_for_all_devices_not_busy();
  
  /* 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->read_status_register() & POWERSTEP01_STATUS_MOT_STATUS)!=
        POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD)
  {
    /* Record the reached speed in step/s rounded to integer */
    unsignedIntegerValue = motor1->get_speed();
    /* 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->get_analog_value(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->queue_commands(POWERSTEP01_NOP,0);
  motor2->queue_commands((uint8_t)POWERSTEP01_RUN|(uint8_t)StepperMotor::FWD,PowerStep01::speed_steps_s_to_reg_val(300));
  motor3->queue_commands((uint8_t)POWERSTEP01_RUN|(uint8_t)StepperMotor::FWD,PowerStep01::speed_steps_s_to_reg_val(200));
  motor1->send_queued_commands();  
 
  /* Wait for device to reach the targeted speed */
  while(((motor2->read_status_register() & POWERSTEP01_STATUS_MOT_STATUS)!=
        POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD)||
        ((motor3->read_status_register() & POWERSTEP01_STATUS_MOT_STATUS)!=
        POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD));
 
  /* Record the reached speed in step/s */
  floatValue = motor2->get_analog_value(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->get_analog_value(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->soft_stop();
  
  /* Wait for motor 2 end moving */
  motor2->wait_while_active();  

  /* Request motor 1 and 3 to make a hard stop */
  printf("--> Request motor1 and motor3 to stop immediately\r\n");
  motor1->queue_commands(POWERSTEP01_HARD_STOP,0);
  motor2->queue_commands(POWERSTEP01_NOP,0);
  motor3->queue_commands(POWERSTEP01_HARD_STOP,0);
  motor1->send_queued_commands();

  /* Wait for both motors end moving */  
  wait_for_all_devices_not_busy();
  
  /* Request all motors to go home and print to the console */
  printf("--> Request all motors to go home.\r\n");
  motor1->queue_commands(POWERSTEP01_GO_HOME,0);
  motor2->queue_commands(POWERSTEP01_GO_HOME,0);
  motor3->queue_commands(POWERSTEP01_GO_HOME,0);
  motor1->send_queued_commands();
    
  /* Wait for all device ends moving */ 
  wait_for_all_devices_not_busy();

  /* Get acceleration, deceleration, Maxspeed and MinSpeed of motor 1*/
  myMaxSpeed= motor1->get_raw_parameter(POWERSTEP01_MAX_SPEED);
  myAcceleration = motor1->get_raw_parameter(POWERSTEP01_ACC);
  myDeceleration = motor1->get_raw_parameter(POWERSTEP01_DEC);
  myMinSpeed = motor1->get_raw_parameter(POWERSTEP01_MIN_SPEED); 
  
  /* Select 1/16 microstepping mode for motor 1 */
  printf("    Set 1/16 microstepping mode for motor1.\r\n"); 
  motor1->set_step_mode(StepperMotor::STEP_MODE_1_16);
  
  /* Select 1/8 microstepping mode for motor 2 */
  printf("    Set 1/8 microstepping mode for motor2.\r\n"); 
  motor2->set_step_mode(StepperMotor::STEP_MODE_1_8);
  
  /* Set speed and acceleration of motor 2 */
  /* Do not scale with microstepping mode */
  motor2->set_raw_parameter(POWERSTEP01_ACC, myAcceleration);
  motor2->set_raw_parameter(POWERSTEP01_DEC, myDeceleration);
  motor2->set_raw_parameter(POWERSTEP01_MIN_SPEED, myMinSpeed);
  motor2->set_raw_parameter(POWERSTEP01_MAX_SPEED, myMaxSpeed);
  
  /* Select ful step mode for motor 3 */
  printf("    Set ful step mode for motor3.\r\n"); 
  motor3->set_step_mode(StepperMotor::STEP_MODE_FULL);

  /* Set speed and acceleration of motor 3 */
  /* Do not scale with microstepping mode */
  motor3->set_raw_parameter(POWERSTEP01_ACC, myAcceleration);
  motor3->set_raw_parameter(POWERSTEP01_DEC, myDeceleration);
  motor3->set_raw_parameter(POWERSTEP01_MIN_SPEED, myMinSpeed);
  motor3->set_raw_parameter(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->queue_commands(POWERSTEP01_GO_TO,-3200);
    motor2->queue_commands(POWERSTEP01_GO_TO,1600);
    motor3->queue_commands(POWERSTEP01_GO_TO,-200);
    motor1->send_queued_commands();
    
    /* Wait for all device ends moving */ 
    wait_for_all_devices_not_busy();

    motor1->queue_commands(POWERSTEP01_GO_TO,3200);
    motor2->queue_commands(POWERSTEP01_GO_TO,-1600);
    motor3->queue_commands(POWERSTEP01_GO_TO,200);
    motor1->send_queued_commands();
    
    /* Wait for all device ends moving */ 
    wait_for_all_devices_not_busy();
  }
}
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/