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 #include "rtos.h"
00045 
00046 /* Helper header files. */
00047 #include "DevSPI.h"
00048 
00049 /* Expansion Board specific header files. */
00050 #include "XNucleoIHM02A1.h"
00051 
00052 
00053 /* Definitions ---------------------------------------------------------------*/
00054 
00055 /* Number of movements per revolution. */
00056 #define MPR_1 4
00057 
00058 /* Number of steps. */
00059 #define STEPS_1 (400 * 128)   /* 1 revolution given a 400 steps motor configured at 1/128 microstep mode. */
00060 #define STEPS_2 (STEPS_1 * 2)
00061 
00062 /* Delay in milliseconds. */
00063 #define DELAY_1 1000
00064 #define DELAY_2 2000
00065 #define DELAY_3 5000
00066 
00067 
00068 /* Variables -----------------------------------------------------------------*/
00069 
00070 /* Motor Control Expansion Board. */
00071 XNucleoIHM02A1 *x_nucleo_ihm02a1;
00072 
00073 /* Initialization parameters of the motors connected to the expansion board. */
00074 L6470_init_t init[L6470DAISYCHAINSIZE] = {
00075     /* First Motor. */
00076     {
00077         9.0,                           /* Motor supply voltage in V. */
00078         400,                           /* Min number of steps per revolution for the motor. */
00079         1.7,                           /* Max motor phase voltage in A. */
00080         3.06,                          /* Max motor phase voltage in V. */
00081         300.0,                         /* Motor initial speed [step/s]. */
00082         500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00083         500.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00084         992.0,                         /* Motor maximum speed [step/s]. */
00085         0.0,                           /* Motor minimum speed [step/s]. */
00086         602.7,                         /* Motor full-step speed threshold [step/s]. */
00087         3.06,                          /* Holding kval [V]. */
00088         3.06,                          /* Constant speed kval [V]. */
00089         3.06,                          /* Acceleration starting kval [V]. */
00090         3.06,                          /* Deceleration starting kval [V]. */
00091         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00092         392.1569e-6,                   /* Start slope [s/step]. */
00093         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00094         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00095         0,                             /* Thermal compensation factor (range [0, 15]). */
00096         3.06 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00097         3.06 * 1000 * 1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00098         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
00099         0xFF,                          /* Alarm conditions enable. */
00100         0x2E88                         /* Ic configuration. */
00101     },
00102 
00103     /* Second Motor. */
00104     {
00105         9.0,                           /* Motor supply voltage in V. */
00106         400,                           /* Min number of steps per revolution for the motor. */
00107         1.7,                           /* Max motor phase voltage in A. */
00108         3.06,                          /* Max motor phase voltage in V. */
00109         300.0,                         /* Motor initial speed [step/s]. */
00110         500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
00111         500.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
00112         992.0,                         /* Motor maximum speed [step/s]. */
00113         0.0,                           /* Motor minimum speed [step/s]. */
00114         602.7,                         /* Motor full-step speed threshold [step/s]. */
00115         3.06,                          /* Holding kval [V]. */
00116         3.06,                          /* Constant speed kval [V]. */
00117         3.06,                          /* Acceleration starting kval [V]. */
00118         3.06,                          /* Deceleration starting kval [V]. */
00119         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
00120         392.1569e-6,                   /* Start slope [s/step]. */
00121         643.1372e-6,                   /* Acceleration final slope [s/step]. */
00122         643.1372e-6,                   /* Deceleration final slope [s/step]. */
00123         0,                             /* Thermal compensation factor (range [0, 15]). */
00124         3.06 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
00125         3.06 * 1000 * 1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
00126         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
00127         0xFF,                          /* Alarm conditions enable. */
00128         0x2E88                         /* Ic configuration. */
00129     }
00130 };
00131 
00132 
00133 /* Main ----------------------------------------------------------------------*/
00134 
00135 int main()
00136 {
00137     /*----- Initialization. -----*/
00138 
00139     /* Initializing SPI bus. */
00140 #ifdef TARGET_STM32F429
00141     DevSPI dev_spi(D11, D12, D13);
00142 #else
00143     DevSPI dev_spi(D11, D12, D3);
00144 #endif
00145 
00146     /* Initializing Motor Control Expansion Board. */
00147     x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
00148 
00149     /* Building a list of motor control components. */
00150     L6470 **motors = x_nucleo_ihm02a1->get_components();
00151 
00152     /* Printing to the console. */
00153     printf("Motor Control Application Example for 2 Motors\r\n\n");
00154 
00155 
00156     /*----- Setting home and marke positions, getting positions, and going to positions. -----*/
00157 
00158     /* Printing to the console. */
00159     printf("--> Setting home position.\r\n");
00160 
00161     /* Setting the home position. */
00162     motors[0]->set_home();
00163 
00164     /* Waiting. */
00165     wait_ms(DELAY_1);
00166 
00167     /* Getting the current position. */
00168     int position = motors[0]->get_position();
00169 
00170     /* Printing to the console. */
00171     printf("--> Getting the current position: %d\r\n", position);
00172 
00173     /* Waiting. */
00174     wait_ms(DELAY_1);
00175 
00176     /* Printing to the console. */
00177     printf("--> Moving forward %d steps.\r\n", STEPS_1);
00178 
00179     /* Moving. */
00180     motors[0]->move(StepperMotor::FWD, STEPS_1);
00181 
00182     /* Waiting while active. */
00183     motors[0]->wait_while_active();
00184 
00185     /* Getting the current position. */
00186     position = motors[0]->get_position();
00187     
00188     /* Printing to the console. */
00189     printf("--> Getting the current position: %d\r\n", position);
00190 
00191     /* Printing to the console. */
00192     printf("--> Marking the current position.\r\n");
00193 
00194     /* Marking the current position. */
00195     motors[0]->set_mark();
00196 
00197     /* Waiting. */
00198     wait_ms(DELAY_1);
00199 
00200     /* Printing to the console. */
00201     printf("--> Moving backward %d steps.\r\n", STEPS_2);
00202 
00203     /* Moving. */
00204     motors[0]->move(StepperMotor::BWD, STEPS_2);
00205 
00206     /* Waiting while active. */
00207     motors[0]->wait_while_active();
00208 
00209     /* Waiting. */
00210     wait_ms(DELAY_1);
00211 
00212     /* Getting the current position. */
00213     position = motors[0]->get_position();
00214     
00215     /* Printing to the console. */
00216     printf("--> Getting the current position: %d\r\n", position);
00217 
00218     /* Waiting. */
00219     wait_ms(DELAY_1);
00220 
00221     /* Printing to the console. */
00222     printf("--> Going to marked position.\r\n");
00223 
00224     /* Going to marked position. */
00225     motors[0]->go_mark();
00226     
00227     /* Waiting while active. */
00228     motors[0]->wait_while_active();
00229 
00230     /* Waiting. */
00231     wait_ms(DELAY_1);
00232 
00233     /* Getting the current position. */
00234     position = motors[0]->get_position();
00235     
00236     /* Printing to the console. */
00237     printf("--> Getting the current position: %d\r\n", position);
00238 
00239     /* Waiting. */
00240     wait_ms(DELAY_1);
00241 
00242     /* Printing to the console. */
00243     printf("--> Going to home position.\r\n");
00244 
00245     /* Going to home position. */
00246     motors[0]->go_home();
00247     
00248     /* Waiting while active. */
00249     motors[0]->wait_while_active();
00250 
00251     /* Waiting. */
00252     wait_ms(DELAY_1);
00253 
00254     /* Getting the current position. */
00255     position = motors[0]->get_position();
00256     
00257     /* Printing to the console. */
00258     printf("--> Getting the current position: %d\r\n", position);
00259 
00260     /* Waiting. */
00261     wait_ms(DELAY_1);
00262 
00263     /* Printing to the console. */
00264     printf("--> Halving the microsteps.\r\n");
00265 
00266     /* Halving the microsteps. */
00267     init[0].step_sel = (init[0].step_sel > 0 ? init[0].step_sel -  1 : init[0].step_sel);
00268     if (!motors[0]->set_step_mode((StepperMotor::step_mode_t) init[0].step_sel)) {
00269         printf("    Step Mode not allowed.\r\n");
00270     }
00271 
00272     /* Waiting. */
00273     wait_ms(DELAY_1);
00274 
00275     /* Printing to the console. */
00276     printf("--> Setting home position.\r\n");
00277 
00278     /* Setting the home position. */
00279     motors[0]->set_home();
00280 
00281     /* Waiting. */
00282     wait_ms(DELAY_1);
00283 
00284     /* Getting the current position. */
00285     position = motors[0]->get_position();
00286     
00287     /* Printing to the console. */
00288     printf("--> Getting the current position: %d\r\n", position);
00289 
00290     /* Waiting. */
00291     wait_ms(DELAY_1);
00292 
00293     /* Printing to the console. */
00294     printf("--> Moving forward %d steps.\r\n", STEPS_1);
00295 
00296     /* Moving. */
00297     motors[0]->move(StepperMotor::FWD, STEPS_1);
00298 
00299     /* Waiting while active. */
00300     motors[0]->wait_while_active();
00301 
00302     /* Getting the current position. */
00303     position = motors[0]->get_position();
00304     
00305     /* Printing to the console. */
00306     printf("--> Getting the current position: %d\r\n", position);
00307 
00308     /* Printing to the console. */
00309     printf("--> Marking the current position.\r\n");
00310 
00311     /* Marking the current position. */
00312     motors[0]->set_mark();
00313 
00314     /* Waiting. */
00315     wait_ms(DELAY_2);
00316 
00317 
00318     /*----- Running together for a certain amount of time. -----*/
00319 
00320     /* Printing to the console. */
00321     printf("--> Running together for %d seconds.\r\n", DELAY_3 / 1000);
00322 
00323     /* Preparing each motor to perform a run at a specified speed. */
00324     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00325         motors[m]->prepare_run(StepperMotor::BWD, 400);
00326     }
00327 
00328     /* Performing the action on each motor at the same time. */
00329     x_nucleo_ihm02a1->perform_prepared_actions();
00330 
00331     /* Waiting. */
00332     wait_ms(DELAY_3);
00333 
00334 
00335     /*----- Increasing the speed while running. -----*/
00336 
00337     /* Preparing each motor to perform a run at a specified speed. */
00338     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00339         motors[m]->prepare_get_speed();
00340     }
00341 
00342     /* Performing the action on each motor at the same time. */
00343     uint32_t* results = x_nucleo_ihm02a1->perform_prepared_actions();
00344 
00345     /* Printing to the console. */
00346     printf("    Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
00347 
00348     /* Printing to the console. */
00349     printf("--> Doublig the speed while running again for %d seconds.\r\n", DELAY_3 / 1000);
00350 
00351     /* Preparing each motor to perform a run at a specified speed. */
00352     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00353         motors[m]->prepare_run(StepperMotor::BWD, results[m] << 1);
00354     }
00355 
00356     /* Performing the action on each motor at the same time. */
00357     results = x_nucleo_ihm02a1->perform_prepared_actions();
00358 
00359     /* Waiting. */
00360     wait_ms(DELAY_3);
00361 
00362     /* Preparing each motor to perform a run at a specified speed. */
00363     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00364         motors[m]->prepare_get_speed();
00365     }
00366 
00367     /* Performing the action on each motor at the same time. */
00368     results = x_nucleo_ihm02a1->perform_prepared_actions();
00369 
00370     /* Printing to the console. */
00371     printf("    Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
00372 
00373     /* Waiting. */
00374     wait_ms(DELAY_1);
00375 
00376 
00377     /*----- Hard Stop. -----*/
00378 
00379     /* Printing to the console. */
00380     printf("--> Hard Stop.\r\n");
00381 
00382     /* Preparing each motor to perform a hard stop. */
00383     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00384         motors[m]->prepare_hard_stop();
00385     }
00386 
00387     /* Performing the action on each motor at the same time. */
00388     x_nucleo_ihm02a1->perform_prepared_actions();
00389 
00390     /* Waiting. */
00391     wait_ms(DELAY_2);
00392 
00393 
00394     /*----- Doing a full revolution on each motor, one after the other. -----*/
00395 
00396     /* Printing to the console. */
00397     printf("--> Doing a full revolution on each motor, one after the other.\r\n");
00398 
00399     /* Doing a full revolution on each motor, one after the other. */
00400     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00401         for (int i = 0; i < MPR_1; i++) {
00402             /* Computing the number of steps. */
00403             int steps = (int) (((int) init[m].fullstepsperrevolution * pow(2.0f, init[m].step_sel)) / MPR_1);
00404 
00405             /* Moving. */
00406             motors[m]->move(StepperMotor::FWD, steps);
00407             
00408             /* Waiting while active. */
00409             motors[m]->wait_while_active();
00410 
00411             /* Waiting. */
00412             wait_ms(DELAY_1);
00413         }
00414     }
00415 
00416     /* Waiting. */
00417     wait_ms(DELAY_2);
00418 
00419 
00420     /*----- High Impedance State. -----*/
00421 
00422     /* Printing to the console. */
00423     printf("--> High Impedance State.\r\n");
00424 
00425     /* Preparing each motor to set High Impedance State. */
00426     for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
00427         motors[m]->prepare_hard_hiz();
00428     }
00429 
00430     /* Performing the action on each motor at the same time. */
00431     x_nucleo_ihm02a1->perform_prepared_actions();
00432 
00433     /* Waiting. */
00434     wait_ms(DELAY_2);
00435 }