Diff: main.cpp

Revision:

5:05b73855a2e1

Parent:

2:c29a38e427f6

--- a/main.cpp	Thu Apr 14 09:01:03 2016 +0000
+++ b/main.cpp	Thu Oct 18 13:41:14 2018 +0000
@@ -47,13 +47,13 @@
 #include "DevSPI.h"
 
 /* Component specific header files. */
-#include "powerstep01_class.h"
+#include "PowerStep01.h"
 
 /* Variables -----------------------------------------------------------------*/
 
 /* Initialization parameters of the motor connected to the expansion board. */
 /* Current mode. */
-powerstep01_Init_u_t initDeviceParameters =
+powerstep01_init_u_t init_device_parameters =
 {
   /* common parameters */
   .cm.cp.cmVmSelection = POWERSTEP01_CM_VM_CURRENT, // enum powerstep01_CmVm_t
@@ -101,9 +101,9 @@
 };
 
 /* Motor Control Component. */
-POWERSTEP01 *motor1;
-POWERSTEP01 *motor2;
-POWERSTEP01 *motor3;
+PowerStep01 *motor1;
+PowerStep01 *motor2;
+PowerStep01 *motor3;
 
 /* Functions -----------------------------------------------------------------*/
 
@@ -112,28 +112,28 @@
  * @param  None
  * @retval None
  * @note   If needed, implement it, and then attach and enable it:
- *           + motor->AttachFlagIRQ(&myFlagIRQHandler);
- *           + motor->EnableFlagIRQ();
+ *           + motor->attach_flag_irq(&myFlagIRQHandler);
+ *           + motor->enable_flag_irq();
  *         To disable it:
  *           + motor->DisbleFlagIRQ();
  */
 void myFlagIRQHandler(void)
 {
   /* Set ISR flag. */
-  POWERSTEP01::isrFlag = TRUE;
+  PowerStep01::isrFlag = TRUE;
 
-  motor1->FetchAndClearAllStatus();
-  POWERSTEP01 *motor;
+  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::GetNbDevices();loop++)
+  for (uint8_t loop = 0; loop<PowerStep01::get_nb_devices();loop++)
   {
     if (loop==1) motor = motor2;
     if (loop==2) motor = motor3;
-    statusRegister = motor->GetFetchedStatus();
+    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)
@@ -264,7 +264,7 @@
     }
   } 
   /* Reset ISR flag. */
-  POWERSTEP01::isrFlag = FALSE;
+  PowerStep01::isrFlag = FALSE;
 }
 
 /**
@@ -272,18 +272,18 @@
  * @param  None
  * @retval None
  * @note   If needed, implement it, and then attach and enable it:
- *           + motor->AttachBusyIRQ(&myBusyIRQHandler);
- *           + motor->EnableBusyIRQ();
+ *           + motor->attach_busy_irq(&myBusyIRQHandler);
+ *           + motor->enable_busy_irq();
  *         To disable it:
  *           + motor->DisbleBusyIRQ();
  */
 void myBusyIRQHandler(void)
 {
   /* Set ISR flag. */
-  POWERSTEP01::isrFlag = TRUE;
+  PowerStep01::isrFlag = TRUE;
   
   /* Reset ISR flag. */
-  POWERSTEP01::isrFlag = FALSE;
+  PowerStep01::isrFlag = FALSE;
 }
 
 /**
@@ -291,7 +291,7 @@
  * @param[in] error Number of the error
  * @retval None
  * @note   If needed, implement it, and then attach it:
- *           + motor->AttachErrorHandler(&myErrorHandler);
+ *           + motor->attach_error_handler(&myErrorHandler);
  */
 void myErrorHandler(uint16_t error)
 {
@@ -304,10 +304,10 @@
   }    
 }
 
-void WaitForAllDevicesNotBusy(void)
+void wait_for_all_devices_not_busy(void)
 {
   /* Wait while at least one is active */
-  while (motor1->IsDeviceBusy()|motor2->IsDeviceBusy()|motor3->IsDeviceBusy());
+  while (motor1->is_device_busy()|motor2->is_device_busy()|motor3->is_device_busy());
 }
 
 /* Main ----------------------------------------------------------------------*/
@@ -334,44 +334,44 @@
   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);
