Diff: main.cpp

Revision:

0:becf0d313663

Child:

1:8f312c1686b6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 03 12:17:33 2015 +0000
@@ -0,0 +1,151 @@
+/**
+ ******************************************************************************
+ * @file    main.cpp
+ * @author  Davide Aliprandi, STMicrolectronics
+ * @version V1.0.0
+ * @date    October 16th, 2015
+ * @brief   mbed vertical application using the STMicrolectronics
+ *          X-NUCLEO-IHM01A1 Motor Control Expansion Board and the
+ *          X-NUCLEO-IKS01A1 MEMS Inertial & Environmental Sensors Expansion
+ *          Board to get a MEMS-based motor control (direction and speed).
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *   1. Redistributions of source code must retain the above copyright notice,
+ *      this list of conditions and the following disclaimer.
+ *   2. Redistributions in binary form must reproduce the above copyright notice,
+ *      this list of conditions and the following disclaimer in the documentation
+ *      and/or other materials provided with the distribution.
+ *   3. Neither the name of STMicroelectronics nor the names of its contributors
+ *      may be used to endorse or promote products derived from this software
+ *      without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+
+/* Includes ------------------------------------------------------------------*/
+
+/* mbed specific header files. */
+#include "mbed.h"
+
+/* Helper header files. */
+#include "DevSPI.h"
+
+/* Components and expansion boards specific header files. */
+#include "x_nucleo_iks01a1.h"
+#include "l6474_class.h"
+
+
+/* Definitions ---------------------------------------------------------------*/
+
+/* Absolute value of the threshold on the Y axis acceleration. */
+#define ACCELERATION_TH 50
+
+/* Rotation gain. */
+#define ROTATION_SPEED_GAIN 20
+
+
+/* Variables -----------------------------------------------------------------*/
+
+/* MEMS Expansion Board. */
+X_NUCLEO_IKS01A1 *x_nucleo_iks01a1;
+
+/* Motor Control Component. */
+L6474 *motor;
+
+
+/* Main ----------------------------------------------------------------------*/
+
+int main()
+{
+    /*----- Initialization. -----*/
+
+    /* Initializing I2C bus. */
+    DevI2C dev_i2c(D14, D15);
+
+    /* Initializing SPI bus. */
+    DevSPI dev_spi(D11, D12, D13);
+
+    /* Initializing MEMS Expansion Board. */
+    x_nucleo_iks01a1 = X_NUCLEO_IKS01A1::Instance(&dev_i2c);
+
+    /* Retrieving the accelerometer. */
+    MotionSensor *accelerometer = x_nucleo_iks01a1->GetAccelerometer();
+    int acceleration_axis = x_nucleo_iks01a1->gyro_lsm6ds3 == NULL ? 0 : 1;
+
+    /* Initializing Motor Control Component. */
+    motor = new L6474(D2, D8, D7, D9, D10, dev_spi);
+    if (motor->Init(NULL) != COMPONENT_OK)
+        return false;
+
+    /* Set defaults. */
+    motor->SetAcceleration(10000);
+    motor->SetDeceleration(10000);
+    motor->SetMinSpeed(100);
+    int status = 0;
+    int speed = 0;
+
+
+    /*----- Infinite Loop. -----*/
+
+    /* Printing to the console. */
+    printf("Motor Control with MEMS\r\n\n");
+
+    /* Main Loop. */
+    while(true)
+    {
+        /* Reading Accelerometer. */
+        int accelerometer_data[3];
+        accelerometer->Get_X_Axes(accelerometer_data);
+
+        /* Motor Control. */
+        int module = abs(accelerometer_data[acceleration_axis]);
+        if (module > ACCELERATION_TH)
+        {
+            int sign = accelerometer_data[acceleration_axis] < 0 ? -1 : 1;
+            speed = module * ROTATION_SPEED_GAIN;
+            
+            /* Requesting to run. */
+            if (status != sign)
+            {
+                motor->Run(sign == -1 ? StepperMotor::BWD : StepperMotor::FWD);
+                status = sign;
+            }
+
+            /* Setting Speed. */
+            motor->SetMaxSpeed(speed);
+
+            /* Printing to the console. */
+            printf("Speed: %c%d\r\n", sign == -1 ? '-' : '+', motor->GetSpeed());
+        }
+        else if (status != 0)
+        {
+            /* Requesting to stop. */
+            motor->SoftStop();
+            status = 0;
+            speed = 0;
+
+            /* Printing to the console. */
+            printf("Stop.\r\n");
+        }
+
+        /* Waiting. */
+        wait_ms(50);
+    }
+}