SunTracker_BLE
Dependencies: BLE_API X_NUCLEO_6180XA1 X_NUCLEO_IDB0XA1 X_NUCLEO_IHM01A1 X_NUCLEO_IKS01A1 mbed
Fork of SunTracker_BLE by
Overview
The SunTracker is a demo application running on ST Nucleo-F401RE stacking a set of ST X-NUCLEO expansion boards.
Main features provided are:
- A solar panel follows the light source, orienting the panel in order to achieve the best panel efficiency.
- Orientation is controlled thanks to a couple of VL6180X FlightSense light sensors mounted on a X-NUCLEO-6180XA1 expansion board and driven by X-NUCLEO-IHM01A1 controlled stepper motor acting as actuator to orientate the panel.
- The system features a progressive control on the stepper motor in order to modulate the panel rotation speed according to the light angle.
- The application is also able to control the panel productivity reading the panel voltage through an ADC and proving feedback on the local display.
- A manual orientation is possible by using the accelerometer on a X-NUCLEO-IKS01A1 expansion board that, according on board tilt, controls the speed and the rotate direction.
- A remote control is available using a X-NUCLEO-IDB04A1 or a X-NUCLEO-IDB05A1 Bluetooth Low Energy expansion board. Remote control software is here.
Working Status
- SunTracker has 3 working status visible on FlightSense display and switchable by pressing the User Button:
Status 0 (Idle)
- Motor: Free Turning
- Display: Waiting for User Button
Status 1
- Motor: Driven by Light
- Display: Direction and Light Intensity = Direction and Motor Speed
Status 2
- Motor: Driven by Light
- Display: Solar Panel Efficiency
Status 3
- Motor: Driven by Accelerometer
- Display: Direction and Accelerometer Intensity
Server Startup
- When you plug the power supply, the word ‘PUSH’ is shown on display.
- You can manually rotate the structure to assign the ‘Zero Point’. Then press the User Button to launch the application.
- The display will show this status, which means that the structureis oriented to maximize the efficiency of the solar panel.
- If there is a light displacement, the structure will rotate, left or right,to follow the light source and on display is shown the direction and the speed.
- You can press the User Button to show the panel efficiencywith 4 digits that represent the range from 0v (0000) to 3,3v (3300).
- Further pressing the User Button you will manual rotate the panel by tilt the Server or Client accelerometer depending by BLE connection.
Client Startup
- The Client application can remotely control the User Button and the Accelerometer functions.
- Power on the Client AFTER the Server, it will automatically search for the SunTracker and will establish a BLE connection.
- The Green Led on Nucleo Client board will be powered on.
Rotation Features
- It has been implemented a block of rotation to avoid cables twist.
- The blocking point can be set in the firmware by changing a constant.
- You can manually rotate the structure to assign the ‘Zero Point’ before press the User Button to launch the application.
- The system features a progressive control on the stepper motor in order to modulate the rotation speed according to the light or accelerometer angle.
List of Components
SERVER SunTracker_BLE
- Nucleo-F401RE platform using a STM32F401RET6 microcontroller.
- X-NUCLEO-IHM01A1 - Stepper motor driver board based on the EasySPIN L6474.
- X-NUCLEO-6180XA1 - 3-in-1 proximity and ambient light sensor board based on ST FlightSense technology.
- VL6180X-SATEL - Satellite boards compatible with X-NUCLEO-6180XA1 board.
- X-NUCLEO-IKS01A1 - Motion MEMS and environmental sensor board.
- X-NUCLEO-IDB04A1 or X-NUCLEO-IDB05A1 - Bluetooth Low Energy Bluetooth low energy evaluation board.
- Stepper Motor 400’’ (Part Number 5350401) - To orientate the Mechanical Structure.
- Solar Panel 0.446w (Part Number 0194127) - To capture sunlight and generate electrical current.
- Power Supply 12v (Part Number 7262993) - To provide power supply at the Stepper Motor.
- Flat Cable 6 ways (Part Number 1807010) - To plug VL6180X-SATEL with X-NUCLEO-6180XA1 (60cm length each x2).
- Cable Connector (Part Number 6737694) - To plug the Flat Cable (x4).
- Power Connector (Part Number 0487842) - To provide Power Supply to X-NUCLEO-IHM01A1.
CLIENT SunTracker_BLE_Remote
- Nucleo-F401RE platform using a STM32F401RET6 microcontroller.
- X-NUCLEO-IKS01A1 - Motion MEMS and environmental sensor board.
- X-NUCLEO-IDB04A1 or X-NUCLEO-IDB05A1- Bluetooth Low Energy Bluetooth low energy evaluation board.
MECHANICAL STRUCTURE
Find here the STL files to print with a 3D printer.
FLAT CABLE ASSEMBLY
HARDWARE SETUP
Nucleo ADC + Solar Panel
Connect Solar Panel cables to Nucleo Morpho PC_3 (white) and Nucleo Morpho GND (black). Connect a capacitor 10uF between PC_3 and GND to stabilize its voltage value shown on display.
EasySpin (L6474) + BLE
Hardware conflict between EasySpin DIR1 and BLE Reset, both on same Arduino Pin PA_8. Disconnect PA_8 between EasySpin and Nucleo by fold EasySpin Pin. PB_2 has been configured as EasySpin DIR1 in the firmware .Connect Nucleo Morpho PB_2 to FlightSense Arduino PA_8 by a wire.
FlightSense Satellites
In case of instability with I2C due to long flat cables, solder 4 SMD capacitors 47pF on FlightSense board in parallel between R15, R16, R17, R18 and plug 2 capacitors 15pF between FlightSense Arduino PB_8 and PB_9 to GND pin to cut-off noises over 720 KHz.
Arduino & Morpho Pinout
main.cpp@18:319a1bb8f837, 2016-04-22 (annotated)
- Committer:
- fabiombed
- Date:
- Fri Apr 22 14:40:14 2016 +0000
- Revision:
- 18:319a1bb8f837
- Parent:
- 17:582eba752042
- Child:
- 19:b2c04428ffed
Added ticker for services
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fabiombed | 0:becf0d313663 | 1 | /** |
fabiombed | 0:becf0d313663 | 2 | ****************************************************************************** |
fabiombed | 0:becf0d313663 | 3 | * @file main.cpp |
fabiombed | 1:8f312c1686b6 | 4 | * @author Fabio Brembilla |
fabiombed | 4:1d3d071a4c2c | 5 | * @version V2.0.0 |
fabiombed | 17:582eba752042 | 6 | * @date April, 2016 |
fabiombed | 12:1e6a1d3a66b5 | 7 | * @brief SunTracker + BLE (Server) Vertical Application |
fabiombed | 1:8f312c1686b6 | 8 | * This application use IHM01A1, 6180XA1, IKS01A1, IDB0XA1 expansion boards |
fabiombed | 0:becf0d313663 | 9 | ****************************************************************************** |
fabiombed | 0:becf0d313663 | 10 | * @attention |
fabiombed | 4:1d3d071a4c2c | 11 | * |
fabiombed | 4:1d3d071a4c2c | 12 | * <h2><center>© COPYRIGHT(c) 2016 STMicroelectronics</center></h2> |
fabiombed | 0:becf0d313663 | 13 | * |
fabiombed | 0:becf0d313663 | 14 | * Redistribution and use in source and binary forms, with or without modification, |
fabiombed | 0:becf0d313663 | 15 | * are permitted provided that the following conditions are met: |
fabiombed | 0:becf0d313663 | 16 | * 1. Redistributions of source code must retain the above copyright notice, |
fabiombed | 0:becf0d313663 | 17 | * this list of conditions and the following disclaimer. |
fabiombed | 0:becf0d313663 | 18 | * 2. Redistributions in binary form must reproduce the above copyright notice, |
fabiombed | 0:becf0d313663 | 19 | * this list of conditions and the following disclaimer in the documentation |
fabiombed | 0:becf0d313663 | 20 | * and/or other materials provided with the distribution. |
fabiombed | 0:becf0d313663 | 21 | * 3. Neither the name of STMicroelectronics nor the names of its contributors |
fabiombed | 0:becf0d313663 | 22 | * may be used to endorse or promote products derived from this software |
fabiombed | 0:becf0d313663 | 23 | * without specific prior written permission. |
fabiombed | 0:becf0d313663 | 24 | * |
fabiombed | 0:becf0d313663 | 25 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
fabiombed | 0:becf0d313663 | 26 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
fabiombed | 0:becf0d313663 | 27 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
fabiombed | 0:becf0d313663 | 28 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE |
fabiombed | 0:becf0d313663 | 29 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
fabiombed | 0:becf0d313663 | 30 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR |
fabiombed | 0:becf0d313663 | 31 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
fabiombed | 0:becf0d313663 | 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, |
fabiombed | 0:becf0d313663 | 33 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
fabiombed | 0:becf0d313663 | 34 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
fabiombed | 0:becf0d313663 | 35 | * |
fabiombed | 0:becf0d313663 | 36 | ****************************************************************************** |
fabiombed | 0:becf0d313663 | 37 | */ |
fabiombed | 4:1d3d071a4c2c | 38 | |
fabiombed | 14:644f9e7278e9 | 39 | /* Define --------------------------------------------------------------------*/ |
fabiombed | 14:644f9e7278e9 | 40 | |
fabiombed | 14:644f9e7278e9 | 41 | #define FlightSense //6180XA1 Main (remove only for debug) |
fabiombed | 14:644f9e7278e9 | 42 | #define EasySpin //IHM01A1 Main (remove only for debug) |
fabiombed | 14:644f9e7278e9 | 43 | #define Sensors //IKS01A1 Option |
fabiombed | 14:644f9e7278e9 | 44 | #define Ble //IDB0XA1 Option |
fabiombed | 14:644f9e7278e9 | 45 | |
fabiombed | 0:becf0d313663 | 46 | /* Includes ------------------------------------------------------------------*/ |
fabiombed | 4:1d3d071a4c2c | 47 | |
fabiombed | 6:4cbf7303b496 | 48 | // Mbed specific header files |
fabiombed | 0:becf0d313663 | 49 | #include "mbed.h" |
fabiombed | 4:1d3d071a4c2c | 50 | |
fabiombed | 6:4cbf7303b496 | 51 | // Helper header files |
fabiombed | 0:becf0d313663 | 52 | #include "DevSPI.h" |
fabiombed | 1:8f312c1686b6 | 53 | #include "DevI2C.h" |
fabiombed | 0:becf0d313663 | 54 | |
fabiombed | 6:4cbf7303b496 | 55 | // Component specific header files |
fabiombed | 14:644f9e7278e9 | 56 | #ifdef FlightSense |
fabiombed | 14:644f9e7278e9 | 57 | #include "x_nucleo_6180xa1.h" |
fabiombed | 14:644f9e7278e9 | 58 | #endif |
fabiombed | 14:644f9e7278e9 | 59 | |
fabiombed | 14:644f9e7278e9 | 60 | #ifdef EasySpin |
fabiombed | 14:644f9e7278e9 | 61 | #include "l6474_class.h" |
fabiombed | 14:644f9e7278e9 | 62 | #endif |
fabiombed | 14:644f9e7278e9 | 63 | |
fabiombed | 14:644f9e7278e9 | 64 | #ifdef Sensors |
fabiombed | 14:644f9e7278e9 | 65 | #include "x_nucleo_iks01a1.h" |
fabiombed | 14:644f9e7278e9 | 66 | #endif |
fabiombed | 0:becf0d313663 | 67 | |
fabiombed | 6:4cbf7303b496 | 68 | // C header files |
fabiombed | 1:8f312c1686b6 | 69 | #include <string.h> |
fabiombed | 1:8f312c1686b6 | 70 | #include <stdlib.h> |
fabiombed | 1:8f312c1686b6 | 71 | #include <stdio.h> |
fabiombed | 1:8f312c1686b6 | 72 | #include <assert.h> |
fabiombed | 0:becf0d313663 | 73 | |
fabiombed | 5:76fb6b783487 | 74 | /* BlueTooth -----------------------------------------------------------------*/ |
fabiombed | 5:76fb6b783487 | 75 | |
fabiombed | 14:644f9e7278e9 | 76 | #ifdef Ble |
fabiombed | 14:644f9e7278e9 | 77 | |
fabiombed | 5:76fb6b783487 | 78 | #include "debug.h" // Need for PRINTF |
fabiombed | 14:644f9e7278e9 | 79 | #include "Utils.h" // Need for STORE_LE_16 and STORE_LE_32 |
fabiombed | 5:76fb6b783487 | 80 | |
fabiombed | 17:582eba752042 | 81 | // Not need to re-define it, it is already defined into UUID.h, just use UUID:: when you call it |
fabiombed | 17:582eba752042 | 82 | //const unsigned LENGTH_OF_LONG_UUID = 16; |
fabiombed | 17:582eba752042 | 83 | //typedef uint16_t ShortUUIDBytes_t; |
fabiombed | 17:582eba752042 | 84 | //typedef uint8_t LongUUIDBytes_t[LENGTH_OF_LONG_UUID]; |
fabiombed | 8:144855fe02bd | 85 | |
fabiombed | 5:76fb6b783487 | 86 | typedef enum ConnectionStatus_t { |
fabiombed | 5:76fb6b783487 | 87 | DISCONNECTED =0, |
fabiombed | 5:76fb6b783487 | 88 | CONNECTED =1 |
fabiombed | 8:144855fe02bd | 89 | } cns_t; |
fabiombed | 5:76fb6b783487 | 90 | |
fabiombed | 8:144855fe02bd | 91 | #define BLE_DEV_NAME "SunTracker" |
fabiombed | 17:582eba752042 | 92 | //#define BLE_DEV_MAC 0xAA,0xBB,0xCC,0xDD,0xEE,0xFF // SERVER address (must be set into CLIENT BLE_peer_address_BE) |
fabiombed | 17:582eba752042 | 93 | #define BLE_DEV_MAC 0xFD,0x66,0x05,0x13,0xBE,0xBA // Default ST address (must be set into CLIENT BLE_peer_address_BE) |
fabiombed | 8:144855fe02bd | 94 | #define BLE_ADVERTISING_INTERVAL 1000 |
fabiombed | 5:76fb6b783487 | 95 | |
fabiombed | 5:76fb6b783487 | 96 | #include "CustomControlService.h" |
fabiombed | 5:76fb6b783487 | 97 | #include "CustomSensorsService.h" |
fabiombed | 5:76fb6b783487 | 98 | |
fabiombed | 6:4cbf7303b496 | 99 | static BLE *p_BLEdev = NULL; |
fabiombed | 7:54984d031243 | 100 | static CustomControlService *p_customcontrolservice = NULL; |
fabiombed | 7:54984d031243 | 101 | static CustomSensorService *p_customsensorservice = NULL; |
fabiombed | 7:54984d031243 | 102 | |
fabiombed | 18:319a1bb8f837 | 103 | #define FeatureStatus 0x00000800 |
fabiombed | 18:319a1bb8f837 | 104 | #define FeatureDifference 0x00001000 |
fabiombed | 18:319a1bb8f837 | 105 | #define FeaturePosition 0x00002000 |
fabiombed | 18:319a1bb8f837 | 106 | #define FeatureSunPanel 0x00004000 |
fabiombed | 15:019b8d60c89d | 107 | |
fabiombed | 14:644f9e7278e9 | 108 | #endif |
fabiombed | 14:644f9e7278e9 | 109 | |
fabiombed | 0:becf0d313663 | 110 | /* Definitions ---------------------------------------------------------------*/ |
fabiombed | 0:becf0d313663 | 111 | |
fabiombed | 1:8f312c1686b6 | 112 | #define SET_ACC 400 // Set Motor Acceleration |
fabiombed | 1:8f312c1686b6 | 113 | #define SET_DEC 400 // Set Motor Deceleration |
fabiombed | 1:8f312c1686b6 | 114 | #define SET_MAX 200 // Set Motor MaxSpeed |
fabiombed | 1:8f312c1686b6 | 115 | #define SET_MIN 100 // Set Motor MinSpeed |
fabiombed | 1:8f312c1686b6 | 116 | #define STOP 1000 // Set Motor Stop Position |
fabiombed | 18:319a1bb8f837 | 117 | #define TOLLERANCE 80 // Tollerance between Left and Right before Start Movement |
fabiombed | 18:319a1bb8f837 | 118 | #define RANGE_1 250 // Range 1 for Motor Speed |
fabiombed | 1:8f312c1686b6 | 119 | #define RANGE_2 500 // Range 2 for Motor Speed |
fabiombed | 18:319a1bb8f837 | 120 | #define TIMEOUT_STA 1 // Timeout for Ticker Status (in second) |
fabiombed | 18:319a1bb8f837 | 121 | #define TIMEOUT_DIF 0.5 // Timeout for Ticker Difference (in second) |
fabiombed | 18:319a1bb8f837 | 122 | #define TIMEOUT_POS 0.5 // Timeout for Ticker Position (in second) |
fabiombed | 18:319a1bb8f837 | 123 | #define TIMEOUT_SUN 0.5 // Timeout for Ticker SunPanel (in second) |
fabiombed | 0:becf0d313663 | 124 | |
fabiombed | 0:becf0d313663 | 125 | /* Variables -----------------------------------------------------------------*/ |
fabiombed | 0:becf0d313663 | 126 | |
fabiombed | 14:644f9e7278e9 | 127 | int8_t dir=0; // Motor Rotation Direction: 0 = Stop, 1 = Anticlockwise, 2 = Clockwise |
fabiombed | 14:644f9e7278e9 | 128 | int8_t changedir=0; // Change Direction: 0 = No, 1 = Yes |
fabiombed | 14:644f9e7278e9 | 129 | int8_t arrest=0; // Arrest: 0 = No, 1 = Yes |
fabiombed | 15:019b8d60c89d | 130 | int16_t babybear=0; // Difference (in Lux) between Left and Right |
fabiombed | 14:644f9e7278e9 | 131 | int32_t acc_data[3]; // Difference of Accelerometer |
fabiombed | 15:019b8d60c89d | 132 | int16_t diff=0; // Babybear or Accelerometer difference [--> Send BLE] |
fabiombed | 15:019b8d60c89d | 133 | int16_t diff_abs=0; // Abs of Babybear or Accelerometer difference |
fabiombed | 14:644f9e7278e9 | 134 | int8_t left=0; // Left Command for Rotate Direction |
fabiombed | 14:644f9e7278e9 | 135 | int8_t right=0; // Right Command for Rotate Direction |
fabiombed | 15:019b8d60c89d | 136 | int16_t pos=0; // Motor Position [--> Send BLE] |
fabiombed | 15:019b8d60c89d | 137 | int16_t measure=0; // ADC Value from SunPanel [--> Send BLE] |
fabiombed | 17:582eba752042 | 138 | char DisplayStr[5]; // Display String |
fabiombed | 17:582eba752042 | 139 | int16_t Status=0; // Status Shown on Display: 0 = Idle, 1 = Motor Speed, 2 = Solar Panel Value, 3 = Manual Control [--> Send BLE] |
fabiombed | 17:582eba752042 | 140 | int16_t status_bb, status_t, status_b, status_l, status_r; // Babybear Status |
fabiombed | 17:582eba752042 | 141 | uint16_t TimeStamp=0; // TimeStamp for BLE |
fabiombed | 18:319a1bb8f837 | 142 | bool tickerSta=false; |
fabiombed | 18:319a1bb8f837 | 143 | bool tickerDif=false; |
fabiombed | 18:319a1bb8f837 | 144 | bool tickerPos=false; |
fabiombed | 18:319a1bb8f837 | 145 | bool tickerSun=false; |
fabiombed | 1:8f312c1686b6 | 146 | |
fabiombed | 8:144855fe02bd | 147 | /* Initializations ------------------------------------------------------------*/ |
fabiombed | 8:144855fe02bd | 148 | |
fabiombed | 8:144855fe02bd | 149 | // Initializing SPI bus |
fabiombed | 8:144855fe02bd | 150 | DevSPI dev_spi(D11, D12, D13); |
fabiombed | 8:144855fe02bd | 151 | |
fabiombed | 8:144855fe02bd | 152 | // Initializing I2C bus |
fabiombed | 8:144855fe02bd | 153 | DevI2C dev_i2c(D14, D15); |
fabiombed | 8:144855fe02bd | 154 | |
fabiombed | 14:644f9e7278e9 | 155 | #ifdef FlightSense |
fabiombed | 14:644f9e7278e9 | 156 | // Initializing FlightSense Component 6180XA1 |
fabiombed | 8:144855fe02bd | 157 | static X_NUCLEO_6180XA1 *board; |
fabiombed | 8:144855fe02bd | 158 | MeasureData_t data_sensor_top, data_sensor_bottom, data_sensor_left, data_sensor_right; |
fabiombed | 14:644f9e7278e9 | 159 | #endif |
fabiombed | 14:644f9e7278e9 | 160 | |
fabiombed | 14:644f9e7278e9 | 161 | #ifdef EasySpin |
fabiombed | 14:644f9e7278e9 | 162 | // Initializing EasySpin Component IHM01A1 |
fabiombed | 14:644f9e7278e9 | 163 | static L6474 *motor; |
fabiombed | 14:644f9e7278e9 | 164 | #endif |
fabiombed | 14:644f9e7278e9 | 165 | |
fabiombed | 14:644f9e7278e9 | 166 | #ifdef Sensors |
fabiombed | 14:644f9e7278e9 | 167 | // Initializing Sensors Component IKS01A1 |
fabiombed | 8:144855fe02bd | 168 | static X_NUCLEO_IKS01A1 *mems; |
fabiombed | 8:144855fe02bd | 169 | MotionSensor *accelerometer; |
fabiombed | 14:644f9e7278e9 | 170 | #endif |
fabiombed | 8:144855fe02bd | 171 | |
fabiombed | 8:144855fe02bd | 172 | InterruptIn mybutton(USER_BUTTON); |
fabiombed | 8:144855fe02bd | 173 | |
fabiombed | 14:644f9e7278e9 | 174 | AnalogIn analog_read(PC_3); // A1 Conflict with BLE SPI_CS --> Changed in Morpho PC_3 |
fabiombed | 8:144855fe02bd | 175 | |
fabiombed | 17:582eba752042 | 176 | /* Update_Status ------------------------------------------------------------*/ |
fabiombed | 17:582eba752042 | 177 | |
fabiombed | 17:582eba752042 | 178 | void Update_Status(int16_t Stat) |
fabiombed | 17:582eba752042 | 179 | { |
fabiombed | 17:582eba752042 | 180 | |
fabiombed | 17:582eba752042 | 181 | char State[12]; // Characters into State must be <12 and never =>12 or it crash!!! |
fabiombed | 17:582eba752042 | 182 | if (Stat==1) strcpy(State,"FlightSense"); |
fabiombed | 17:582eba752042 | 183 | if (Stat==2) strcpy(State,"ADCSunPanel"); |
fabiombed | 17:582eba752042 | 184 | if (Stat==3) strcpy(State,"Sensors"); |
fabiombed | 17:582eba752042 | 185 | printf("\r\n\r\nPUSH Status: %s", State); |
fabiombed | 17:582eba752042 | 186 | |
fabiombed | 17:582eba752042 | 187 | } |
fabiombed | 17:582eba752042 | 188 | |
fabiombed | 8:144855fe02bd | 189 | /* User_Button_Pressed -------------------------------------------------------*/ |
fabiombed | 8:144855fe02bd | 190 | |
fabiombed | 14:644f9e7278e9 | 191 | void User_Button_Pressed(void) |
fabiombed | 8:144855fe02bd | 192 | { |
fabiombed | 8:144855fe02bd | 193 | |
fabiombed | 17:582eba752042 | 194 | Status++; |
fabiombed | 17:582eba752042 | 195 | |
fabiombed | 18:319a1bb8f837 | 196 | //#ifdef Sensors |
fabiombed | 18:319a1bb8f837 | 197 | #ifdef Ble |
fabiombed | 17:582eba752042 | 198 | if (Status>3) { Status=1; } |
fabiombed | 14:644f9e7278e9 | 199 | #else |
fabiombed | 17:582eba752042 | 200 | if (Status>2) { Status=1; } |
fabiombed | 14:644f9e7278e9 | 201 | #endif |
fabiombed | 14:644f9e7278e9 | 202 | |
fabiombed | 17:582eba752042 | 203 | Update_Status(Status); |
fabiombed | 8:144855fe02bd | 204 | |
fabiombed | 8:144855fe02bd | 205 | } |
fabiombed | 8:144855fe02bd | 206 | |
fabiombed | 14:644f9e7278e9 | 207 | #ifdef Ble |
fabiombed | 14:644f9e7278e9 | 208 | |
fabiombed | 9:ca289bf57f52 | 209 | /* Bluetooth CallBack ---------------------------------------------------------*/ |
fabiombed | 9:ca289bf57f52 | 210 | |
fabiombed | 9:ca289bf57f52 | 211 | static void onUpdatesEnabledCallback(GattAttribute::Handle_t handle) |
fabiombed | 9:ca289bf57f52 | 212 | { |
fabiombed | 17:582eba752042 | 213 | if (p_customcontrolservice->isCommandHandle(handle)) p_customcontrolservice->enNotify(handle); |
fabiombed | 17:582eba752042 | 214 | |
fabiombed | 17:582eba752042 | 215 | if (p_customsensorservice->isStatusHandle(handle)) p_customsensorservice->enNotify(handle); |
fabiombed | 15:019b8d60c89d | 216 | if (p_customsensorservice->isDifferenceHandle(handle)) p_customsensorservice->enNotify(handle); |
fabiombed | 15:019b8d60c89d | 217 | if (p_customsensorservice->isPositionHandle(handle)) p_customsensorservice->enNotify(handle); |
fabiombed | 15:019b8d60c89d | 218 | if (p_customsensorservice->isSunpanelHandle(handle)) p_customsensorservice->enNotify(handle); |
fabiombed | 9:ca289bf57f52 | 219 | } |
fabiombed | 9:ca289bf57f52 | 220 | |
fabiombed | 9:ca289bf57f52 | 221 | static void onUpdatesDisabledCallback(Gap::Handle_t handle) |
fabiombed | 9:ca289bf57f52 | 222 | { |
fabiombed | 17:582eba752042 | 223 | if (p_customcontrolservice->isCommandHandle(handle)) p_customcontrolservice->disNotify(handle); |
fabiombed | 17:582eba752042 | 224 | |
fabiombed | 17:582eba752042 | 225 | if (p_customsensorservice->isStatusHandle(handle)) p_customsensorservice->disNotify(handle); |
fabiombed | 15:019b8d60c89d | 226 | if (p_customsensorservice->isDifferenceHandle(handle)) p_customsensorservice->disNotify(handle); |
fabiombed | 15:019b8d60c89d | 227 | if (p_customsensorservice->isPositionHandle(handle)) p_customsensorservice->disNotify(handle); |
fabiombed | 15:019b8d60c89d | 228 | if (p_customsensorservice->isSunpanelHandle(handle)) p_customsensorservice->disNotify(handle); |
fabiombed | 9:ca289bf57f52 | 229 | } |
fabiombed | 9:ca289bf57f52 | 230 | |
fabiombed | 9:ca289bf57f52 | 231 | static void onDataReadCallback(const GattReadCallbackParams *eventDataP) |
fabiombed | 9:ca289bf57f52 | 232 | { |
fabiombed | 18:319a1bb8f837 | 233 | |
fabiombed | 17:582eba752042 | 234 | // if receive a manual Read request |
fabiombed | 17:582eba752042 | 235 | |
fabiombed | 17:582eba752042 | 236 | if (p_customsensorservice->isStatusHandle(eventDataP->handle)) |
fabiombed | 17:582eba752042 | 237 | { |
fabiombed | 17:582eba752042 | 238 | printf("\n\r\n\rSend BLE Display Status %d", Status); |
fabiombed | 17:582eba752042 | 239 | p_customsensorservice->sendEnvStatus(Status, TimeStamp); |
fabiombed | 17:582eba752042 | 240 | |
fabiombed | 17:582eba752042 | 241 | } else if (p_customsensorservice->isDifferenceHandle(eventDataP->handle)) { |
fabiombed | 17:582eba752042 | 242 | |
fabiombed | 17:582eba752042 | 243 | printf("\n\rSend BLE Difference %d lux/mems", diff); // Send BLE diff, no diff_abs |
fabiombed | 17:582eba752042 | 244 | p_customsensorservice->sendEnvDifference(diff, TimeStamp); |
fabiombed | 17:582eba752042 | 245 | |
fabiombed | 17:582eba752042 | 246 | } else if (p_customsensorservice->isPositionHandle(eventDataP->handle)) { |
fabiombed | 17:582eba752042 | 247 | |
fabiombed | 17:582eba752042 | 248 | printf("\n\rSend BLE Position %d", pos); |
fabiombed | 17:582eba752042 | 249 | p_customsensorservice->sendEnvPosition(pos, TimeStamp); |
fabiombed | 17:582eba752042 | 250 | |
fabiombed | 17:582eba752042 | 251 | } else if (p_customsensorservice->isSunpanelHandle(eventDataP->handle)) { |
fabiombed | 17:582eba752042 | 252 | |
fabiombed | 17:582eba752042 | 253 | printf("\n\rSend BLE Sunpanel %d mV", measure); |
fabiombed | 17:582eba752042 | 254 | p_customsensorservice->sendEnvSunpanel(measure, TimeStamp); |
fabiombed | 17:582eba752042 | 255 | |
fabiombed | 17:582eba752042 | 256 | } |
fabiombed | 18:319a1bb8f837 | 257 | |
fabiombed | 9:ca289bf57f52 | 258 | } |
fabiombed | 9:ca289bf57f52 | 259 | |
fabiombed | 14:644f9e7278e9 | 260 | // This Callback happen when it RECEIVE a WRITE |
fabiombed | 9:ca289bf57f52 | 261 | static void myonDataWriteCallback(const GattWriteCallbackParams *eventDataP) |
fabiombed | 14:644f9e7278e9 | 262 | { |
fabiombed | 15:019b8d60c89d | 263 | |
fabiombed | 17:582eba752042 | 264 | if (p_customcontrolservice->isCommandHandle(eventDataP->handle)) |
fabiombed | 17:582eba752042 | 265 | { |
fabiombed | 17:582eba752042 | 266 | // From CLIENT it receives a command in this format: Feature (4) + Type (1) + Data (2) |
fabiombed | 17:582eba752042 | 267 | uint32_t Feature = eventDataP->data[0]<<24 | eventDataP->data[1]<<16 | eventDataP->data[2]<<8 | eventDataP->data[3]; |
fabiombed | 17:582eba752042 | 268 | uint8_t Type = eventDataP->data[4]; |
fabiombed | 17:582eba752042 | 269 | int16_t Data = eventDataP->data[5]<<8 | eventDataP->data[6]; |
fabiombed | 14:644f9e7278e9 | 270 | |
fabiombed | 17:582eba752042 | 271 | //printf("\r\n\r\nmyonDataWriteCallback (Feature %x)", Feature); |
fabiombed | 17:582eba752042 | 272 | //printf("\r\nmyonDataWriteCallback (Type %x)", Type); // Not Used |
fabiombed | 17:582eba752042 | 273 | //printf("\r\nmyonDataWriteCallback (Data Hex %x)", Data); |
fabiombed | 17:582eba752042 | 274 | //printf("\r\nmyonDataWriteCallback (Data Dec %d)", Data); |
fabiombed | 17:582eba752042 | 275 | |
fabiombed | 17:582eba752042 | 276 | switch(Feature) |
fabiombed | 17:582eba752042 | 277 | { |
fabiombed | 17:582eba752042 | 278 | case FeatureStatus: |
fabiombed | 17:582eba752042 | 279 | printf("\r\n\r\nReceived STATUS %d", Data); |
fabiombed | 17:582eba752042 | 280 | Status = Data; |
fabiombed | 17:582eba752042 | 281 | Update_Status(Data); |
fabiombed | 17:582eba752042 | 282 | break; |
fabiombed | 17:582eba752042 | 283 | |
fabiombed | 17:582eba752042 | 284 | case FeatureDifference: |
fabiombed | 17:582eba752042 | 285 | printf("\r\n\r\nReceived DIFF %d", Data); |
fabiombed | 17:582eba752042 | 286 | diff = Data; |
fabiombed | 17:582eba752042 | 287 | break; |
fabiombed | 17:582eba752042 | 288 | |
fabiombed | 17:582eba752042 | 289 | case FeaturePosition: |
fabiombed | 17:582eba752042 | 290 | break; |
fabiombed | 17:582eba752042 | 291 | |
fabiombed | 17:582eba752042 | 292 | case FeatureSunPanel: |
fabiombed | 17:582eba752042 | 293 | break; |
fabiombed | 17:582eba752042 | 294 | |
fabiombed | 17:582eba752042 | 295 | default: |
fabiombed | 17:582eba752042 | 296 | break; |
fabiombed | 17:582eba752042 | 297 | } |
fabiombed | 15:019b8d60c89d | 298 | } |
fabiombed | 15:019b8d60c89d | 299 | |
fabiombed | 9:ca289bf57f52 | 300 | } |
fabiombed | 9:ca289bf57f52 | 301 | |
fabiombed | 9:ca289bf57f52 | 302 | static void onConnectionCallback(const Gap::ConnectionCallbackParams_t * connectionParams) |
fabiombed | 9:ca289bf57f52 | 303 | { |
fabiombed | 14:644f9e7278e9 | 304 | printf("\r\n\r\nonConnectionCallback (Line %d)", __LINE__); |
fabiombed | 9:ca289bf57f52 | 305 | } |
fabiombed | 9:ca289bf57f52 | 306 | |
fabiombed | 9:ca289bf57f52 | 307 | static void onDisconnectionCallback(const Gap::DisconnectionCallbackParams_t * disConnectionReason) |
fabiombed | 9:ca289bf57f52 | 308 | { |
fabiombed | 14:644f9e7278e9 | 309 | printf("\r\n\r\nonDisconnectionCallback (Line %d)", __LINE__); |
fabiombed | 9:ca289bf57f52 | 310 | p_BLEdev->gap().startAdvertising(); |
fabiombed | 9:ca289bf57f52 | 311 | } |
fabiombed | 9:ca289bf57f52 | 312 | |
fabiombed | 14:644f9e7278e9 | 313 | #endif |
fabiombed | 14:644f9e7278e9 | 314 | |
fabiombed | 9:ca289bf57f52 | 315 | /* Bluetooth Initialization ---------------------------------------------------*/ |
fabiombed | 9:ca289bf57f52 | 316 | |
fabiombed | 9:ca289bf57f52 | 317 | bool BLE_Initialization(void) |
fabiombed | 9:ca289bf57f52 | 318 | { |
fabiombed | 9:ca289bf57f52 | 319 | |
fabiombed | 14:644f9e7278e9 | 320 | #ifdef Ble |
fabiombed | 14:644f9e7278e9 | 321 | |
fabiombed | 9:ca289bf57f52 | 322 | p_BLEdev = new BLE; |
fabiombed | 9:ca289bf57f52 | 323 | if (!p_BLEdev) { printf("\r\nBLE Device creation failed\r\n"); } |
fabiombed | 9:ca289bf57f52 | 324 | const Gap::Address_t BLE_address_BE = {BLE_DEV_MAC}; |
fabiombed | 9:ca289bf57f52 | 325 | p_BLEdev->gap().setAddress(BLEProtocol::AddressType::PUBLIC, BLE_address_BE); |
fabiombed | 9:ca289bf57f52 | 326 | |
fabiombed | 9:ca289bf57f52 | 327 | p_BLEdev->init(); |
fabiombed | 9:ca289bf57f52 | 328 | |
fabiombed | 9:ca289bf57f52 | 329 | // Set BLE CallBack Functions |
fabiombed | 9:ca289bf57f52 | 330 | p_BLEdev->gattServer().onUpdatesEnabled(onUpdatesEnabledCallback); |
fabiombed | 9:ca289bf57f52 | 331 | p_BLEdev->gattServer().onUpdatesDisabled(onUpdatesDisabledCallback); |
fabiombed | 9:ca289bf57f52 | 332 | p_BLEdev->gattServer().onDataRead(onDataReadCallback); |
fabiombed | 9:ca289bf57f52 | 333 | p_BLEdev->gattServer().onDataWritten(myonDataWriteCallback); |
fabiombed | 9:ca289bf57f52 | 334 | p_BLEdev->gap().onConnection(onConnectionCallback); |
fabiombed | 9:ca289bf57f52 | 335 | p_BLEdev->gap().onDisconnection(onDisconnectionCallback); |
fabiombed | 9:ca289bf57f52 | 336 | //p_BLEdev->gattServer().onConfirmationReceived(onConfirmationReceivedCallback); |
fabiombed | 9:ca289bf57f52 | 337 | //p_BLEdev->gattServer().onDataSent(onDataSentCallback); |
fabiombed | 9:ca289bf57f52 | 338 | //p_BLEdev->gap().onTimeout(onTimeoutCallback); |
fabiombed | 9:ca289bf57f52 | 339 | |
fabiombed | 9:ca289bf57f52 | 340 | // BLE Services |
fabiombed | 14:644f9e7278e9 | 341 | p_customcontrolservice = new CustomControlService(*p_BLEdev); |
fabiombed | 9:ca289bf57f52 | 342 | p_customsensorservice = new CustomSensorService(*p_BLEdev); |
fabiombed | 9:ca289bf57f52 | 343 | |
fabiombed | 9:ca289bf57f52 | 344 | // Setup BLE Advertising |
fabiombed | 9:ca289bf57f52 | 345 | const static char DEVICE_NAME[] = BLE_DEV_NAME; |
fabiombed | 9:ca289bf57f52 | 346 | p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); |
fabiombed | 18:319a1bb8f837 | 347 | uint8_t dat[] = {0x01,0x80,0x00,0x00,0x78,0x00}; // Data Sent during Advertising, must respect a Standard ST |
fabiombed | 9:ca289bf57f52 | 348 | p_BLEdev->gap().accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,dat,6); |
fabiombed | 9:ca289bf57f52 | 349 | p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::UNKNOWN); |
fabiombed | 9:ca289bf57f52 | 350 | p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); |
fabiombed | 9:ca289bf57f52 | 351 | p_BLEdev->gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
fabiombed | 9:ca289bf57f52 | 352 | p_BLEdev->gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL); |
fabiombed | 9:ca289bf57f52 | 353 | p_BLEdev->gap().startAdvertising(); |
fabiombed | 9:ca289bf57f52 | 354 | |
fabiombed | 14:644f9e7278e9 | 355 | printf("Initialization Ble OK (Line %d)\r\n", __LINE__); |
fabiombed | 14:644f9e7278e9 | 356 | |
fabiombed | 14:644f9e7278e9 | 357 | #endif |
fabiombed | 14:644f9e7278e9 | 358 | |
fabiombed | 9:ca289bf57f52 | 359 | return true; |
fabiombed | 9:ca289bf57f52 | 360 | |
fabiombed | 9:ca289bf57f52 | 361 | } |
fabiombed | 9:ca289bf57f52 | 362 | |
fabiombed | 8:144855fe02bd | 363 | /* Initialization ------------------------------------------------------------*/ |
fabiombed | 8:144855fe02bd | 364 | |
fabiombed | 8:144855fe02bd | 365 | bool Initialization(void) |
fabiombed | 8:144855fe02bd | 366 | { |
fabiombed | 11:b5e23d64d366 | 367 | |
fabiombed | 14:644f9e7278e9 | 368 | dev_i2c.frequency(100000); // Default 100KHz. At 10KHz, 20KHz, 200KHz and 400KHz doesn't work |
fabiombed | 11:b5e23d64d366 | 369 | |
fabiombed | 11:b5e23d64d366 | 370 | //---- |
fabiombed | 14:644f9e7278e9 | 371 | |
fabiombed | 14:644f9e7278e9 | 372 | #ifdef FlightSense |
fabiombed | 14:644f9e7278e9 | 373 | |
fabiombed | 14:644f9e7278e9 | 374 | // Initializing FlightSense Component |
fabiombed | 14:644f9e7278e9 | 375 | board=X_NUCLEO_6180XA1::Instance(&dev_i2c, NC, NC, NC, NC); // NC as Interrupt fixed from FlightSense library 42 |
fabiombed | 14:644f9e7278e9 | 376 | |
fabiombed | 17:582eba752042 | 377 | status_bb=board->InitBoard(); |
fabiombed | 17:582eba752042 | 378 | if(status_bb) VL6180x_ErrLog("Failed to init the board!\n\r"); |
fabiombed | 8:144855fe02bd | 379 | |
fabiombed | 8:144855fe02bd | 380 | // Put GPIO not used as Interrupt in Hi-Z |
fabiombed | 8:144855fe02bd | 381 | status_t=board->sensor_top->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF); |
fabiombed | 14:644f9e7278e9 | 382 | //status_b=board->sensor_botton->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF); Not Present |
fabiombed | 8:144855fe02bd | 383 | status_l=board->sensor_left->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF); |
fabiombed | 8:144855fe02bd | 384 | status_r=board->sensor_right->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF); |
fabiombed | 8:144855fe02bd | 385 | |
fabiombed | 8:144855fe02bd | 386 | // Set Babybears |
fabiombed | 8:144855fe02bd | 387 | status_l=board->sensor_left->AlsSetAnalogueGain(3); |
fabiombed | 8:144855fe02bd | 388 | status_r=board->sensor_right->AlsSetAnalogueGain(3); |
fabiombed | 8:144855fe02bd | 389 | status_l=board->sensor_left->StartMeasurement(als_continuous_polling, NULL, NULL, NULL); |
fabiombed | 8:144855fe02bd | 390 | status_r=board->sensor_right->StartMeasurement(als_continuous_polling, NULL, NULL, NULL); |
fabiombed | 8:144855fe02bd | 391 | |
fabiombed | 14:644f9e7278e9 | 392 | printf("Init FlightSense OK\r\n"); |
fabiombed | 14:644f9e7278e9 | 393 | |
fabiombed | 14:644f9e7278e9 | 394 | #endif |
fabiombed | 11:b5e23d64d366 | 395 | |
fabiombed | 8:144855fe02bd | 396 | //---- |
fabiombed | 8:144855fe02bd | 397 | |
fabiombed | 14:644f9e7278e9 | 398 | #ifdef EasySpin |
fabiombed | 11:b5e23d64d366 | 399 | |
fabiombed | 14:644f9e7278e9 | 400 | // Initializing EasySpin Component |
fabiombed | 9:ca289bf57f52 | 401 | motor = new L6474(D2, D8, PB_2, D9, D10, dev_spi); // D7 conflict with BNRG_RST (when motor change direction 0-1 reset BLE) |
fabiombed | 9:ca289bf57f52 | 402 | // Used Morpho PB_2 and made HW wiring |
fabiombed | 8:144855fe02bd | 403 | if (motor->Init(NULL) != COMPONENT_OK) |
fabiombed | 8:144855fe02bd | 404 | return false; |
fabiombed | 8:144855fe02bd | 405 | |
fabiombed | 16:d69c0d5d5ab2 | 406 | motor->SetStepMode(StepperMotor::STEP_MODE_1_8); // Default is STEP_MODE_1_16 |
fabiombed | 8:144855fe02bd | 407 | |
fabiombed | 8:144855fe02bd | 408 | // Set defaults Motor Speed |
fabiombed | 8:144855fe02bd | 409 | motor->SetAcceleration(SET_ACC); |
fabiombed | 8:144855fe02bd | 410 | motor->SetDeceleration(SET_DEC); |
fabiombed | 8:144855fe02bd | 411 | motor->SetMaxSpeed(SET_MAX); // Variable by Light/Mems Sensors |
fabiombed | 8:144855fe02bd | 412 | motor->SetMinSpeed(SET_MIN); |
fabiombed | 8:144855fe02bd | 413 | |
fabiombed | 14:644f9e7278e9 | 414 | printf("Init EasySpin OK\r\n"); |
fabiombed | 14:644f9e7278e9 | 415 | |
fabiombed | 14:644f9e7278e9 | 416 | #endif |
fabiombed | 14:644f9e7278e9 | 417 | |
fabiombed | 14:644f9e7278e9 | 418 | //---- |
fabiombed | 14:644f9e7278e9 | 419 | |
fabiombed | 14:644f9e7278e9 | 420 | #ifdef Sensors |
fabiombed | 11:b5e23d64d366 | 421 | |
fabiombed | 14:644f9e7278e9 | 422 | // Initializing Sensors Component |
fabiombed | 14:644f9e7278e9 | 423 | mems=X_NUCLEO_IKS01A1::Instance(&dev_i2c); |
fabiombed | 14:644f9e7278e9 | 424 | accelerometer = mems->GetAccelerometer(); |
fabiombed | 14:644f9e7278e9 | 425 | |
fabiombed | 14:644f9e7278e9 | 426 | printf("Init Sensors OK\r\n"); |
fabiombed | 14:644f9e7278e9 | 427 | |
fabiombed | 14:644f9e7278e9 | 428 | #endif |
fabiombed | 14:644f9e7278e9 | 429 | |
fabiombed | 14:644f9e7278e9 | 430 | printf("Initialization OK (Line %d)\r\n", __LINE__); |
fabiombed | 8:144855fe02bd | 431 | return true; |
fabiombed | 8:144855fe02bd | 432 | |
fabiombed | 8:144855fe02bd | 433 | } |
fabiombed | 8:144855fe02bd | 434 | |
fabiombed | 8:144855fe02bd | 435 | /* Measure_Babybear ----------------------------------------------------------*/ |
fabiombed | 8:144855fe02bd | 436 | |
fabiombed | 8:144855fe02bd | 437 | void Measure_Babybear(void) |
fabiombed | 8:144855fe02bd | 438 | { |
fabiombed | 8:144855fe02bd | 439 | |
fabiombed | 14:644f9e7278e9 | 440 | #ifdef FlightSense |
fabiombed | 14:644f9e7278e9 | 441 | |
fabiombed | 8:144855fe02bd | 442 | status_l=board->sensor_left->GetMeasurement(als_continuous_polling, &data_sensor_left); |
fabiombed | 8:144855fe02bd | 443 | status_r=board->sensor_right->GetMeasurement(als_continuous_polling, &data_sensor_right); |
fabiombed | 8:144855fe02bd | 444 | |
fabiombed | 8:144855fe02bd | 445 | babybear = data_sensor_right.lux - data_sensor_left.lux; |
fabiombed | 8:144855fe02bd | 446 | |
fabiombed | 14:644f9e7278e9 | 447 | diff = babybear; |
fabiombed | 1:8f312c1686b6 | 448 | |
fabiombed | 14:644f9e7278e9 | 449 | #endif |
fabiombed | 8:144855fe02bd | 450 | |
fabiombed | 8:144855fe02bd | 451 | } |
fabiombed | 8:144855fe02bd | 452 | |
fabiombed | 8:144855fe02bd | 453 | /* Measure_Accelerometer -----------------------------------------------------*/ |
fabiombed | 8:144855fe02bd | 454 | |
fabiombed | 8:144855fe02bd | 455 | void Measure_Accelerometer(void) |
fabiombed | 8:144855fe02bd | 456 | { |
fabiombed | 8:144855fe02bd | 457 | |
fabiombed | 14:644f9e7278e9 | 458 | #ifdef Sensors |
fabiombed | 14:644f9e7278e9 | 459 | |
fabiombed | 8:144855fe02bd | 460 | accelerometer->Get_X_Axes(acc_data); |
fabiombed | 8:144855fe02bd | 461 | |
fabiombed | 14:644f9e7278e9 | 462 | diff = acc_data[0]; |
fabiombed | 8:144855fe02bd | 463 | |
fabiombed | 14:644f9e7278e9 | 464 | #endif |
fabiombed | 8:144855fe02bd | 465 | |
fabiombed | 8:144855fe02bd | 466 | } |
fabiombed | 8:144855fe02bd | 467 | |
fabiombed | 8:144855fe02bd | 468 | /* Control_Motor -------------------------------------------------------------*/ |
fabiombed | 8:144855fe02bd | 469 | |
fabiombed | 8:144855fe02bd | 470 | void Control_Motor(void) |
fabiombed | 8:144855fe02bd | 471 | { |
fabiombed | 8:144855fe02bd | 472 | |
fabiombed | 14:644f9e7278e9 | 473 | #ifdef EasySpin |
fabiombed | 14:644f9e7278e9 | 474 | |
fabiombed | 14:644f9e7278e9 | 475 | diff_abs = abs(diff); |
fabiombed | 8:144855fe02bd | 476 | |
fabiombed | 14:644f9e7278e9 | 477 | //printf("Diff: %d lux/mems\n\r", diff_abs); |
fabiombed | 14:644f9e7278e9 | 478 | motor->SetMaxSpeed(diff_abs); |
fabiombed | 14:644f9e7278e9 | 479 | |
fabiombed | 14:644f9e7278e9 | 480 | if (diff>0) { left=0; right=1; } |
fabiombed | 14:644f9e7278e9 | 481 | if (diff<0) { left=1; right=0; } |
fabiombed | 14:644f9e7278e9 | 482 | |
fabiombed | 14:644f9e7278e9 | 483 | if (diff_abs>TOLLERANCE) { |
fabiombed | 14:644f9e7278e9 | 484 | if (diff_abs <=RANGE_1) |
fabiombed | 8:144855fe02bd | 485 | { |
fabiombed | 8:144855fe02bd | 486 | if (left) { strcpy(DisplayStr,"E___"); } |
fabiombed | 8:144855fe02bd | 487 | if (right) { strcpy(DisplayStr,"___3"); } |
fabiombed | 8:144855fe02bd | 488 | } |
fabiombed | 14:644f9e7278e9 | 489 | else if (diff_abs >RANGE_1 & diff_abs <=RANGE_2) |
fabiombed | 8:144855fe02bd | 490 | { |
fabiombed | 8:144855fe02bd | 491 | if (left) { strcpy(DisplayStr,"E==="); } |
fabiombed | 8:144855fe02bd | 492 | if (right) { strcpy(DisplayStr,"===3"); } |
fabiombed | 8:144855fe02bd | 493 | } |
fabiombed | 14:644f9e7278e9 | 494 | else if (diff_abs >RANGE_2) |
fabiombed | 8:144855fe02bd | 495 | { |
fabiombed | 8:144855fe02bd | 496 | if (left) { strcpy(DisplayStr,"E~~~"); } |
fabiombed | 8:144855fe02bd | 497 | if (right) { strcpy(DisplayStr,"~~~3"); } |
fabiombed | 8:144855fe02bd | 498 | } |
fabiombed | 8:144855fe02bd | 499 | |
fabiombed | 8:144855fe02bd | 500 | // In Case of Change Direction |
fabiombed | 8:144855fe02bd | 501 | if (left & dir==2) { changedir=1; } |
fabiombed | 8:144855fe02bd | 502 | if (right & dir==1) { changedir=1; } |
fabiombed | 0:becf0d313663 | 503 | |
fabiombed | 8:144855fe02bd | 504 | // Run only if Stop or Change Direction |
fabiombed | 14:644f9e7278e9 | 505 | if (diff_abs>TOLLERANCE & (dir==0 | changedir==1)) { |
fabiombed | 8:144855fe02bd | 506 | if (left) { motor->Run(StepperMotor::FWD); dir=1; changedir=0; } |
fabiombed | 8:144855fe02bd | 507 | if (right) { motor->Run(StepperMotor::BWD); dir=2; changedir=0; } |
fabiombed | 14:644f9e7278e9 | 508 | printf("\n\rRUN"); |
fabiombed | 8:144855fe02bd | 509 | } |
fabiombed | 8:144855fe02bd | 510 | } |
fabiombed | 8:144855fe02bd | 511 | |
fabiombed | 8:144855fe02bd | 512 | // Get Motor Position and Control Rotation Block |
fabiombed | 8:144855fe02bd | 513 | pos = motor->GetPosition(); |
fabiombed | 8:144855fe02bd | 514 | if (pos>STOP | pos<-STOP) { |
fabiombed | 8:144855fe02bd | 515 | if (pos>0) { motor->GoTo(STOP); } |
fabiombed | 8:144855fe02bd | 516 | if (pos<0) { motor->GoTo(-STOP); } |
fabiombed | 14:644f9e7278e9 | 517 | printf("\n\rGOTO"); |
fabiombed | 8:144855fe02bd | 518 | } |
fabiombed | 1:8f312c1686b6 | 519 | |
fabiombed | 8:144855fe02bd | 520 | // Stop Motor |
fabiombed | 14:644f9e7278e9 | 521 | if (diff_abs<=TOLLERANCE) { |
fabiombed | 8:144855fe02bd | 522 | arrest=1; |
fabiombed | 17:582eba752042 | 523 | if (Status==1) { strcpy(DisplayStr,"----"); } |
fabiombed | 17:582eba752042 | 524 | if (Status==3) { strcpy(DisplayStr,"E 3"); } |
fabiombed | 8:144855fe02bd | 525 | } |
fabiombed | 8:144855fe02bd | 526 | |
fabiombed | 8:144855fe02bd | 527 | if (arrest==1 && dir!=0 ) { // Stop just one time |
fabiombed | 8:144855fe02bd | 528 | motor->HardStop(); |
fabiombed | 8:144855fe02bd | 529 | dir=0; |
fabiombed | 8:144855fe02bd | 530 | changedir=0; |
fabiombed | 8:144855fe02bd | 531 | arrest=0; |
fabiombed | 14:644f9e7278e9 | 532 | printf("\n\rSTOP"); |
fabiombed | 8:144855fe02bd | 533 | } |
fabiombed | 8:144855fe02bd | 534 | |
fabiombed | 14:644f9e7278e9 | 535 | #endif |
fabiombed | 14:644f9e7278e9 | 536 | |
fabiombed | 8:144855fe02bd | 537 | } |
fabiombed | 8:144855fe02bd | 538 | |
fabiombed | 8:144855fe02bd | 539 | /* Measure_SolarPanel --------------------------------------------------------*/ |
fabiombed | 8:144855fe02bd | 540 | |
fabiombed | 8:144855fe02bd | 541 | void Measure_SolarPanel(void) |
fabiombed | 8:144855fe02bd | 542 | { |
fabiombed | 8:144855fe02bd | 543 | |
fabiombed | 8:144855fe02bd | 544 | // AnalogIn: 0V return 0.0 , 3.3V return 1.0 |
fabiombed | 14:644f9e7278e9 | 545 | measure = analog_read.read() * 3300; |
fabiombed | 8:144855fe02bd | 546 | //printf("Measure = %.0f mV\r\n", measure); |
fabiombed | 8:144855fe02bd | 547 | //board->display->DisplayDigit("A", 0); |
fabiombed | 8:144855fe02bd | 548 | |
fabiombed | 17:582eba752042 | 549 | if (Status==2) { sprintf(DisplayStr, "%d", measure); } |
fabiombed | 14:644f9e7278e9 | 550 | |
fabiombed | 8:144855fe02bd | 551 | } |
fabiombed | 1:8f312c1686b6 | 552 | |
fabiombed | 18:319a1bb8f837 | 553 | /* Ticker --------------------------------------------------------------------*/ |
fabiombed | 18:319a1bb8f837 | 554 | |
fabiombed | 18:319a1bb8f837 | 555 | void tickerSta_Callback(void) { tickerSta=true; } |
fabiombed | 18:319a1bb8f837 | 556 | void tickerDif_Callback(void) { tickerDif=true; } |
fabiombed | 18:319a1bb8f837 | 557 | void tickerPos_Callback(void) { tickerPos=true; } |
fabiombed | 18:319a1bb8f837 | 558 | void tickerSun_Callback(void) { tickerSun=true; } |
fabiombed | 18:319a1bb8f837 | 559 | |
fabiombed | 8:144855fe02bd | 560 | /* Main ----------------------------------------------------------------------*/ |
fabiombed | 8:144855fe02bd | 561 | |
fabiombed | 8:144855fe02bd | 562 | int main() |
fabiombed | 8:144855fe02bd | 563 | { |
fabiombed | 8:144855fe02bd | 564 | |
fabiombed | 8:144855fe02bd | 565 | // Printing to the console |
fabiombed | 14:644f9e7278e9 | 566 | printf("\r\n\r\nSunTracker by Fabio Brembilla\r\n\r\n"); |
fabiombed | 11:b5e23d64d366 | 567 | |
fabiombed | 8:144855fe02bd | 568 | Initialization(); |
fabiombed | 14:644f9e7278e9 | 569 | BLE_Initialization(); |
fabiombed | 7:54984d031243 | 570 | |
fabiombed | 8:144855fe02bd | 571 | mybutton.fall(&User_Button_Pressed); |
fabiombed | 7:54984d031243 | 572 | |
fabiombed | 17:582eba752042 | 573 | #ifdef Ble |
fabiombed | 18:319a1bb8f837 | 574 | Ticker tickerSta_ticker, tickerDif_ticker, tickerPos_ticker, tickerSun_ticker; |
fabiombed | 18:319a1bb8f837 | 575 | tickerSta_ticker.attach(tickerSta_Callback, TIMEOUT_STA); |
fabiombed | 18:319a1bb8f837 | 576 | #endif |
fabiombed | 18:319a1bb8f837 | 577 | |
fabiombed | 8:144855fe02bd | 578 | // Loop until push User Button to Set 0 Point |
fabiombed | 14:644f9e7278e9 | 579 | printf("\r\nWait PUSH Button"); |
fabiombed | 8:144855fe02bd | 580 | strcpy(DisplayStr,"pusH"); |
fabiombed | 17:582eba752042 | 581 | while(Status==0) { |
fabiombed | 14:644f9e7278e9 | 582 | #ifdef FlightSense |
fabiombed | 8:144855fe02bd | 583 | board->display->DisplayString(DisplayStr, 4); |
fabiombed | 8:144855fe02bd | 584 | //printf("%s\n\r", DisplayStr); |
fabiombed | 17:582eba752042 | 585 | #endif |
fabiombed | 17:582eba752042 | 586 | |
fabiombed | 17:582eba752042 | 587 | #ifdef Ble |
fabiombed | 18:319a1bb8f837 | 588 | if (p_customsensorservice->isStatusNotificationEn()) { |
fabiombed | 18:319a1bb8f837 | 589 | if (tickerSta) { |
fabiombed | 18:319a1bb8f837 | 590 | tickerSta=false; |
fabiombed | 18:319a1bb8f837 | 591 | printf("\n\r\n\rSend BLE Display Status %d", Status); |
fabiombed | 18:319a1bb8f837 | 592 | //p_customsensorservice->sendEnvStatus(Status, TimeStamp); |
fabiombed | 18:319a1bb8f837 | 593 | p_customsensorservice->updateEnvStatus(Status, TimeStamp); |
fabiombed | 18:319a1bb8f837 | 594 | } |
fabiombed | 17:582eba752042 | 595 | } |
fabiombed | 18:319a1bb8f837 | 596 | p_BLEdev->waitForEvent(); // Without it the CLIENT can't start to communicate |
fabiombed | 14:644f9e7278e9 | 597 | #endif |
fabiombed | 8:144855fe02bd | 598 | } |
fabiombed | 4:1d3d071a4c2c | 599 | |
fabiombed | 14:644f9e7278e9 | 600 | printf("\r\n\r\nStart Main Loop"); |
fabiombed | 18:319a1bb8f837 | 601 | |
fabiombed | 18:319a1bb8f837 | 602 | #ifdef Ble |
fabiombed | 18:319a1bb8f837 | 603 | tickerDif_ticker.attach(tickerDif_Callback, TIMEOUT_DIF); |
fabiombed | 18:319a1bb8f837 | 604 | tickerPos_ticker.attach(tickerPos_Callback, TIMEOUT_POS); |
fabiombed | 18:319a1bb8f837 | 605 | tickerSun_ticker.attach(tickerSun_Callback, TIMEOUT_SUN); |
fabiombed | 18:319a1bb8f837 | 606 | #endif |
fabiombed | 17:582eba752042 | 607 | |
fabiombed | 14:644f9e7278e9 | 608 | #ifdef EasySpin |
fabiombed | 14:644f9e7278e9 | 609 | motor->Enable(); // To put the motor on hold by execute CmdEnable |
fabiombed | 14:644f9e7278e9 | 610 | #endif |
fabiombed | 10:d154094297ce | 611 | |
fabiombed | 6:4cbf7303b496 | 612 | // Main Loop |
fabiombed | 8:144855fe02bd | 613 | while(true) |
fabiombed | 8:144855fe02bd | 614 | { |
fabiombed | 17:582eba752042 | 615 | if (Status==1 | Status==2) { Measure_Babybear(); } |
fabiombed | 15:019b8d60c89d | 616 | |
fabiombed | 15:019b8d60c89d | 617 | #ifndef Ble // without BLE it uses Accelerometer from SERVER, otherwise receive the value from CLIENT |
fabiombed | 17:582eba752042 | 618 | if (Status==3) { Measure_Accelerometer(); } |
fabiombed | 15:019b8d60c89d | 619 | #endif |
fabiombed | 15:019b8d60c89d | 620 | |
fabiombed | 8:144855fe02bd | 621 | Control_Motor(); |
fabiombed | 8:144855fe02bd | 622 | Measure_SolarPanel(); |
fabiombed | 14:644f9e7278e9 | 623 | |
fabiombed | 14:644f9e7278e9 | 624 | #ifdef FlightSense |
fabiombed | 14:644f9e7278e9 | 625 | board->display->DisplayString(DisplayStr, 4); |
fabiombed | 14:644f9e7278e9 | 626 | //printf("%s\n\r", DisplayStr); |
fabiombed | 18:319a1bb8f837 | 627 | #endif |
fabiombed | 17:582eba752042 | 628 | |
fabiombed | 18:319a1bb8f837 | 629 | #ifdef Ble |
fabiombed | 18:319a1bb8f837 | 630 | if (p_customsensorservice->isStatusNotificationEn()) { |
fabiombed | 18:319a1bb8f837 | 631 | if (tickerSta) { |
fabiombed | 18:319a1bb8f837 | 632 | tickerSta=false; |
fabiombed | 17:582eba752042 | 633 | printf("\n\r\n\rSend BLE Display Status %d", Status); |
fabiombed | 18:319a1bb8f837 | 634 | //p_customsensorservice->sendEnvStatus(Status, TimeStamp); |
fabiombed | 18:319a1bb8f837 | 635 | p_customsensorservice->updateEnvStatus(Status, TimeStamp); |
fabiombed | 18:319a1bb8f837 | 636 | } |
fabiombed | 18:319a1bb8f837 | 637 | } |
fabiombed | 18:319a1bb8f837 | 638 | if (p_customsensorservice->isDifferenceNotificationEn()) { |
fabiombed | 18:319a1bb8f837 | 639 | if (tickerDif) { |
fabiombed | 18:319a1bb8f837 | 640 | tickerDif=false; |
fabiombed | 15:019b8d60c89d | 641 | printf("\n\rSend BLE Difference %d lux/mems", diff); // Send BLE diff, no diff_abs |
fabiombed | 18:319a1bb8f837 | 642 | //p_customsensorservice->sendEnvDifference(diff, TimeStamp); |
fabiombed | 18:319a1bb8f837 | 643 | p_customsensorservice->updateEnvDifference(diff, TimeStamp); |
fabiombed | 18:319a1bb8f837 | 644 | } |
fabiombed | 18:319a1bb8f837 | 645 | } |
fabiombed | 18:319a1bb8f837 | 646 | if (p_customsensorservice->isPositionNotificationEn()) { |
fabiombed | 18:319a1bb8f837 | 647 | if (tickerPos) { |
fabiombed | 18:319a1bb8f837 | 648 | tickerPos=false; |
fabiombed | 15:019b8d60c89d | 649 | printf("\n\rSend BLE Position %d", pos); |
fabiombed | 18:319a1bb8f837 | 650 | //p_customsensorservice->sendEnvPosition(pos, TimeStamp); |
fabiombed | 18:319a1bb8f837 | 651 | p_customsensorservice->updateEnvPosition(pos, TimeStamp); |
fabiombed | 18:319a1bb8f837 | 652 | } |
fabiombed | 14:644f9e7278e9 | 653 | } |
fabiombed | 18:319a1bb8f837 | 654 | if (p_customsensorservice->isSunpanelNotificationEn()) { |
fabiombed | 18:319a1bb8f837 | 655 | if (tickerSun) { |
fabiombed | 18:319a1bb8f837 | 656 | tickerSun=false; |
fabiombed | 18:319a1bb8f837 | 657 | printf("\n\rSend BLE SunPanel %d mV", measure); |
fabiombed | 18:319a1bb8f837 | 658 | //p_customsensorservice->sendEnvSunpanel(measure, TimeStamp); |
fabiombed | 18:319a1bb8f837 | 659 | p_customsensorservice->updateEnvSunpanel(measure, TimeStamp); |
fabiombed | 18:319a1bb8f837 | 660 | } |
fabiombed | 18:319a1bb8f837 | 661 | } |
fabiombed | 18:319a1bb8f837 | 662 | //diff=0; // To reset it in case from Accelerometer doesn't arrive a future value (is better that Acc. always send a value) |
fabiombed | 18:319a1bb8f837 | 663 | |
fabiombed | 11:b5e23d64d366 | 664 | p_BLEdev->waitForEvent(); |
fabiombed | 14:644f9e7278e9 | 665 | #endif |
fabiombed | 17:582eba752042 | 666 | |
fabiombed | 8:144855fe02bd | 667 | } |
fabiombed | 4:1d3d071a4c2c | 668 | |
fabiombed | 8:144855fe02bd | 669 | //status_l=board->sensor_left->StopMeasurement(als_continuous_polling); |
fabiombed | 8:144855fe02bd | 670 | //status_r=board->sensor_right->StopMeasurement(als_continuous_polling); |
fabiombed | 4:1d3d071a4c2c | 671 | |
fabiombed | 14:644f9e7278e9 | 672 | } |