+  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->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();
+  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->AttachErrorHandler(&myErrorHandler);
-  motor2->AttachErrorHandler(&myErrorHandler);
-  motor3->AttachErrorHandler(&myErrorHandler);
+  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->GoTo(3200);
+  motor1->go_to(3200);
 
   /* Wait for motor 2 ends moving */  
-  motor1->WaitWhileActive();
+  motor1->wait_while_active();
 
   /* Get current position of motor 1 and print to the console */
-  pos = motor1->GetPosition();
+  pos = motor1->get_position();
   printf("    Motor1 position: %d.\r\n", pos);
   
   /* Wait for 2 seconds */  
@@ -383,18 +383,18 @@
   {  
     /* Set current position of motor 1 to be its mark position*/
     printf("    Set mark to current position of motor1.\r\n");     
-    motor1->SetMark();
+    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->GoTo(pos); 
+    motor2->go_to(pos); 
 
     /* Wait for  motor 2 ends moving */  
-    motor2->WaitWhileActive();
+    motor2->wait_while_active();
   }
   
   /* Get current position of motor 2 and print to the console */
-  pos = motor2->GetPosition();
+  pos = motor2->get_position();
   printf("    Motor2 position: %d.\r\n", pos);
 
   /* If the read position of motor 2 is 3200 */
@@ -403,14 +403,14 @@
   {
     /* 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); 
+    motor3->go_to(pos); 
   
     /* Wait for motor 3 ends moving */  
-    motor3->WaitWhileActive();
+    motor3->wait_while_active();
   }
 
   /* Get current position of motor 3 and print to the console */
-  pos = motor3->GetPosition();
+  pos = motor3->get_position();
   printf("    Motor3 position: %d.\r\n", pos);
 
   /* Wait for 1s */
@@ -420,13 +420,13 @@
   {
     /* 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();
+    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 */ 
-    WaitForAllDevicesNotBusy();
+    wait_for_all_devices_not_busy();
   }
   
   /* Wait for 1s */
@@ -434,13 +434,13 @@
   
   /* 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);  
+  motor1->go_to(-3200);  
 
   /* Wait for motor 1 ends moving */  
-  motor1->WaitWhileActive();
+  motor1->wait_while_active();
 
   /* Get current position of motor 1 and print to the console */
-  pos = motor1->GetPosition();
+  pos = motor1->get_position();
   printf("    Motor1 position: %d.\r\n", pos);
     
   /* If the read position of motor 1 is -3200 */
@@ -449,14 +449,14 @@
   {
     /* 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); 
+    motor2->go_to(pos); 
 
     /* Wait for  motor 2 ends moving */  
-    motor2->WaitWhileActive();
+    motor2->wait_while_active();
   }
   
   /* Get current position of motor 2 and print to the console */
-  pos = motor2->GetPosition();
+  pos = motor2->get_position();
   printf("    Motor2 position: %d.\r\n", pos);
 
   /* If the read position of motor 2 is -3200 */
@@ -465,14 +465,14 @@
   {
     /* 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); 
+    motor3->go_to(pos); 
   
     /* Wait for motor 3 ends moving */  
-    motor3->WaitWhileActive();
+    motor3->wait_while_active();
   }
 
   /* Get current position of motor 3 and print to the console */
-  pos = motor3->GetPosition();
+  pos = motor3->get_position();
   printf("    Motor3 position: %d.\r\n", pos);
 
   /* Wait for 1s */
@@ -482,17 +482,17 @@
   {
     /* Set current position of motor 3 to be its mark position*/
     printf("    Set mark to current position of motor3.\r\n");
-    motor3->SetMark();
+    motor3->set_mark();
     
     /* 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();
+    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 */ 
-    WaitForAllDevicesNotBusy();
+    wait_for_all_devices_not_busy();
   }
 
   /* Wait for 1s */
@@ -500,34 +500,34 @@
   
   /* 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();
+  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 */ 
-  WaitForAllDevicesNotBusy();
+  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);
+  motor1->run(StepperMotor::FWD, 400);
 
   /* Wait for device to reach the targeted speed */
-  while((motor1->ReadStatusRegister() & POWERSTEP01_STATUS_MOT_STATUS)!=
+  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->GetSpeed();
+    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->GetAnalogValue(POWERSTEP01_SPEED); 
+  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);
 
