Diff: main.cpp

Revision:

1:8f312c1686b6

Parent:

0:becf0d313663

Child:

2:013921c26f43

diff -r becf0d313663 -r 8f312c1686b6 main.cpp
--- a/main.cpp	Thu Dec 03 12:17:33 2015 +0000
+++ b/main.cpp	Thu Dec 03 15:14:47 2015 +0000
@@ -1,13 +1,11 @@
 /**
  ******************************************************************************
  * @file    main.cpp
- * @author  Davide Aliprandi, STMicrolectronics
+ * @author  Fabio Brembilla
  * @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).
+ * @date    December 1st, 2015
+ * @brief   SunTracker + RemoteControl Vertical Application
+ *          This application use IHM01A1, 6180XA1, IKS01A1, IDB0XA1 expansion boards
  ******************************************************************************
  * @attention
  *
@@ -37,115 +35,258 @@
  *
  ******************************************************************************
  */
-
-
+ 
 /* Includes ------------------------------------------------------------------*/
-
+ 
 /* mbed specific header files. */
 #include "mbed.h"
-
+ 
 /* Helper header files. */
 #include "DevSPI.h"
+#include "DevI2C.h"
 
-/* Components and expansion boards specific header files. */
+/* Component specific header files. */
+#include "l6474_class.h"
+#include "x_nucleo_6180xa1.h"
 #include "x_nucleo_iks01a1.h"
-#include "l6474_class.h"
 
+/* C header files. */
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <assert.h>
 
 /* Definitions ---------------------------------------------------------------*/
 
-/* Absolute value of the threshold on the Y axis acceleration. */
-#define ACCELERATION_TH 50
-
-/* Rotation gain. */
-#define ROTATION_SPEED_GAIN 20
-
+#define SET_ACC 400     // Set Motor Acceleration
+#define SET_DEC 400     // Set Motor Deceleration
+#define SET_MAX 200     // Set Motor MaxSpeed
+#define SET_MIN 100     // Set Motor MinSpeed
+#define STOP 1000       // Set Motor Stop Position
+#define TOLLERANCE 100  // Tollerance between Left and Right before Start Movement
+#define RANGE_1 200     // Range 1 for Motor Speed
+#define RANGE_2 500     // Range 2 for Motor Speed
 
 /* Variables -----------------------------------------------------------------*/
 
-/* MEMS Expansion Board. */
-X_NUCLEO_IKS01A1 *x_nucleo_iks01a1;
+int16_t dir=0;          // Motor Rotation Direction: 0 = Stop, 1 = Anticlockwise, 2 = Clockwise
+int16_t changedir=0;    // Change Direction: 0 = No, 1 = Yes
+int16_t babybear=0;     // Difference (in Lux) between Left and Right
+int acc_data[3];        // Difference of Accelerometer
+int16_t diff=0;         // Abs of Babybear or Accelerometer difference
+int16_t left=0;         // Left Command for Rotate Direction
+int16_t right=0;        // Right Command for Rotate Direction
+int16_t start=0;        // Waiting User Button Push
+int32_t pos=0;          // Motor Position
+char DisplayStr[5];     // Status Display
+int16_t Display=0;      // Shown on Display: 0 = Motor Speed, 1 = Solar Panel Value, 2 = Manual Control
+int16_t status, status_t, status_b, status_l, status_r; // Babybear Status
+
+/* ---------------------------------------------------------------------------*/
 
 /* Motor Control Component. */
 L6474 *motor;
 
+/* Initializing SPI bus. */
+DevSPI dev_spi(D11, D12, D13);
 
-/* Main ----------------------------------------------------------------------*/
+/* Initializing I2C bus. */
+DevI2C dev_i2c(D14, D15);
+
+/* Instance board 6180XA1. */
+static X_NUCLEO_6180XA1 *board=X_NUCLEO_6180XA1::Instance(&dev_i2c, NC, NC, NC, NC);
+MeasureData_t data_sensor_top, data_sensor_bottom, data_sensor_left, data_sensor_right;
 
-int main()
-{
-    /*----- Initialization. -----*/
+/* Instance mems IKS01A1. */
+static X_NUCLEO_IKS01A1 *mems=X_NUCLEO_IKS01A1::Instance(&dev_i2c);
+MotionSensor *accelerometer = mems->GetAccelerometer();
+   
+void DISP_ExecLoopBody(void){};
 
-    /* Initializing I2C bus. */
-    DevI2C dev_i2c(D14, D15);
+AnalogIn analog_read_A1(A1);
+
+InterruptIn mybutton(USER_BUTTON); 
+
+/* User_Button_Pressed -------------------------------------------------------*/
 
-    /* Initializing SPI bus. */
-    DevSPI dev_spi(D11, D12, D13);
+void User_Button_Pressed()
+{
+  
+    if (start>0) { Display++; }
+    if (Display>2) { Display=0; } 
+    if (start==0) { start=1; }
+    
+}
+
+/* Initialization ------------------------------------------------------------*/
 
-    /* Initializing MEMS Expansion Board. */
-    x_nucleo_iks01a1 = X_NUCLEO_IKS01A1::Instance(&dev_i2c);
+bool Initialization(void)
+{
+  
+    /* Initializing Babybear Component. */
+    status=board->InitBoard();
+    if(status)
+        VL6180x_ErrLog("Failed to init the board!\n\r");
 
-    /* Retrieving the accelerometer. */
-    MotionSensor *accelerometer = x_nucleo_iks01a1->GetAccelerometer();
-    int acceleration_axis = x_nucleo_iks01a1->gyro_lsm6ds3 == NULL ? 0 : 1;
-
+    // Put GPIO not used as Interrupt in Hi-Z
+    status_t=board->sensor_top->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
+    //status_b=board->sensor_botton->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF); No Present
+    status_l=board->sensor_left->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);    
+    status_r=board->sensor_right->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
+      
     /* Initializing Motor Control Component. */
     motor = new L6474(D2, D8, D7, D9, D10, dev_spi);
     if (motor->Init(NULL) != COMPONENT_OK)
         return false;
+ 
+    motor->SetStepMode(STEP_MODE_1_8);  // Default is STEP_MODE_1_16
+ 
+    /* Set defaults Motor Speed. */
+    motor->SetAcceleration(SET_ACC);
+    motor->SetDeceleration(SET_DEC);
+    motor->SetMaxSpeed(SET_MAX);        // Variable by Light/Mems Sensors
+    motor->SetMinSpeed(SET_MIN);
+  
+    return true;
+    
+}
 
