main.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    main.cpp
00004  * @author  Davide Aliprandi, STMicroelectronics
00005  * @version V1.0.0
00006  * @date    October 16th, 2015
00007  * @brief   mbed vertical application using the STMicroelectronics
00008  *          X-NUCLEO-IHM01A1 Motor Control Expansion Board and the
00009  *          X-NUCLEO-IKS01A2 MEMS Inertial & Environmental Sensors Expansion
00010  *          Board to get a MEMS-based motor control (direction and speed).
00011  ******************************************************************************
00012  * @attention
00013  *
00014  * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
00015  *
00016  * Redistribution and use in source and binary forms, with or without modification,
00017  * are permitted provided that the following conditions are met:
00018  *   1. Redistributions of source code must retain the above copyright notice,
00019  *      this list of conditions and the following disclaimer.
00020  *   2. Redistributions in binary form must reproduce the above copyright notice,
00021  *      this list of conditions and the following disclaimer in the documentation
00022  *      and/or other materials provided with the distribution.
00023  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00024  *      may be used to endorse or promote products derived from this software
00025  *      without specific prior written permission.
00026  *
00027  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00028  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00029  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00030  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00031  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00032  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00033  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00034  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00035  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00036  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00037  *
00038  ******************************************************************************
00039  */
00040 
00041 
00042 /* Includes ------------------------------------------------------------------*/
00043 
00044 /* mbed specific header files. */
00045 #include "mbed.h"
00046 
00047 /* Helper header files. */
00048 #include "DevSPI.h"
00049 
00050 /* Components and expansion boards specific header files. */
00051 #include "XNucleoIKS01A2.h"
00052 #include "L6474.h"
00053 
00054 
00055 /* Definitions ---------------------------------------------------------------*/
00056 
00057 /* Absolute value of the threshold on the Y axis acceleration. */
00058 #define ACCELERATION_TH 50
00059 
00060 /* Rotation gain. */
00061 #define ROTATION_SPEED_GAIN 20
00062 
00063 
00064 /* Variables -----------------------------------------------------------------*/
00065 
00066 /* MEMS Expansion Board. */
00067 XNucleoIKS01A2 *sensors;
00068 
00069 /* Motor Control Component. */
00070 L6474 *motor;
00071 
00072 
00073 /* Main ----------------------------------------------------------------------*/
00074 
00075 int main()
00076 {
00077     /*----- Initialization. -----*/
00078 
00079     /* Initializing I2C bus. */
00080     DevI2C dev_i2c(D14, D15);
00081 
00082     /* Initializing SPI bus. */
00083     DevSPI dev_spi(D11, D12, D13);
00084 
00085     /* Initializing MEMS Expansion Board. */
00086     sensors = XNucleoIKS01A2::instance(&dev_i2c);
00087 
00088     /* Retrieving the accelerometer. */
00089     LSM303AGRAccSensor *accelerometer = sensors->accelerometer;
00090     accelerometer->enable();
00091     int acceleration_axis = 1;
00092 
00093     /* Initializing Motor Control Component. */
00094     motor = new L6474(D2, D8, D7, D9, D10, dev_spi);
00095     if (motor->init() != COMPONENT_OK)
00096         exit(EXIT_FAILURE);
00097 
00098     /* Set defaults. */
00099     motor->set_acceleration(10000);
00100     motor->set_deceleration(10000);
00101     motor->set_min_speed(100);
00102     int status = 0;
00103     int speed = 0;
00104 
00105 
00106     /*----- Infinite Loop. -----*/
00107 
00108     /* Printing to the console. */
00109     printf("Motor Control with MEMS\r\n\n");
00110 
00111     /* Main Loop. */
00112     while(true)
00113     {
00114         /* Reading Accelerometer. */
00115         int accelerometer_data[3];
00116         accelerometer->get_x_axes(accelerometer_data);
00117 
00118         /* Motor Control. */
00119         int module = abs(accelerometer_data[acceleration_axis]);
00120         if (module > ACCELERATION_TH)
00121         {
00122             int sign = accelerometer_data[acceleration_axis] < 0 ? -1 : 1;
00123             speed = module * ROTATION_SPEED_GAIN;
00124 
00125             /* Requesting to run. */
00126             if (status != sign)
00127             {
00128                 motor->run(sign == -1 ? StepperMotor::BWD : StepperMotor::FWD);
00129                 status = sign;
00130             }
00131 
00132             /* Setting Speed. */
00133             motor->set_max_speed(speed);
00134 
00135             /* Printing to the console. */
00136             printf("Speed: %c%d\r\n", sign == -1 ? '-' : '+', motor->get_speed());
00137         }
00138         else if (status != 0)
00139         {
00140             /* Requesting to stop. */
00141             motor->soft_stop();
00142             status = 0;
00143             speed = 0;
00144 
00145             /* Printing to the console. */
00146             printf("Stop.\r\n");
00147         }
00148 
00149         /* Waiting. */
00150         wait_ms(50);
00151     }
00152 }