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 
00131 
00132 /* Main ----------------------------------------------------------------------*/
00133 
00134 int main()
00135 {
00136     /*----- Initialization. -----*/
00137 
00138     /* Initializing SPI bus. */
00139 #ifdef TARGET_STM32F429
00140     DevSPI dev_spi(D11, D12, D13);
00141 #else
00142     DevSPI dev_spi(D11, D12, D3);
00143 #endif
00144 
00145     /* Initializing Motor Control Expansion Board. */
00146     x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
00147 
00148     /* Building a list of motor control components. */
00149     L6470 **motors = x_nucleo_ihm02a1->get_components();
00150 
00151     /* Printing to the console. */
00152     printf("Motor Control Application Example for 2 Motors\r\n\n");
00153 
00154 
00155     /*----- Setting home and marke positions, getting positions, and going to positions. -----*/
00156 
00157     /* Printing to the console. */
00158     printf("--> Setting home position.\r\n");
00159 
00160     /* Setting the home position. */
00161     motors[0]->set_home();
00162 
00163     /* Waiting. */
00164     wait_ms(DELAY_1);
00165 
00166     /* Getting the current position. */
00167     int position = motors[0]->get_position();
00168 
00169     /* Printing to the console. */
00170     printf("--> Getting the current position: %d\r\n", position);
00171 
00172     /* Waiting. */
00173     wait_ms(DELAY_1);
00174 
00175     /* Printing to the console. */
00176     printf("--> Moving forward %d steps.\r\n", STEPS_1);
00177 
00178     /* Moving. */
00179     motors[0]->move(StepperMotor::FWD, STEPS_1);
00180 
00181     /* Waiting while active. */
00182     motors[0]->wait_while_active();
00183 
00184     /* Getting the current position. */
00185     position = motors[0]->get_position();
00186     
00187     /* Printing to the console. */
00188     printf("--> Getting the current position: %d\r\n", position);
00189 
00190     /* Printing to the console. */
00191     printf("--> Marking the current position.\r\n");
00192 
00193     /* Marking the current position. */
00194     motors[0]->set_mark();
00195 
00196     /* Waiting. */
00197     wait_ms(DELAY_1);
00198 
00199     /* Printing to the console. */
00200     printf("--> Moving backward %d steps.\r\n", STEPS_2);
00201 
00202     /* Moving. */
00203     motors[0]->move(StepperMotor::BWD, STEPS_2);
00204 
00205     /* Waiting while active. */
00206     motors[0]->wait_while_active();
00207 
00208     /* Waiting. */
00209     wait_ms(DELAY_1);
00210 
00211     /* Getting the current position. */
00212     position = motors[0]->get_position();
00213     
00214     /* Printing to the console. */
00215     printf("--> Getting the current position: %d\r\n", position);
00216 
00217     /* Waiting. */
00218     wait_ms(DELAY_1);
00219 
00220     /* Printing to the console. */
00221     printf("--> Going to marked position.\r\n");
00222 
00223     /* Going to marked position. */
00224     motors[0]->go_mark();
00225     
00226     /* Waiting while active. */
00227     motors[0]->wait_while_active();
00228 
00229     /* Waiting. */
00230     wait_ms(DELAY_1);
00231 
00232     /* Getting the current position. */
00233     position = motors[0]->get_position();
00234     
00235     /* Printing to the console. */
00236     printf("--> Getting the current position: %d\r\n", position);
00237 
00238     /* Waiting. */
00239     wait_ms(DELAY_1);
00240 
00241     /* Printing to the console. */
00242     printf("--> Going to home position.\r\n");
00243 
00244     /* Going to home position. */
00245     motors[0]->go_home();
00246     
00247     /* Waiting while active. */
00248     motors[0]->wait_while_active();
00249 
00250     /* Waiting. */
00251     wait_ms(DELAY_1);
00252 
00253     /* Getting the current position. */
00254     position = motors[0]->get_position();
00255     
00256     /* Printing to the console. */
00257     printf("--> Getting the current position: %d\r\n", position);
00258 
00259     /* Waiting. */
00260     wait_ms(DELAY_1);
00261 
00262     /* Printing to the console. */
00263     printf("--> Halving the microsteps.\r\n");
00264 
00265     /* Halving the microsteps. */
00266     init[0].step_sel = (init[0].step_sel > 0 ? init[0].step_sel -  1 : init[0].step_sel);
00267     if (!motors[0]->set_step_mode((StepperMotor::step_mode_t) init[0].step_sel)) {
00268         printf("    Step Mode not allowed.\r\n");
00269     }
00270 
00271     /* Waiting. */
00272     wait_ms(DELAY_1);
00273 
00274     /* Printing to the console. */
00275     printf("--> Setting home position.\r\n");
00276 
00277     /* Setting the home position. */
00278     motors[0]->set_home();
00279 
00280     /* Waiting. */
00281     wait_ms(DELAY_1);
00282 
00283     /* Getting the current position. */
00284     position = motors[0]->get_position();
00285     
00286     /* Printing to the console. */
00287     printf("--> Getting the current position: %d\r\n", position);
00288 
00289     /* Waiting. */
00290     wait_ms(DELAY_1);
00291 
00292     /* Printing to the console. */
00293     printf("--> Moving forward %d steps.\r\n", STEPS_1);
00294 
00295     /* Moving. */
00296     motors[0]->move(StepperMotor::FWD, STEPS_1);
00297 
00298     /* Waiting while active. */
00299     motors[0]->wait_while_active();
00300 
00301     /* Getting the current position. */
00302     position = motors[0]->get_position();
00303     
00304     /* Printing to the console. */
00305     printf("--> Getting the current position: %d\r\n", position);
00306 
00307     /* Printing to the console. */
00308     printf("--> Marking the current position.\r\n");
00309 
00310     /* Marking the current position. */
00311     motors[0]->set_mark();
00312 
00313     /* Waiting. */
00314     wait_ms(DELAY_2);
00315 
00316 
00317     /*----- Running together for a certain amount of time. -----*/
00318 
00319     /* Printing to the console. */
00320     printf("--> Running together for %d seconds.\r\n", DELAY_3 / 1000);
00321 
00322     /* Preparing each motor to perform a run at a specified speed. */
00323     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00324         motors[m]->prepare_run(StepperMotor::BWD, 400);
00325     }
00326 
00327     /* Performing the action on each motor at the same time. */
00328     x_nucleo_ihm02a1->perform_prepared_actions();
00329 
00330     /* Waiting. */
00331     wait_ms(DELAY_3);
00332 
00333 
00334     /*----- Increasing the speed while running. -----*/
00335 
00336     /* Preparing each motor to perform a run at a specified speed. */
00337     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00338         motors[m]->prepare_get_speed();
00339     }
00340 
00341     /* Performing the action on each motor at the same time. */
00342     uint32_t* results = x_nucleo_ihm02a1->perform_prepared_actions();
00343 
00344     /* Printing to the console. */
00345     printf("    Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
00346 
00347     /* Printing to the console. */
00348     printf("--> Doublig the speed while running again for %d seconds.\r\n", DELAY_3 / 1000);
00349 
00350     /* Preparing each motor to perform a run at a specified speed. */
00351     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00352         motors[m]->prepare_run(StepperMotor::BWD, results[m] << 1);
00353     }
00354 
00355     /* Performing the action on each motor at the same time. */
00356     results = x_nucleo_ihm02a1->perform_prepared_actions();
00357 
00358     /* Waiting. */
00359     wait_ms(DELAY_3);
00360 
00361     /* Preparing each motor to perform a run at a specified speed. */
00362     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00363         motors[m]->prepare_get_speed();
00364     }
00365 
00366     /* Performing the action on each motor at the same time. */
00367     results = x_nucleo_ihm02a1->perform_prepared_actions();
00368 
00369     /* Printing to the console. */
00370     printf("    Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
00371 
00372     /* Waiting. */
00373     wait_ms(DELAY_1);
00374 
00375 
00376     /*----- Hard Stop. -----*/
00377 
00378     /* Printing to the console. */
00379     printf("--> Hard Stop.\r\n");
00380 
00381     /* Preparing each motor to perform a hard stop. */
00382     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00383         motors[m]->prepare_hard_stop();
00384     }
00385 
00386     /* Performing the action on each motor at the same time. */
00387     x_nucleo_ihm02a1->perform_prepared_actions();
00388 
00389     /* Waiting. */
00390     wait_ms(DELAY_2);
00391 
00392 
00393     /*----- Doing a full revolution on each motor, one after the other. -----*/
00394 
00395     /* Printing to the console. */
00396     printf("--> Doing a full revolution on each motor, one after the other.\r\n");
00397 
00398     /* Doing a full revolution on each motor, one after the other. */
00399     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00400         for (int i = 0; i < MPR_1; i++) {
00401             /* Computing the number of steps. */
00402             int steps = (int) (((int) init[m].fullstepsperrevolution * pow(2.0f, init[m].step_sel)) / MPR_1);
00403 
00404             /* Moving. */
00405             motors[m]->move(StepperMotor::FWD, steps);
00406             
00407             /* Waiting while active. */
00408             motors[m]->wait_while_active();
00409 
00410             /* Waiting. */
00411             wait_ms(DELAY_1);
00412         }
00413     }
00414 
00415     /* Waiting. */
00416     wait_ms(DELAY_2);
00417 
00418 
00419     /*----- High Impedance State. -----*/
00420 
00421     /* Printing to the console. */
00422     printf("--> High Impedance State.\r\n");
00423 
00424     /* Preparing each motor to set High Impedance State. */
00425     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00426         motors[m]->prepare_hard_hiz();
00427     }
00428 
00429     /* Performing the action on each motor at the same time. */
00430     x_nucleo_ihm02a1->perform_prepared_actions();
00431 
00432     /* Waiting. */
00433     wait_ms(DELAY_2);
00434 }