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-IKS01A1 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 "x_nucleo_iks01a1.h"
00052 #include "l6474_class.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 X_NUCLEO_IKS01A1 *x_nucleo_iks01a1;
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     x_nucleo_iks01a1 = X_NUCLEO_IKS01A1::Instance(&dev_i2c);
00087 
00088     /* Retrieving the accelerometer. */
00089     MotionSensor *accelerometer = x_nucleo_iks01a1->GetAccelerometer();
00090     int acceleration_axis = x_nucleo_iks01a1->gyro_lsm6ds3 == NULL ? 0 : 1;
00091 
00092     /* Initializing Motor Control Component. */
00093     motor = new L6474(D2, D8, D7, D9, D10, dev_spi);
00094     if (motor->Init() != COMPONENT_OK)
00095         exit(EXIT_FAILURE);
00096 
00097     /* Set defaults. */
00098     motor->SetAcceleration(10000);
00099     motor->SetDeceleration(10000);
00100     motor->SetMinSpeed(100);
00101     int status = 0;
00102     int speed = 0;
00103 
00104 
00105     /*----- Infinite Loop. -----*/
00106 
00107     /* Printing to the console. */
00108     printf("Motor Control with MEMS\r\n\n");
00109 
00110     /* Main Loop. */
00111     while(true)
00112     {
00113         /* Reading Accelerometer. */
00114         int accelerometer_data[3];
00115         accelerometer->Get_X_Axes(accelerometer_data);
00116 
00117         /* Motor Control. */
00118         int module = abs(accelerometer_data[acceleration_axis]);
00119         if (module > ACCELERATION_TH)
00120         {
00121             int sign = accelerometer_data[acceleration_axis] < 0 ? -1 : 1;
00122             speed = module * ROTATION_SPEED_GAIN;
00123             
00124             /* Requesting to run. */
00125             if (status != sign)
00126             {
00127                 motor->Run(sign == -1 ? StepperMotor::BWD : StepperMotor::FWD);
00128                 status = sign;
00129             }
00130 
00131             /* Setting Speed. */
00132             motor->SetMaxSpeed(speed);
00133 
00134             /* Printing to the console. */
00135             printf("Speed: %c%d\r\n", sign == -1 ? '-' : '+', motor->GetSpeed());
00136         }
00137         else if (status != 0)
00138         {
00139             /* Requesting to stop. */
00140             motor->SoftStop();
00141             status = 0;
00142             speed = 0;
00143 
00144             /* Printing to the console. */
00145             printf("Stop.\r\n");
00146         }
00147 
00148         /* Waiting. */
00149         wait_ms(50);
00150     }
00151 }