/**
******************************************************************************
* @file main.cpp
* @author Fabio Brembilla
* @version V2.0.0
* @date April, 2016
* @brief SunTracker + BLE (Server) Vertical Application
* This application use IHM01A1, 6180XA1, IKS01A1, IDB0XA1 expansion boards
******************************************************************************
* @attention
*
*
© COPYRIGHT(c) 2016 STMicroelectronics
*
* 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.
*
******************************************************************************
*/
/* Define --------------------------------------------------------------------*/
#define FlightSense //6180XA1 Main (remove only for debug)
#define EasySpin //IHM01A1 Main (remove only for debug)
#define Sensors //IKS01A1 Option
#define Ble //IDB0XA1 Option
/* Includes ------------------------------------------------------------------*/
// Mbed specific header files
#include "mbed.h"
// Helper header files
#include "DevSPI.h"
#include "DevI2C.h"
// Component specific header files
#ifdef FlightSense
#include "x_nucleo_6180xa1.h"
#endif
#ifdef EasySpin
#include "l6474_class.h"
#endif
#ifdef Sensors
#include "x_nucleo_iks01a1.h"
#endif
// C header files
#include
#include
#include
#include
/* BlueTooth -----------------------------------------------------------------*/
#ifdef Ble
#include "debug.h" // Need for PRINTF
#include "Utils.h" // Need for STORE_LE_16 and STORE_LE_32
// Not need to re-define it, it is already defined into UUID.h, just use UUID:: when you call it
//const unsigned LENGTH_OF_LONG_UUID = 16;
//typedef uint16_t ShortUUIDBytes_t;
//typedef uint8_t LongUUIDBytes_t[LENGTH_OF_LONG_UUID];
typedef enum ConnectionStatus_t {
DISCONNECTED =0,
CONNECTED =1
} cns_t;
#define BLE_DEV_NAME "SunTracker"
//#define BLE_DEV_MAC 0xAA,0xBB,0xCC,0xDD,0xEE,0xFF // SERVER address (must be set into CLIENT BLE_peer_address_BE)
#define BLE_DEV_MAC 0xFD,0x66,0x05,0x13,0xBE,0xBA // Default ST address (must be set into CLIENT BLE_peer_address_BE)
#define BLE_ADVERTISING_INTERVAL 1000
#include "CustomControlService.h"
#include "CustomSensorsService.h"
static BLE *p_BLEdev = NULL;
static CustomControlService *p_customcontrolservice = NULL;
static CustomSensorService *p_customsensorservice = NULL;
#define FeatureStatus 0x00000400
#define FeatureDifference 0x00000800
#define FeaturePosition 0x00001000
#define FeatureSunPanel 0x00002000
#endif
/* Definitions ---------------------------------------------------------------*/
#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 -----------------------------------------------------------------*/
int8_t dir=0; // Motor Rotation Direction: 0 = Stop, 1 = Anticlockwise, 2 = Clockwise
int8_t changedir=0; // Change Direction: 0 = No, 1 = Yes
int8_t arrest=0; // Arrest: 0 = No, 1 = Yes
int16_t babybear=0; // Difference (in Lux) between Left and Right
int32_t acc_data[3]; // Difference of Accelerometer
int16_t diff=0; // Babybear or Accelerometer difference [--> Send BLE]
int16_t diff_abs=0; // Abs of Babybear or Accelerometer difference
int8_t left=0; // Left Command for Rotate Direction
int8_t right=0; // Right Command for Rotate Direction
int16_t pos=0; // Motor Position [--> Send BLE]
int16_t measure=0; // ADC Value from SunPanel [--> Send BLE]
char DisplayStr[5]; // Display String
int16_t Status=0; // Status Shown on Display: 0 = Idle, 1 = Motor Speed, 2 = Solar Panel Value, 3 = Manual Control [--> Send BLE]
int16_t status_bb, status_t, status_b, status_l, status_r; // Babybear Status
uint16_t TimeStamp=0; // TimeStamp for BLE
/* Initializations ------------------------------------------------------------*/
// Initializing SPI bus
DevSPI dev_spi(D11, D12, D13);
// Initializing I2C bus
DevI2C dev_i2c(D14, D15);
#ifdef FlightSense
// Initializing FlightSense Component 6180XA1
static X_NUCLEO_6180XA1 *board;
MeasureData_t data_sensor_top, data_sensor_bottom, data_sensor_left, data_sensor_right;
#endif
#ifdef EasySpin
// Initializing EasySpin Component IHM01A1
static L6474 *motor;
#endif
#ifdef Sensors
// Initializing Sensors Component IKS01A1
static X_NUCLEO_IKS01A1 *mems;
MotionSensor *accelerometer;
#endif
InterruptIn mybutton(USER_BUTTON);
AnalogIn analog_read(PC_3); // A1 Conflict with BLE SPI_CS --> Changed in Morpho PC_3
/* Update_Status ------------------------------------------------------------*/
void Update_Status(int16_t Stat)
{
char State[12]; // Characters into State must be <12 and never =>12 or it crash!!!
if (Stat==1) strcpy(State,"FlightSense");
if (Stat==2) strcpy(State,"ADCSunPanel");
if (Stat==3) strcpy(State,"Sensors");
printf("\r\n\r\nPUSH Status: %s", State);
}
/* User_Button_Pressed -------------------------------------------------------*/
void User_Button_Pressed(void)
{
Status++;
#ifdef Sensors
if (Status>3) { Status=1; }
#else
if (Status>2) { Status=1; }
#endif
Update_Status(Status);
}
#ifdef Ble
/* Bluetooth CallBack ---------------------------------------------------------*/
static void onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
{
if (p_customcontrolservice->isCommandHandle(handle)) p_customcontrolservice->enNotify(handle);
if (p_customsensorservice->isStatusHandle(handle)) p_customsensorservice->enNotify(handle);
if (p_customsensorservice->isDifferenceHandle(handle)) p_customsensorservice->enNotify(handle);
if (p_customsensorservice->isPositionHandle(handle)) p_customsensorservice->enNotify(handle);
if (p_customsensorservice->isSunpanelHandle(handle)) p_customsensorservice->enNotify(handle);
}
static void onUpdatesDisabledCallback(Gap::Handle_t handle)
{
if (p_customcontrolservice->isCommandHandle(handle)) p_customcontrolservice->disNotify(handle);
if (p_customsensorservice->isStatusHandle(handle)) p_customsensorservice->disNotify(handle);
if (p_customsensorservice->isDifferenceHandle(handle)) p_customsensorservice->disNotify(handle);
if (p_customsensorservice->isPositionHandle(handle)) p_customsensorservice->disNotify(handle);
if (p_customsensorservice->isSunpanelHandle(handle)) p_customsensorservice->disNotify(handle);
}
static void onDataReadCallback(const GattReadCallbackParams *eventDataP)
{
/*
// if receive a manual Read request
if (p_customsensorservice->isStatusHandle(eventDataP->handle))
{
printf("\n\r\n\rSend BLE Display Status %d", Status);
p_customsensorservice->sendEnvStatus(Status, TimeStamp);
} else if (p_customsensorservice->isDifferenceHandle(eventDataP->handle)) {
printf("\n\rSend BLE Difference %d lux/mems", diff); // Send BLE diff, no diff_abs
p_customsensorservice->sendEnvDifference(diff, TimeStamp);
} else if (p_customsensorservice->isPositionHandle(eventDataP->handle)) {
printf("\n\rSend BLE Position %d", pos);
p_customsensorservice->sendEnvPosition(pos, TimeStamp);
} else if (p_customsensorservice->isSunpanelHandle(eventDataP->handle)) {
printf("\n\rSend BLE Sunpanel %d mV", measure);
p_customsensorservice->sendEnvSunpanel(measure, TimeStamp);
}
*/
}
// This Callback happen when it RECEIVE a WRITE
static void myonDataWriteCallback(const GattWriteCallbackParams *eventDataP)
{
if (p_customcontrolservice->isCommandHandle(eventDataP->handle))
{
// From CLIENT it receives a command in this format: Feature (4) + Type (1) + Data (2)
uint32_t Feature = eventDataP->data[0]<<24 | eventDataP->data[1]<<16 | eventDataP->data[2]<<8 | eventDataP->data[3];
uint8_t Type = eventDataP->data[4];
int16_t Data = eventDataP->data[5]<<8 | eventDataP->data[6];
//printf("\r\n\r\nmyonDataWriteCallback (Feature %x)", Feature);
//printf("\r\nmyonDataWriteCallback (Type %x)", Type); // Not Used
//printf("\r\nmyonDataWriteCallback (Data Hex %x)", Data);
//printf("\r\nmyonDataWriteCallback (Data Dec %d)", Data);
switch(Feature)
{
case FeatureStatus:
printf("\r\n\r\nReceived STATUS %d", Data);
Status = Data;
Update_Status(Data);
break;
case FeatureDifference:
printf("\r\n\r\nReceived DIFF %d", Data);
diff = Data;
break;
case FeaturePosition:
break;
case FeatureSunPanel:
break;
default:
break;
}
}
}
static void onConnectionCallback(const Gap::ConnectionCallbackParams_t * connectionParams)
{
printf("\r\n\r\nonConnectionCallback (Line %d)", __LINE__);
}
static void onDisconnectionCallback(const Gap::DisconnectionCallbackParams_t * disConnectionReason)
{
printf("\r\n\r\nonDisconnectionCallback (Line %d)", __LINE__);
p_BLEdev->gap().startAdvertising();
}
#endif
/* Bluetooth Initialization ---------------------------------------------------*/
bool BLE_Initialization(void)
{
#ifdef Ble
p_BLEdev = new BLE;
if (!p_BLEdev) { printf("\r\nBLE Device creation failed\r\n"); }
const Gap::Address_t BLE_address_BE = {BLE_DEV_MAC};
p_BLEdev->gap().setAddress(BLEProtocol::AddressType::PUBLIC, BLE_address_BE);
p_BLEdev->init();
// Set BLE CallBack Functions
p_BLEdev->gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
p_BLEdev->gattServer().onUpdatesDisabled(onUpdatesDisabledCallback);
p_BLEdev->gattServer().onDataRead(onDataReadCallback);
p_BLEdev->gattServer().onDataWritten(myonDataWriteCallback);
p_BLEdev->gap().onConnection(onConnectionCallback);
p_BLEdev->gap().onDisconnection(onDisconnectionCallback);
//p_BLEdev->gattServer().onConfirmationReceived(onConfirmationReceivedCallback);
//p_BLEdev->gattServer().onDataSent(onDataSentCallback);
//p_BLEdev->gap().onTimeout(onTimeoutCallback);
// BLE Services
p_customcontrolservice = new CustomControlService(*p_BLEdev);
p_customsensorservice = new CustomSensorService(*p_BLEdev);
// Setup BLE Advertising
const static char DEVICE_NAME[] = BLE_DEV_NAME;
p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
uint8_t dat[] = {0x01,0x80,0x00,0xFC,0x00,0x00};
p_BLEdev->gap().accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,dat,6);
p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::UNKNOWN);
p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
p_BLEdev->gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
p_BLEdev->gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL);
p_BLEdev->gap().startAdvertising();
printf("Initialization Ble OK (Line %d)\r\n", __LINE__);
#endif
return true;
}
/* Initialization ------------------------------------------------------------*/
bool Initialization(void)
{
dev_i2c.frequency(100000); // Default 100KHz. At 10KHz, 20KHz, 200KHz and 400KHz doesn't work
//----
#ifdef FlightSense
// Initializing FlightSense Component
board=X_NUCLEO_6180XA1::Instance(&dev_i2c, NC, NC, NC, NC); // NC as Interrupt fixed from FlightSense library 42
status_bb=board->InitBoard();
if(status_bb) VL6180x_ErrLog("Failed to init the board!\n\r");
// 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); Not Present
status_l=board->sensor_left->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
status_r=board->sensor_right->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
// 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);
printf("Init FlightSense OK\r\n");
#endif
//----
#ifdef EasySpin
// Initializing EasySpin Component
motor = new L6474(D2, D8, PB_2, D9, D10, dev_spi); // D7 conflict with BNRG_RST (when motor change direction 0-1 reset BLE)
// Used Morpho PB_2 and made HW wiring
if (motor->Init(NULL) != COMPONENT_OK)
return false;
motor->SetStepMode(StepperMotor::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);
printf("Init EasySpin OK\r\n");
#endif
//----
#ifdef Sensors
// Initializing Sensors Component
mems=X_NUCLEO_IKS01A1::Instance(&dev_i2c);
accelerometer = mems->GetAccelerometer();
printf("Init Sensors OK\r\n");
#endif
printf("Initialization OK (Line %d)\r\n", __LINE__);
return true;
}
/* Measure_Babybear ----------------------------------------------------------*/
void Measure_Babybear(void)
{
#ifdef FlightSense
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 = babybear;
#endif
}
/* Measure_Accelerometer -----------------------------------------------------*/
void Measure_Accelerometer(void)
{
#ifdef Sensors
accelerometer->Get_X_Axes(acc_data);
diff = acc_data[0];
#endif
}
/* Control_Motor -------------------------------------------------------------*/
void Control_Motor(void)
{
#ifdef EasySpin
diff_abs = abs(diff);
//printf("Diff: %d lux/mems\n\r", diff_abs);
motor->SetMaxSpeed(diff_abs);
if (diff>0) { left=0; right=1; }
if (diff<0) { left=1; right=0; }
if (diff_abs>TOLLERANCE) {
if (diff_abs <=RANGE_1)
{
if (left) { strcpy(DisplayStr,"E___"); }
if (right) { strcpy(DisplayStr,"___3"); }
}
else if (diff_abs >RANGE_1 & diff_abs <=RANGE_2)
{
if (left) { strcpy(DisplayStr,"E==="); }
if (right) { strcpy(DisplayStr,"===3"); }
}
else if (diff_abs >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_abs>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; }
printf("\n\rRUN");
}
}
// 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); }
printf("\n\rGOTO");
}
// Stop Motor
if (diff_abs<=TOLLERANCE) {
arrest=1;
if (Status==1) { strcpy(DisplayStr,"----"); }
if (Status==3) { strcpy(DisplayStr,"E 3"); }
}
if (arrest==1 && dir!=0 ) { // Stop just one time
motor->HardStop();
dir=0;
changedir=0;
arrest=0;
printf("\n\rSTOP");
}
#endif
}
/* Measure_SolarPanel --------------------------------------------------------*/
void Measure_SolarPanel(void)
{
// AnalogIn: 0V return 0.0 , 3.3V return 1.0
measure = analog_read.read() * 3300;
//printf("Measure = %.0f mV\r\n", measure);
//board->display->DisplayDigit("A", 0);
if (Status==2) { sprintf(DisplayStr, "%d", measure); }
}
/* Main ----------------------------------------------------------------------*/
int main()
{
// Printing to the console
printf("\r\n\r\nSunTracker by Fabio Brembilla\r\n\r\n");
Initialization();
BLE_Initialization();
mybutton.fall(&User_Button_Pressed);
#ifdef Ble
static int INTLOOP=0;
#endif
// Loop until push User Button to Set 0 Point
printf("\r\nWait PUSH Button");
strcpy(DisplayStr,"pusH");
while(Status==0) {
#ifdef FlightSense
board->display->DisplayString(DisplayStr, 4);
//printf("%s\n\r", DisplayStr);
#endif
#ifdef Ble
/*
INTLOOP++;
if (INTLOOP==100)
{
p_customsensorservice->sendEnvStatus(Status, TimeStamp);
p_customsensorservice->sendEnvDifference(diff, TimeStamp);
p_customsensorservice->sendEnvPosition(pos, TimeStamp);
p_customsensorservice->sendEnvSunpanel(measure, TimeStamp);
INTLOOP=0;
}
*/
//p_BLEdev->waitForEvent(); // Without it the CLIENT can't start to communicate
#endif
}
printf("\r\n\r\nStart Main Loop");
INTLOOP=0;
#ifdef EasySpin
motor->Enable(); // To put the motor on hold by execute CmdEnable
#endif
// Main Loop
while(true)
{
if (Status==1 | Status==2) { Measure_Babybear(); }
#ifndef Ble // without BLE it uses Accelerometer from SERVER, otherwise receive the value from CLIENT
if (Status==3) { Measure_Accelerometer(); }
#endif
Control_Motor();
Measure_SolarPanel();
#ifdef FlightSense
board->display->DisplayString(DisplayStr, 4);
//printf("%s\n\r", DisplayStr);
#endif
#ifdef Ble
INTLOOP++;
if (INTLOOP==100)
{
//if (p_customsensorservice->isStatusNotificationEn()) {
printf("\n\r\n\rSend BLE Display Status %d", Status);
p_customsensorservice->sendEnvStatus(Status, TimeStamp);
//p_customsensorservice->updateEnvStatus(Status, TimeStamp));
//}
//if (p_customsensorservice->isDifferenceNotificationEn()) {
printf("\n\rSend BLE Difference %d lux/mems", diff); // Send BLE diff, no diff_abs
p_customsensorservice->sendEnvDifference(diff, TimeStamp);
//p_customsensorservice->updateEnvDifference(diff, TimeStamp));
//}
//if (p_customsensorservice->isPositionNotificationEn()) {
printf("\n\rSend BLE Position %d", pos);
p_customsensorservice->sendEnvPosition(pos, TimeStamp);
//p_customsensorservice->updateEnvPosition(pos, TimeStamp));
//}
//if (p_customsensorservice->isSunpanelNotificationEn()) {
printf("\n\rSend BLE Sunpanel %d mV", measure);
p_customsensorservice->sendEnvSunpanel(measure, TimeStamp);
//p_customsensorservice->updateEnvSunpanel(measure, TimeStamp));
//}
INTLOOP=0;
//diff=0; // To reset it in case from Mems don't arrive a future value
}
p_BLEdev->waitForEvent();
#endif
}
//status_l=board->sensor_left->StopMeasurement(als_continuous_polling);
//status_r=board->sensor_right->StopMeasurement(als_continuous_polling);
}