-    /* Set defaults. */
-    motor->SetAcceleration(10000);
-    motor->SetDeceleration(10000);
-    motor->SetMinSpeed(100);
-    int status = 0;
-    int speed = 0;
+/* Measure_Babybear ----------------------------------------------------------*/
+
+void Measure_Babybear(void)
+{
+
+    status_l=board->sensor_left->GetMeasurement(als_continuous_polling, &data_sensor_left);
+    status_r=board->sensor_right->GetMeasurement(als_continuous_polling, &data_sensor_right);
+
+    babybear = data_sensor_right.lux - data_sensor_left.lux;
+
+    diff = abs(babybear);
+
+    if (babybear>0) { left=0; right=1; }
+    if (babybear<0) { left=1; right=0; }
+ 
+}
+
+/* Measure_Accelerometer -----------------------------------------------------*/
+
+void Measure_Accelerometer(void)
+{
+
+    accelerometer->Get_X_Axes(acc_data);
+
+    diff = abs(acc_data[0]);
+
+    if (acc_data[0]>0) { left=0; right=1; }
+    if (acc_data[0]<0) { left=1; right=0; }
+    
+}
 
 
-    /*----- Infinite Loop. -----*/
+/* Control_Motor -------------------------------------------------------------*/
+
+void Control_Motor(void)
+{  
+        
+    //printf("Diff: %d lux/mems\n\r", diff);   
+    motor->SetMaxSpeed(diff);        
 
+    if (diff>TOLLERANCE)
+    {
+        if (diff <=RANGE_1) {
+            if (left)  { strcpy(DisplayStr,"E___"); }
+            if (right) { strcpy(DisplayStr,"___3"); }
+        }    
+        else if (diff >RANGE_1 & diff <=RANGE_2) {
+            if (left)  { strcpy(DisplayStr,"E==="); }
+            if (right) { strcpy(DisplayStr,"===3"); }
+        } 
+        else if (diff >RANGE_2) {
+            if (left)  { strcpy(DisplayStr,"E~~~"); }
+            if (right) { strcpy(DisplayStr,"~~~3"); }
+        }   
+      
+        // In Case of Change Direction
+        if (left & dir==2) { changedir=1; }
+        if (right & dir==1) { changedir=1; }
+        
+        // Run only if Stop or Change Direction
+        if (diff>TOLLERANCE & (dir==0 | changedir==1)) {
+            if (left) {  motor->Run(StepperMotor::FWD); dir=1; changedir=0; }
+            if (right) {  motor->Run(StepperMotor::BWD); dir=2; changedir=0; }
+        } 
+    }
+    
+    // Get Motor Position and Control Rotation Block
+    pos = motor->GetPosition();
+    if (pos>STOP | pos<-STOP) {
+        if (pos>0) { motor->GoTo(STOP); }
+        if (pos<0) { motor->GoTo(-STOP); }
+    }
+    
+    // Stop Motor
+    if (diff<=TOLLERANCE) {  
+        motor->HardStop();
+        if (Display==0) { strcpy(DisplayStr,"----"); }
+        if (Display==2) { strcpy(DisplayStr,"E  3"); }  
+        dir=0;
+        changedir=0;
+    }
+  
+}
+
+/* Measure_SolarPanel --------------------------------------------------------*/
+
+void Measure_SolarPanel(void)
+{
+
+    // AnalogIn A1: 0V return 0.0 , 3.3V return 1.0
+    float measure = analog_read_A1.read() * 3300;
+    //printf("Measure = %.0f mV\r\n", measure);
+    //board->display->DisplayDigit("A", 0);
+
+    if (Display==1) { sprintf(DisplayStr, "%.0f", measure); }  
+    
+    board->display->DisplayString(DisplayStr, 4); 
+
+}
+
+/* Main ----------------------------------------------------------------------*/
+
+int main()
+{ 
+  
+    Initialization();    
+    
+    mybutton.fall(&User_Button_Pressed);
+    
     /* Printing to the console. */
-    printf("Motor Control with MEMS\r\n\n");
-
+    printf("SunTracker by Fabio Brembilla\r\n\n");
+ 
+    /* Set Babybears. */
+    status_l=board->sensor_left->AlsSetAnalogueGain(3);
+    status_r=board->sensor_right->AlsSetAnalogueGain(3);
+    status_l=board->sensor_left->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
+    status_r=board->sensor_right->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
+    
+    /* Loop until push User Button to Set 0 Point. */
+    strcpy(DisplayStr,"pusH");
+    while(start<1)
+    {
+        board->display->DisplayString(DisplayStr, 4);
+    }
+    
     /* 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);
+        if (Display==0 | Display==1)    { Measure_Babybear(); }   
+        if (Display==2)                 { Measure_Accelerometer(); }   
+       
+        Control_Motor();
+        Measure_SolarPanel();
     }
+    
+    status_l=board->sensor_left->StopMeasurement(als_continuous_polling);
+    status_r=board->sensor_right->StopMeasurement(als_continuous_polling);
+    
 }