@@ -536,24 +536,24 @@
   /* 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();  
+  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->ReadStatusRegister() & POWERSTEP01_STATUS_MOT_STATUS)!=
+  while(((motor2->read_status_register() & POWERSTEP01_STATUS_MOT_STATUS)!=
         POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD)||
-        ((motor3->ReadStatusRegister() & POWERSTEP01_STATUS_MOT_STATUS)!=
+        ((motor3->read_status_register() & POWERSTEP01_STATUS_MOT_STATUS)!=
         POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD));
  
   /* Record the reached speed in step/s */
-  floatValue = motor2->GetAnalogValue(POWERSTEP01_SPEED); 
+  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->GetAnalogValue(POWERSTEP01_SPEED); 
+  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); 
   
@@ -562,62 +562,62 @@
 
   /* Request motor 2 to make a soft stop */
   printf("--> Request motor2 to stop softly\r\n");
-  motor2->SoftStop();
+  motor2->soft_stop();
   
   /* Wait for motor 2 end moving */
-  motor2->WaitWhileActive();  
+  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->QueueCommands(POWERSTEP01_HARD_STOP,0);
-  motor2->QueueCommands(POWERSTEP01_NOP,0);
-  motor3->QueueCommands(POWERSTEP01_HARD_STOP,0);
-  motor1->SendQueuedCommands();
+  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 */  
-  WaitForAllDevicesNotBusy();
+  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->QueueCommands(POWERSTEP01_GO_HOME,0);
-  motor2->QueueCommands(POWERSTEP01_GO_HOME,0);
-  motor3->QueueCommands(POWERSTEP01_GO_HOME,0);
-  motor1->SendQueuedCommands();
+  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 */ 
-  WaitForAllDevicesNotBusy();
+  wait_for_all_devices_not_busy();
 
   /* Get acceleration, deceleration, Maxspeed and MinSpeed of motor 1*/
-  myMaxSpeed= motor1->GetRawParameter(POWERSTEP01_MAX_SPEED);
-  myAcceleration = motor1->GetRawParameter(POWERSTEP01_ACC);
-  myDeceleration = motor1->GetRawParameter(POWERSTEP01_DEC);
-  myMinSpeed  = motor1->GetRawParameter(POWERSTEP01_MIN_SPEED); 
+  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->SetStepMode(StepperMotor::STEP_MODE_1_16);
+  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->SetStepMode(StepperMotor::STEP_MODE_1_8);
+  motor2->set_step_mode(StepperMotor::STEP_MODE_1_8);
   
   /* Set speed and acceleration of motor 2 */
   /* Do not scale with microstepping mode */
-  motor2->SetRawParameter(POWERSTEP01_ACC, myAcceleration);
-  motor2->SetRawParameter(POWERSTEP01_DEC, myDeceleration);
-  motor2->SetRawParameter(POWERSTEP01_MIN_SPEED, myMinSpeed);
-  motor2->SetRawParameter(POWERSTEP01_MAX_SPEED, myMaxSpeed);
+  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->SetStepMode(StepperMotor::STEP_MODE_FULL);
+  motor3->set_step_mode(StepperMotor::STEP_MODE_FULL);
 
   /* Set speed and acceleration of motor 3 */
   /* Do not scale with microstepping mode */
-  motor3->SetRawParameter(POWERSTEP01_ACC, myAcceleration);
-  motor3->SetRawParameter(POWERSTEP01_DEC, myDeceleration);
-  motor3->SetRawParameter(POWERSTEP01_MIN_SPEED, myMinSpeed);
-  motor3->SetRawParameter(POWERSTEP01_MAX_SPEED, myMaxSpeed);
+  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"); 
@@ -628,21 +628,21 @@
     /* 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();
+    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 */ 
-    WaitForAllDevicesNotBusy();
+    wait_for_all_devices_not_busy();
 
-    motor1->QueueCommands(POWERSTEP01_GO_TO,3200);
-    motor2->QueueCommands(POWERSTEP01_GO_TO,-1600);
-    motor3->QueueCommands(POWERSTEP01_GO_TO,200);
-    motor1->SendQueuedCommands();
+    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 */ 
-    WaitForAllDevicesNotBusy();
+    wait_for_all_devices_not_busy();
   }
 }
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/