main.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    main.cpp
00004  * @author  IPC Rennes
00005  * @version V1.0.0
00006  * @date    March 18th, 2016
00007  * @brief   mbed test application for the STMicroelectronics X-NUCLEO-IHM03A1
00008  *          Motor Control Expansion Boards: control of 3 motors with 3
00009  *          expansions boards.
00010  ******************************************************************************
00011  * @attention
00012  *
00013  * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
00014  *
00015  * Redistribution and use in source and binary forms, with or without modification,
00016  * are permitted provided that the following conditions are met:
00017  *   1. Redistributions of source code must retain the above copyright notice,
00018  *      this list of conditions and the following disclaimer.
00019  *   2. Redistributions in binary form must reproduce the above copyright notice,
00020  *      this list of conditions and the following disclaimer in the documentation
00021  *      and/or other materials provided with the distribution.
00022  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00023  *      may be used to endorse or promote products derived from this software
00024  *      without specific prior written permission.
00025  *
00026  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00027  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00028  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00029  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00030  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00031  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00032  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00033  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00034  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00035  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  ******************************************************************************
00038  */
00039 
00040 
00041 /* Includes ------------------------------------------------------------------*/
00042 
00043 /* mbed specific header files. */
00044 #include "mbed.h"
00045 
00046 /* Helper header files. */
00047 #include "DevSPI.h"
00048 
00049 /* Component specific header files. */
00050 #include "PowerStep01.h"
00051 
00052 /* Variables -----------------------------------------------------------------*/
00053 
00054 /* Initialization parameters of the motor connected to the expansion board. */
00055 /* Current mode. */
00056 powerstep01_init_u_t init_device_parameters =
00057 {
00058   /* common parameters */
00059   .cm.cp.cmVmSelection = POWERSTEP01_CM_VM_CURRENT, // enum powerstep01_CmVm_t
00060   582, // Acceleration rate in step/s2, range 14.55 to 59590 steps/s^2
00061   582, // Deceleration rate in step/s2, range 14.55 to 59590 steps/s^2
00062   488, // Maximum speed in step/s, range 15.25 to 15610 steps/s
00063   0, // Minimum speed in step/s, range 0 to 976.3 steps/s
00064   POWERSTEP01_LSPD_OPT_OFF, // Low speed optimization bit, enum powerstep01_LspdOpt_t
00065   244.16, // Full step speed in step/s, range 7.63 to 15625 steps/s
00066   POWERSTEP01_BOOST_MODE_OFF, // Boost of the amplitude square wave, enum powerstep01_BoostMode_t
00067   281.25, // Overcurrent threshold settings via enum powerstep01_OcdTh_t
00068   STEP_MODE_1_16, // Step mode settings via enum motorStepMode_t
00069   POWERSTEP01_SYNC_SEL_DISABLED, // Synch. Mode settings via enum powerstep01_SyncSel_t
00070   (POWERSTEP01_ALARM_EN_OVERCURRENT|
00071    POWERSTEP01_ALARM_EN_THERMAL_SHUTDOWN|
00072    POWERSTEP01_ALARM_EN_THERMAL_WARNING|
00073    POWERSTEP01_ALARM_EN_UVLO|
00074    POWERSTEP01_ALARM_EN_STALL_DETECTION|
00075    POWERSTEP01_ALARM_EN_SW_TURN_ON|
00076    POWERSTEP01_ALARM_EN_WRONG_NPERF_CMD), // Alarm settings via bitmap enum powerstep01_AlarmEn_t
00077   POWERSTEP01_IGATE_64mA, // Gate sink/source current via enum powerstep01_Igate_t 
00078   POWERSTEP01_TBOOST_0ns, // Duration of the overboost phase during gate turn-off via enum powerstep01_Tboost_t
00079   POWERSTEP01_TCC_500ns, // Controlled current time via enum powerstep01_Tcc_t
00080   POWERSTEP01_WD_EN_DISABLE, // External clock watchdog, enum powerstep01_WdEn_t  
00081   POWERSTEP01_TBLANK_375ns, // Duration of the blanking time via enum powerstep01_TBlank_t
00082   POWERSTEP01_TDT_125ns, // Duration of the dead time via enum powerstep01_Tdt_t
00083   /* current mode parameters */
00084   328.12, // Hold torque in mV, range from 7.8mV to 1000 mV
00085   328.12, // Running torque in mV, range from 7.8mV to 1000 mV 
00086   328.12, // Acceleration torque in mV, range from 7.8mV to 1000 mV
00087   328.12, // Deceleration torque in mV, range from 7.8mV to 1000 mV
00088   POWERSTEP01_TOFF_FAST_8us, //Maximum fast decay time , enum powerstep01_ToffFast_t 
00089   POWERSTEP01_FAST_STEP_12us, //Maximum fall step time , enum powerstep01_FastStep_t 
00090   3.0, // Minimum on-time in us, range 0.5us to 64us
00091   21.0, // Minimum off-time in us, range 0.5us to 64us 
00092   POWERSTEP01_CONFIG_INT_16MHZ_OSCOUT_2MHZ, // Clock setting , enum powerstep01_ConfigOscMgmt_t
00093   POWERSTEP01_CONFIG_SW_HARD_STOP, // External switch hard stop interrupt mode, enum powerstep01_ConfigSwMode_t
00094   POWERSTEP01_CONFIG_TQ_REG_TVAL_USED, // External torque regulation enabling , enum powerstep01_ConfigEnTqReg_t
00095   POWERSTEP01_CONFIG_VS_COMP_DISABLE, // Motor Supply Voltage Compensation enabling , enum powerstep01_ConfigEnVscomp_t 
00096   POWERSTEP01_CONFIG_OC_SD_DISABLE, // Over current shutwdown enabling, enum powerstep01_ConfigOcSd_t
00097   POWERSTEP01_CONFIG_UVLOVAL_LOW, // UVLO Threshold via powerstep01_ConfigUvLoVal_t
00098   POWERSTEP01_CONFIG_VCCVAL_15V, // VCC Val, enum powerstep01_ConfigVccVal_t
00099   POWERSTEP01_CONFIG_TSW_048us, // Switching period, enum powerstep01_ConfigTsw_t
00100   POWERSTEP01_CONFIG_PRED_DISABLE // Predictive current enabling , enum powerstep01_ConfigPredEn_t 
00101 };
00102 
00103 /* Motor Control Component. */
00104 PowerStep01 *motor1;
00105 PowerStep01 *motor2;
00106 PowerStep01 *motor3;
00107 
00108 /* Functions -----------------------------------------------------------------*/
00109 
00110 /**
00111  * @brief  This is an example of user handler for the flag interrupt.
00112  * @param  None
00113  * @retval None
00114  * @note   If needed, implement it, and then attach and enable it:
00115  *           + motor->attach_flag_irq(&myFlagIRQHandler);
00116  *           + motor->enable_flag_irq();
00117  *         To disable it:
00118  *           + motor->DisbleFlagIRQ();
00119  */
00120 void myFlagIRQHandler(void)
00121 {
00122   /* Set ISR flag. */
00123   PowerStep01::isrFlag = TRUE;
00124 
00125   motor1->fetch_and_clear_all_status();
00126   PowerStep01 *motor;
00127   motor = motor1;
00128   unsigned int statusRegister;
00129   
00130   printf("    WARNING: \"FLAG\" interrupt triggered.\r\n");
00131   /* Get the value of the status register. */
00132   for (uint8_t loop = 0; loop<PowerStep01::get_nb_devices();loop++)
00133   {
00134     if (loop==1) motor = motor2;
00135     if (loop==2) motor = motor3;
00136     statusRegister = motor->get_fetched_status();
00137     printf("    Motor%d:\r\n",loop+1);
00138     /* Check HIZ flag: if set, power brigdes are disabled */
00139     if ((statusRegister & POWERSTEP01_STATUS_HIZ)==POWERSTEP01_STATUS_HIZ)
00140     {
00141       // HIZ state
00142       printf("    HiZ state.\r\n");
00143     }
00144     /* Check BUSY flag: if not set, a command is under execution */
00145     if ((statusRegister & POWERSTEP01_STATUS_BUSY)==0)
00146     {
00147       // BUSY
00148       printf("    Busy.\r\n");
00149     }
00150     /* Check SW_F flag: if not set, the SW input is opened */
00151     if ((statusRegister & POWERSTEP01_STATUS_SW_F )!=0)
00152     {
00153        // SW closed (connected to ground)
00154        printf("    SW closed (connected to ground).\r\n");
00155     }
00156     /* Check SW_EN bit */
00157     if ((statusRegister & POWERSTEP01_STATUS_SW_EVN)==
00158         POWERSTEP01_STATUS_SW_EVN)
00159     {
00160       // SW turn_on event
00161        printf("    SW turn_on event.\r\n");
00162     }
00163     if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)==
00164         POWERSTEP01_STATUS_MOT_STATUS_STOPPED)
00165     {
00166       // MOTOR STOPPED
00167       printf("    Stopped.\r\n");
00168     }
00169     else
00170     {
00171       if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)==
00172                 POWERSTEP01_STATUS_MOT_STATUS_ACCELERATION)
00173       {
00174         // MOTOR ACCELERATION
00175         printf("    Accelerating ");
00176       }  
00177       else  if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)==
00178                 POWERSTEP01_STATUS_MOT_STATUS_DECELERATION)
00179       {
00180         // MOTOR DECELERATION
00181         printf("    Decelerating ");
00182       }
00183       else  if ((statusRegister & POWERSTEP01_STATUS_MOT_STATUS)==
00184                 POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD)
00185       {
00186         // MOTOR RUNNING AT CONSTANT SPEED
00187         printf("    Steady running ");        
00188       } 
00189       /* Check direction bit */
00190       if ((statusRegister & POWERSTEP01_STATUS_DIR)==0)
00191       {
00192       // StepperMotor::BWD
00193        printf(" in backward direction.\r\n");
00194       }
00195       else  
00196       {
00197         // StepperMotor::FWD
00198         printf(" in forward direction.\r\n");
00199       }
00200     } 
00201     /* Check Command Error flag: if set, the command received by SPI can't be */
00202     /* performed. This occurs for instance when a move command is sent to the */
00203     /* Powerstep01 while it is already running */
00204     if ((statusRegister & POWERSTEP01_STATUS_CMD_ERROR)==
00205         POWERSTEP01_STATUS_CMD_ERROR)
00206     {
00207       // Command Error
00208       printf("    Non-performable command detected.\r\n");
00209     }  
00210     /* Check Step mode clock flag: if set, the device is working in step clock mode */
00211     if ((statusRegister & POWERSTEP01_STATUS_STCK_MOD)==
00212         POWERSTEP01_STATUS_STCK_MOD)
00213     {
00214        //Step clock mode enabled
00215       printf("    Step clock mode enabled.\r\n");
00216     }  
00217     /* Check UVLO flag: if not set, there is an undervoltage lock-out */
00218     if ((statusRegister & POWERSTEP01_STATUS_UVLO)==0)
00219     {
00220        //Undervoltage lock-out
00221        printf("    undervoltage lock-out.\r\n"); 
00222     }  
00223     /* Check UVLO ADC flag: if not set, there is an ADC undervoltage lock-out */
00224     if ((statusRegister & POWERSTEP01_STATUS_UVLO_ADC)==0)
00225     {
00226        //ADC undervoltage lock-out
00227        printf("    ADC undervoltage lock-out:\r\n");
00228        printf("    Expected with default IHM03A1 HW configuration.\r\n");
00229     } 
00230     /* Check thermal STATUS flags: if  set, the thermal status is not normal */
00231     if ((statusRegister & POWERSTEP01_STATUS_TH_STATUS)!=0)
00232     {
00233       //thermal status: 1: Warning, 2: Bridge shutdown, 3: Device shutdown
00234       if (((statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11)==1)
00235       {
00236         printf("    Thermal status - Warning.\r\n");
00237       }
00238       else if (((statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11)==2)
00239       {
00240         printf("    Thermal status - Bridge shutdown.\r\n");
00241       }
00242       else if (((statusRegister & POWERSTEP01_STATUS_TH_STATUS)>>11)==3)
00243       {
00244         printf("    Thermal status - Device shutdown.\r\n");
00245       }
00246     }    
00247     /* Check OCD  flag: if not set, there is an overcurrent detection */
00248     if ((statusRegister & POWERSTEP01_STATUS_OCD)==0)
00249     {
00250       //Overcurrent detection
00251       printf("    Overcurrent detection.\r\n"); 
00252     }      
00253     /* Check STALL_A flag: if not set, there is a Stall condition on bridge A */
00254     if ((statusRegister & POWERSTEP01_STATUS_STALL_A)==0)
00255     {
00256       //Bridge A stalled
00257       printf("    Bridge A stalled.\r\n");   
00258     }    
00259     /* Check STALL_B flag: if not set, there is a Stall condition on bridge B */
00260     if ((statusRegister & POWERSTEP01_STATUS_STALL_B)==0)
00261     {
00262       //Bridge B stalled
00263       printf("    Bridge B stalled.\r\n");  
00264     }
00265   } 
00266   /* Reset ISR flag. */
00267   PowerStep01::isrFlag = FALSE;
00268 }
00269 
00270 /**
00271  * @brief  This is an example of user handler for the busy interrupt.
00272  * @param  None
00273  * @retval None
00274  * @note   If needed, implement it, and then attach and enable it:
00275  *           + motor->attach_busy_irq(&myBusyIRQHandler);
00276  *           + motor->enable_busy_irq();
00277  *         To disable it:
00278  *           + motor->DisbleBusyIRQ();
00279  */
00280 void myBusyIRQHandler(void)
00281 {
00282   /* Set ISR flag. */
00283   PowerStep01::isrFlag = TRUE;
00284   
00285   /* Reset ISR flag. */
00286   PowerStep01::isrFlag = FALSE;
00287 }
00288 
00289 /**
00290  * @brief  This is an example of error handler.
00291  * @param[in] error Number of the error
00292  * @retval None
00293  * @note   If needed, implement it, and then attach it:
00294  *           + motor->attach_error_handler(&myErrorHandler);
00295  */
00296 void myErrorHandler(uint16_t error)
00297 {
00298   /* Printing to the console. */
00299   printf("Error %d detected\r\n\n", error);
00300   
00301   /* Infinite loop */
00302   while(1)
00303   {
00304   }    
00305 }
00306 
00307 void wait_for_all_devices_not_busy(void)
00308 {
00309   /* Wait while at least one is active */
00310   while (motor1->is_device_busy()|motor2->is_device_busy()|motor3->is_device_busy());
00311 }
00312 
00313 /* Main ----------------------------------------------------------------------*/
00314 
00315 int main()
00316 {
00317   int32_t pos;
00318   uint32_t myMaxSpeed;
00319   uint32_t myMinSpeed;
00320   uint16_t myAcceleration;
00321   uint16_t myDeceleration;
00322   uint32_t unsignedIntegerValue;
00323   float floatValue;
00324   
00325   /* Printing to the console. */
00326   printf("STARTING MAIN PROGRAM\r\n");
00327   printf("    Reminder:\r\n");
00328   printf("    The position unit is in agreement to the step mode.\r\n");
00329   printf("    The speed, acceleration or deceleration unit\r\n");
00330   printf("    do not depend on the step mode and the step unit is a full step.\r\n");
00331     
00332 //----- Initialization 
00333   /* Initializing SPI bus. */
00334   DevSPI dev_spi(D11, D12, D13);
00335 
00336   /* Initializing Motor Control Component. */
00337   motor1 = new PowerStep01(D2, D4, D8, D9, D10, dev_spi);
00338   motor2 = new PowerStep01(D2, D4, D8, D9, D10, dev_spi);
00339   motor3 = new PowerStep01(D2, D4, D8, D9, D10, dev_spi);
00340   if (motor1->init(&init_device_parameters) != COMPONENT_OK) exit(EXIT_FAILURE);
00341   if (motor2->init(&init_device_parameters) != COMPONENT_OK) exit(EXIT_FAILURE);
00342   if (motor3->init(&init_device_parameters) != COMPONENT_OK) exit(EXIT_FAILURE);
00343 
00344   /* Attaching and enabling interrupt handlers. */
00345   motor1->attach_flag_irq(&myFlagIRQHandler);
00346   motor1->enable_flag_irq();
00347   motor1->attach_busy_irq(&myBusyIRQHandler);
00348   motor1->enable_busy_irq();
00349   motor2->attach_flag_irq(&myFlagIRQHandler);
00350   motor2->enable_flag_irq();
00351   motor2->attach_busy_irq(&myBusyIRQHandler);
00352   motor2->enable_busy_irq();
00353   motor3->attach_flag_irq(&myFlagIRQHandler);
00354   motor3->enable_flag_irq();
00355   motor3->attach_busy_irq(&myBusyIRQHandler);
00356   motor3->enable_busy_irq();
00357   
00358   /* Attaching an error handler */
00359   motor1->attach_error_handler(&myErrorHandler);
00360   motor2->attach_error_handler(&myErrorHandler);
00361   motor3->attach_error_handler(&myErrorHandler);
00362   
00363   /* Printing to the console. */
00364   printf("Motor Control Application Example for 3 Motors\r\n");
00365   
00366   /* Request motor 1 to go to position 3200 and print to the console */
00367   printf("--> Request motor1 to go to position 3200.\r\n");
00368   motor1->go_to(3200);
00369 
00370   /* Wait for motor 2 ends moving */  
00371   motor1->wait_while_active();
00372 
00373   /* Get current position of motor 1 and print to the console */
00374   pos = motor1->get_position();
00375   printf("    Motor1 position: %d.\r\n", pos);
00376   
00377   /* Wait for 2 seconds */  
00378   wait_ms(2000);
00379   
00380   /* If the read position of motor 1 is 3200 */
00381   /* Request motor 2 to go to the same position */
00382   if (pos == 3200)
00383   {  
00384     /* Set current position of motor 1 to be its mark position*/
00385     printf("    Set mark to current position of motor1.\r\n");     
00386     motor1->set_mark();
00387   
00388     /* Request motor 2 to Go to the same position and print to the console */
00389     printf("--> Request motor2 to go to position 3200.\r\n"); 
00390     motor2->go_to(pos); 
00391 
00392     /* Wait for  motor 2 ends moving */  
00393     motor2->wait_while_active();
00394   }
00395   
00396   /* Get current position of motor 2 and print to the console */
00397   pos = motor2->get_position();
00398   printf("    Motor2 position: %d.\r\n", pos);
00399 
00400   /* If the read position of motor 2 is 3200 */
00401   /* Request motor 3 to go to the same position */
00402   if (pos == 3200)
00403   {
00404     /* Request motor 3 to Go to the same position and print to the console */
00405     printf("--> Request motor3 to go to position 3200.\r\n");
00406     motor3->go_to(pos); 
00407   
00408     /* Wait for motor 3 ends moving */  
00409     motor3->wait_while_active();
00410   }
00411 
00412   /* Get current position of motor 3 and print to the console */
00413   pos = motor3->get_position();
00414   printf("    Motor3 position: %d.\r\n", pos);
00415 
00416   /* Wait for 1s */
00417   wait_ms(1000);
00418   
00419   if (pos == 3200)
00420   {
00421     /* Request all motors to go home and print to the console */
00422     printf("    Request all motors to go home.\r\n");
00423     motor1->queue_commands(POWERSTEP01_GO_HOME,0);
00424     motor2->queue_commands(POWERSTEP01_GO_HOME,0);
00425     motor3->queue_commands(POWERSTEP01_GO_HOME,0);
00426     motor1->send_queued_commands();
00427 
00428     /* Wait for all motors ends moving */ 
00429     wait_for_all_devices_not_busy();
00430   }
00431   
00432   /* Wait for 1s */
00433   wait_ms(1000);
00434   
00435   /* Request motor 1 to Goto position -3200 and print to the console */
00436   printf("--> Request motor1 to go to position -3200.\r\n"); 
00437   motor1->go_to(-3200);  
00438 
00439   /* Wait for motor 1 ends moving */  
00440   motor1->wait_while_active();
00441 
00442   /* Get current position of motor 1 and print to the console */
00443   pos = motor1->get_position();
00444   printf("    Motor1 position: %d.\r\n", pos);
00445     
00446   /* If the read position of motor 1 is -3200 */
00447   /* Request motor 2 to go to the same position */
00448   if (pos == -3200)
00449   {
00450     /* Request motor 2 to go to the same position and print to the console */
00451     printf("--> Request motor2 to go to position -3200.\r\n"); 
00452     motor2->go_to(pos); 
00453 
00454     /* Wait for  motor 2 ends moving */  
00455     motor2->wait_while_active();
00456   }
00457   
00458   /* Get current position of motor 2 and print to the console */
00459   pos = motor2->get_position();
00460   printf("    Motor2 position: %d.\r\n", pos);
00461 
00462   /* If the read position of motor 2 is -3200 */
00463   /* Request motor 3 to go to the same position */
00464   if (pos == -3200)
00465   {
00466     /* Request motor 3 to go to the same position and print to the console */
00467     printf("--> Request motor3 to go to position -3200.\r\n"); 
00468     motor3->go_to(pos); 
00469   
00470     /* Wait for motor 3 ends moving */  
00471     motor3->wait_while_active();
00472   }
00473 
00474   /* Get current position of motor 3 and print to the console */
00475   pos = motor3->get_position();
00476   printf("    Motor3 position: %d.\r\n", pos);
00477 
00478   /* Wait for 1s */
00479   wait_ms(1000);
00480   
00481   if (pos == -3200)
00482   {
00483     /* Set current position of motor 3 to be its mark position*/
00484     printf("    Set mark to current position of motor3.\r\n");
00485     motor3->set_mark();
00486     
00487     /* Request all motors to go home and print to the console */
00488     printf("--> Request all motors to go home.\r\n");
00489     motor1->queue_commands(POWERSTEP01_GO_HOME,0);
00490     motor2->queue_commands(POWERSTEP01_GO_HOME,0);
00491     motor3->queue_commands(POWERSTEP01_GO_HOME,0);
00492     motor1->send_queued_commands();
00493     
00494     /* Wait for all device ends moving */ 
00495     wait_for_all_devices_not_busy();
00496   }
00497 
00498   /* Wait for 1s */
00499   wait_ms(1000);
00500   
00501   /* Request motor 1 and motor 3 to go their mark position */
00502   printf("--> Request motor1 and motor3 to go to their marked position.\r\n");
00503   motor1->queue_commands(POWERSTEP01_GO_MARK,0);
00504   motor2->queue_commands(POWERSTEP01_NOP,0);
00505   motor3->queue_commands(POWERSTEP01_GO_MARK,0);
00506   motor1->send_queued_commands();
00507  
00508   /* Wait for motor 1 and 2 ends moving */ 
00509   wait_for_all_devices_not_busy();
00510   
00511   /* Wait for 1s */
00512   wait_ms(1000);
00513 
00514   /* Request motor 1 to run in StepperMotor::FWD direction at 400 steps/s*/
00515   printf("--> Request motor1 to run at 400 steps/s in forward direction.\r\n");
00516   motor1->run(StepperMotor::FWD, 400);
00517 
00518   /* Wait for device to reach the targeted speed */
00519   while((motor1->read_status_register() & POWERSTEP01_STATUS_MOT_STATUS)!=
00520         POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD)
00521   {
00522     /* Record the reached speed in step/s rounded to integer */
00523     unsignedIntegerValue = motor1->get_speed();
00524     /* Print reached speed to the console in step/s */
00525     printf("    motor1 reached Speed: %d step/s.\r\n", unsignedIntegerValue);
00526     wait_ms(50);
00527   }
00528  
00529   /* Record the reached speed in step/s */
00530   floatValue = motor1->get_analog_value(POWERSTEP01_SPEED); 
00531   /* Print reached speed to the console in step/s */
00532   printf("    motor1 reached Speed: %f step/s.\r\n", floatValue);
00533 
00534   /* Request motor 2 to run in StepperMotor::FWD direction at 300 steps/s*/
00535   /* Request motor 3 to run in StepperMotor::FWD direction at 200 steps/s*/
00536   /* and start at same time. */
00537   printf("--> Request motor2 and motor3 to run respectively in forward direction\r\n");
00538   printf("    at 300 steps/s and 200 steps/s and start at same time.\r\n");
00539   motor1->queue_commands(POWERSTEP01_NOP,0);
00540   motor2->queue_commands((uint8_t)POWERSTEP01_RUN|(uint8_t)StepperMotor::FWD,PowerStep01::speed_steps_s_to_reg_val(300));
00541   motor3->queue_commands((uint8_t)POWERSTEP01_RUN|(uint8_t)StepperMotor::FWD,PowerStep01::speed_steps_s_to_reg_val(200));
00542   motor1->send_queued_commands();  
00543  
00544   /* Wait for device to reach the targeted speed */
00545   while(((motor2->read_status_register() & POWERSTEP01_STATUS_MOT_STATUS)!=
00546         POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD)||
00547         ((motor3->read_status_register() & POWERSTEP01_STATUS_MOT_STATUS)!=
00548         POWERSTEP01_STATUS_MOT_STATUS_CONST_SPD));
00549  
00550   /* Record the reached speed in step/s */
00551   floatValue = motor2->get_analog_value(POWERSTEP01_SPEED); 
00552   /* Print reached speed to the console in step/s */
00553   printf("    motor2 reached Speed: %f step/s.\r\n", floatValue);
00554   
00555    /* Record the reached speed in step/s */
00556   floatValue = motor3->get_analog_value(POWERSTEP01_SPEED); 
00557   /* Print reached speed to the console in step/s */
00558   printf("    motor3 reached Speed: %f step/s.\r\n", floatValue); 
00559   
00560   /* Wait for 3s */
00561   wait_ms(3000);
00562 
00563   /* Request motor 2 to make a soft stop */
00564   printf("--> Request motor2 to stop softly\r\n");
00565   motor2->soft_stop();
00566   
00567   /* Wait for motor 2 end moving */
00568   motor2->wait_while_active();  
00569 
00570   /* Request motor 1 and 3 to make a hard stop */
00571   printf("--> Request motor1 and motor3 to stop immediately\r\n");
00572   motor1->queue_commands(POWERSTEP01_HARD_STOP,0);
00573   motor2->queue_commands(POWERSTEP01_NOP,0);
00574   motor3->queue_commands(POWERSTEP01_HARD_STOP,0);
00575   motor1->send_queued_commands();
00576 
00577   /* Wait for both motors end moving */  
00578   wait_for_all_devices_not_busy();
00579   
00580   /* Request all motors to go home and print to the console */
00581   printf("--> Request all motors to go home.\r\n");
00582   motor1->queue_commands(POWERSTEP01_GO_HOME,0);
00583   motor2->queue_commands(POWERSTEP01_GO_HOME,0);
00584   motor3->queue_commands(POWERSTEP01_GO_HOME,0);
00585   motor1->send_queued_commands();
00586     
00587   /* Wait for all device ends moving */ 
00588   wait_for_all_devices_not_busy();
00589 
00590   /* Get acceleration, deceleration, Maxspeed and MinSpeed of motor 1*/
00591   myMaxSpeed= motor1->get_raw_parameter(POWERSTEP01_MAX_SPEED);
00592   myAcceleration = motor1->get_raw_parameter(POWERSTEP01_ACC);
00593   myDeceleration = motor1->get_raw_parameter(POWERSTEP01_DEC);
00594   myMinSpeed = motor1->get_raw_parameter(POWERSTEP01_MIN_SPEED); 
00595   
00596   /* Select 1/16 microstepping mode for motor 1 */
00597   printf("    Set 1/16 microstepping mode for motor1.\r\n"); 
00598   motor1->set_step_mode(StepperMotor::STEP_MODE_1_16);
00599   
00600   /* Select 1/8 microstepping mode for motor 2 */
00601   printf("    Set 1/8 microstepping mode for motor2.\r\n"); 
00602   motor2->set_step_mode(StepperMotor::STEP_MODE_1_8);
00603   
00604   /* Set speed and acceleration of motor 2 */
00605   /* Do not scale with microstepping mode */
00606   motor2->set_raw_parameter(POWERSTEP01_ACC, myAcceleration);
00607   motor2->set_raw_parameter(POWERSTEP01_DEC, myDeceleration);
00608   motor2->set_raw_parameter(POWERSTEP01_MIN_SPEED, myMinSpeed);
00609   motor2->set_raw_parameter(POWERSTEP01_MAX_SPEED, myMaxSpeed);
00610   
00611   /* Select ful step mode for motor 3 */
00612   printf("    Set ful step mode for motor3.\r\n"); 
00613   motor3->set_step_mode(StepperMotor::STEP_MODE_FULL);
00614 
00615   /* Set speed and acceleration of motor 3 */
00616   /* Do not scale with microstepping mode */
00617   motor3->set_raw_parameter(POWERSTEP01_ACC, myAcceleration);
00618   motor3->set_raw_parameter(POWERSTEP01_DEC, myDeceleration);
00619   motor3->set_raw_parameter(POWERSTEP01_MIN_SPEED, myMinSpeed);
00620   motor3->set_raw_parameter(POWERSTEP01_MAX_SPEED, myMaxSpeed);
00621 
00622   /* Printing to the console. */
00623   printf("--> Infinite Loop...\r\n"); 
00624   /* Infinite loop */
00625   while(1)
00626   {
00627     /* motor 1 is using 1/16 microstepping mode */ 
00628     /* motor 2 is using 1/8 microstepping mode */
00629     /* motor 3 is using full step mode */
00630     /* position is in microsteps */
00631     motor1->queue_commands(POWERSTEP01_GO_TO,-3200);
00632     motor2->queue_commands(POWERSTEP01_GO_TO,1600);
00633     motor3->queue_commands(POWERSTEP01_GO_TO,-200);
00634     motor1->send_queued_commands();
00635     
00636     /* Wait for all device ends moving */ 
00637     wait_for_all_devices_not_busy();
00638 
00639     motor1->queue_commands(POWERSTEP01_GO_TO,3200);
00640     motor2->queue_commands(POWERSTEP01_GO_TO,-1600);
00641     motor3->queue_commands(POWERSTEP01_GO_TO,200);
00642     motor1->send_queued_commands();
00643     
00644     /* Wait for all device ends moving */ 
00645     wait_for_all_devices_not_busy();
00646   }
00647 }
00648 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/