main.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    main.cpp
00004  * @author  Davide Aliprandi, STMicroelectronics
00005  * @version V1.0.0
00006  * @date    November 4th, 2015
00007  * @brief   mbed test application for the STMicroelectronics X-NUCLEO-IHM02A1
00008  *          Motor Control Expansion Board: control of 2 motors.
00009  ******************************************************************************
00010  * @attention
00011  *
00012  * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
00013  *
00014  * Redistribution and use in source and binary forms, with or without modification,
00015  * are permitted provided that the following conditions are met:
00016  *   1. Redistributions of source code must retain the above copyright notice,
00017  *      this list of conditions and the following disclaimer.
00018  *   2. Redistributions in binary form must reproduce the above copyright notice,
00019  *      this list of conditions and the following disclaimer in the documentation
00020  *      and/or other materials provided with the distribution.
00021  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00022  *      may be used to endorse or promote products derived from this software
00023  *      without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00028  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00029  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00030  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00031  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00033  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00034  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  ******************************************************************************
00037  */
00038 
00039 
00040 /* Includes ------------------------------------------------------------------*/
00041 
00042 /* mbed specific header files. */
00043 #include "mbed.h"
00044 
00045 /* Helper header files. */
00046 #include "DevSPI.h"
00047 
00048 /* Expansion Board specific header files. */
00049 #include "XNucleoIHM02A1.h"
00050 
00051 
00052 /* Definitions ---------------------------------------------------------------*/
00053 
00054 /* Number of movements per revolution. */
00055 #define MPR_1 4
00056 
00057 /* Number of steps. */
00058 #define STEPS_1 (400 * 128)   /* 1 revolution given a 400 steps motor configured at 1/128 microstep mode. */
00059 #define STEPS_2 (STEPS_1 * 2)
00060 
00061 /* Delay in milliseconds. */
00062 #define DELAY_1 1000
00063 #define DELAY_2 2000
00064 #define DELAY_3 5000
00065 
00066 
00067 /* Variables -----------------------------------------------------------------*/
00068 
00069 /* Motor Control Expansion Board. */
00070 XNucleoIHM02A1 *x_nucleo_ihm02a1;
00071 
00072 /* Initialization parameters of the motors connected to the expansion board. */
00073 L6470_init_t init[L6470DAISYCHAINSIZE] = {
00074     /* First Motor. */
00075     {
00076         9.0,                           /* Motor supply voltage in V. */
00077         400,                           /* Min number of steps per revolution for the motor. */
00078         1.7,                           /* Max motor phase voltage in A. */
00079         3.06,                          /* Max motor phase voltage in V. */
00080         300.0,                         /* Motor initial speed [step/s]. */
00081         500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00082         500.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00083         992.0,                         /* Motor maximum speed [step/s]. */
00084         0.0,                           /* Motor minimum speed [step/s]. */
00085         602.7,                         /* Motor full-step speed threshold [step/s]. */
00086         3.06,                          /* Holding kval [V]. */
00087         3.06,                          /* Constant speed kval [V]. */
00088         3.06,                          /* Acceleration starting kval [V]. */
00089         3.06,                          /* Deceleration starting kval [V]. */
00090         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00091         392.1569e-6,                   /* Start slope [s/step]. */
00092         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00093         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00094         0,                             /* Thermal compensation factor (range [0, 15]). */
00095         3.06 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00096         3.06 * 1000 * 1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00097         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
00098         0xFF,                          /* Alarm conditions enable. */
00099         0x2E88                         /* Ic configuration. */
00100     },
00101 
00102     /* Second Motor. */
00103     {
00104         9.0,                           /* Motor supply voltage in V. */
00105         400,                           /* Min number of steps per revolution for the motor. */
00106         1.7,                           /* Max motor phase voltage in A. */
00107         3.06,                          /* Max motor phase voltage in V. */
00108         300.0,                         /* Motor initial speed [step/s]. */
00109         500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00110         500.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00111         992.0,                         /* Motor maximum speed [step/s]. */
00112         0.0,                           /* Motor minimum speed [step/s]. */
00113         602.7,                         /* Motor full-step speed threshold [step/s]. */
00114         3.06,                          /* Holding kval [V]. */
00115         3.06,                          /* Constant speed kval [V]. */
00116         3.06,                          /* Acceleration starting kval [V]. */
00117         3.06,                          /* Deceleration starting kval [V]. */
00118         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00119         392.1569e-6,                   /* Start slope [s/step]. */
00120         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00121         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00122         0,                             /* Thermal compensation factor (range [0, 15]). */
00123         3.06 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00124         3.06 * 1000 * 1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00125         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
00126         0xFF,                          /* Alarm conditions enable. */
00127         0x2E88                         /* Ic configuration. */
00128     }
00129 };
00130 void wait_ms(int delay)
00131 {
00132     wait_us(DELAY_1/1000);
00133 }
00134 
00135 /* Main ----------------------------------------------------------------------*/
00136 
00137 int main()
00138 {
00139     /*----- Initialization. -----*/
00140 
00141     /* Initializing SPI bus. */
00142 #ifdef TARGET_STM32F429
00143     DevSPI dev_spi(D11, D12, D13);
00144 #else
00145     DevSPI dev_spi(D11, D12, D3);
00146 #endif
00147 
00148     /* Initializing Motor Control Expansion Board. */
00149     x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
00150 
00151     /* Building a list of motor control components. */
00152     L6470 **motors = x_nucleo_ihm02a1->get_components();
00153 
00154     /* Printing to the console. */
00155     printf("Motor Control Application Example for 2 Motors\r\n\n");
00156 
00157 
00158     /*----- Setting home and marke positions, getting positions, and going to positions. -----*/
00159 
00160     /* Printing to the console. */
00161     printf("--> Setting home position.\r\n");
00162 
00163     /* Setting the home position. */
00164     motors[0]->set_home();
00165 
00166     /* Waiting. */
00167     wait_ms(DELAY_1);
00168 
00169     /* Getting the current position. */
00170     int position = motors[0]->get_position();
00171 
00172     /* Printing to the console. */
00173     printf("--> Getting the current position: %d\r\n", position);
00174 
00175     /* Waiting. */
00176     wait_ms(DELAY_1);
00177 
00178     /* Printing to the console. */
00179     printf("--> Moving forward %d steps.\r\n", STEPS_1);
00180 
00181     /* Moving. */
00182     motors[0]->move(StepperMotor::FWD, STEPS_1);
00183 
00184     /* Waiting while active. */
00185     motors[0]->wait_while_active();
00186 
00187     /* Getting the current position. */
00188     position = motors[0]->get_position();
00189     
00190     /* Printing to the console. */
00191     printf("--> Getting the current position: %d\r\n", position);
00192 
00193     /* Printing to the console. */
00194     printf("--> Marking the current position.\r\n");
00195 
00196     /* Marking the current position. */
00197     motors[0]->set_mark();
00198 
00199     /* Waiting. */
00200     wait_ms(DELAY_1);
00201 
00202     /* Printing to the console. */
00203     printf("--> Moving backward %d steps.\r\n", STEPS_2);
00204 
00205     /* Moving. */
00206     motors[0]->move(StepperMotor::BWD, STEPS_2);
00207 
00208     /* Waiting while active. */
00209     motors[0]->wait_while_active();
00210 
00211     /* Waiting. */
00212     wait_ms(DELAY_1);
00213 
00214     /* Getting the current position. */
00215     position = motors[0]->get_position();
00216     
00217     /* Printing to the console. */
00218     printf("--> Getting the current position: %d\r\n", position);
00219 
00220     /* Waiting. */
00221     wait_ms(DELAY_1);
00222 
00223     /* Printing to the console. */
00224     printf("--> Going to marked position.\r\n");
00225 
00226     /* Going to marked position. */
00227     motors[0]->go_mark();
00228     
00229     /* Waiting while active. */
00230     motors[0]->wait_while_active();
00231 
00232     /* Waiting. */
00233     wait_ms(DELAY_1);
00234 
00235     /* Getting the current position. */
00236     position = motors[0]->get_position();
00237     
00238     /* Printing to the console. */
00239     printf("--> Getting the current position: %d\r\n", position);
00240 
00241     /* Waiting. */
00242     wait_ms(DELAY_1);
00243 
00244     /* Printing to the console. */
00245     printf("--> Going to home position.\r\n");
00246 
00247     /* Going to home position. */
00248     motors[0]->go_home();
00249     
00250     /* Waiting while active. */
00251     motors[0]->wait_while_active();
00252 
00253     /* Waiting. */
00254     wait_ms(DELAY_1);
00255 
00256     /* Getting the current position. */
00257     position = motors[0]->get_position();
00258     
00259     /* Printing to the console. */
00260     printf("--> Getting the current position: %d\r\n", position);
00261 
00262     /* Waiting. */
00263     wait_ms(DELAY_1);
00264 
00265     /* Printing to the console. */
00266     printf("--> Halving the microsteps.\r\n");
00267 
00268     /* Halving the microsteps. */
00269     init[0].step_sel = (init[0].step_sel > 0 ? init[0].step_sel -  1 : init[0].step_sel);
00270     if (!motors[0]->set_step_mode((StepperMotor::step_mode_t) init[0].step_sel)) {
00271         printf("    Step Mode not allowed.\r\n");
00272     }
00273 
00274     /* Waiting. */
00275     wait_ms(DELAY_1);
00276 
00277     /* Printing to the console. */
00278     printf("--> Setting home position.\r\n");
00279 
00280     /* Setting the home position. */
00281     motors[0]->set_home();
00282 
00283     /* Waiting. */
00284     wait_ms(DELAY_1);
00285 
00286     /* Getting the current position. */
00287     position = motors[0]->get_position();
00288     
00289     /* Printing to the console. */
00290     printf("--> Getting the current position: %d\r\n", position);
00291 
00292     /* Waiting. */
00293     wait_ms(DELAY_1);
00294 
00295     /* Printing to the console. */
00296     printf("--> Moving forward %d steps.\r\n", STEPS_1);
00297 
00298     /* Moving. */
00299     motors[0]->move(StepperMotor::FWD, STEPS_1);
00300 
00301     /* Waiting while active. */
00302     motors[0]->wait_while_active();
00303 
00304     /* Getting the current position. */
00305     position = motors[0]->get_position();
00306     
00307     /* Printing to the console. */
00308     printf("--> Getting the current position: %d\r\n", position);
00309 
00310     /* Printing to the console. */
00311     printf("--> Marking the current position.\r\n");
00312 
00313     /* Marking the current position. */
00314     motors[0]->set_mark();
00315 
00316     /* Waiting. */
00317     wait_ms(DELAY_2);
00318 
00319 
00320     /*----- Running together for a certain amount of time. -----*/
00321 
00322     /* Printing to the console. */
00323     printf("--> Running together for %d seconds.\r\n", DELAY_3 / 1000);
00324 
00325     /* Preparing each motor to perform a run at a specified speed. */
00326     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00327         motors[m]->prepare_run(StepperMotor::BWD, 400);
00328     }
00329 
00330     /* Performing the action on each motor at the same time. */
00331     x_nucleo_ihm02a1->perform_prepared_actions();
00332 
00333     /* Waiting. */
00334     wait_ms(DELAY_3);
00335 
00336 
00337     /*----- Increasing the speed while running. -----*/
00338 
00339     /* Preparing each motor to perform a run at a specified speed. */
00340     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00341         motors[m]->prepare_get_speed();
00342     }
00343 
00344     /* Performing the action on each motor at the same time. */
00345     uint32_t* results = x_nucleo_ihm02a1->perform_prepared_actions();
00346 
00347     /* Printing to the console. */
00348     printf("    Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
00349 
00350     /* Printing to the console. */
00351     printf("--> Doublig the speed while running again for %d seconds.\r\n", DELAY_3 / 1000);
00352 
00353     /* Preparing each motor to perform a run at a specified speed. */
00354     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00355         motors[m]->prepare_run(StepperMotor::BWD, results[m] << 1);
00356     }
00357 
00358     /* Performing the action on each motor at the same time. */
00359     results = x_nucleo_ihm02a1->perform_prepared_actions();
00360 
00361     /* Waiting. */
00362     wait_ms(DELAY_3);
00363 
00364     /* Preparing each motor to perform a run at a specified speed. */
00365     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00366         motors[m]->prepare_get_speed();
00367     }
00368 
00369     /* Performing the action on each motor at the same time. */
00370     results = x_nucleo_ihm02a1->perform_prepared_actions();
00371 
00372     /* Printing to the console. */
00373     printf("    Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
00374 
00375     /* Waiting. */
00376     wait_ms(DELAY_1);
00377 
00378 
00379     /*----- Hard Stop. -----*/
00380 
00381     /* Printing to the console. */
00382     printf("--> Hard Stop.\r\n");
00383 
00384     /* Preparing each motor to perform a hard stop. */
00385     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00386         motors[m]->prepare_hard_stop();
00387     }
00388 
00389     /* Performing the action on each motor at the same time. */
00390     x_nucleo_ihm02a1->perform_prepared_actions();
00391 
00392     /* Waiting. */
00393     wait_ms(DELAY_2);
00394 
00395 
00396     /*----- Doing a full revolution on each motor, one after the other. -----*/
00397 
00398     /* Printing to the console. */
00399     printf("--> Doing a full revolution on each motor, one after the other.\r\n");
00400 
00401     /* Doing a full revolution on each motor, one after the other. */
00402     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00403         for (int i = 0; i < MPR_1; i++) {
00404             /* Computing the number of steps. */
00405             int steps = (int) (((int) init[m].fullstepsperrevolution * pow(2.0f, init[m].step_sel)) / MPR_1);
00406 
00407             /* Moving. */
00408             motors[m]->move(StepperMotor::FWD, steps);
00409             
00410             /* Waiting while active. */
00411             motors[m]->wait_while_active();
00412 
00413             /* Waiting. */
00414             wait_ms(DELAY_1);
00415         }
00416     }
00417 
00418     /* Waiting. */
00419     wait_ms(DELAY_2);
00420 
00421 
00422     /*----- High Impedance State. -----*/
00423 
00424     /* Printing to the console. */
00425     printf("--> High Impedance State.\r\n");
00426 
00427     /* Preparing each motor to set High Impedance State. */
00428     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00429         motors[m]->prepare_hard_hiz();
00430     }
00431 
00432     /* Performing the action on each motor at the same time. */
00433     x_nucleo_ihm02a1->perform_prepared_actions();
00434 
00435     /* Waiting. */
00436     wait_ms(DELAY_2);
00437 }