Francis Lee
test
Dependencies: BLE_API mbed nRF51822
main.cpp
- Committer:
- sk84life85
- Date:
- 2016-02-26
- Revision:
- 1:88e3620627f9
- Parent:
- 0:a394a04bd0b7
File content as of revision 1:88e3620627f9:
/*
SAD: 0x72
TASK: WRITE 0x12 to register 0x42 then start reading.
mbed Microcontroller Library
* Copyright (c) 2006-2013 ARM Limited
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
//#define AnalogTemp //BDE0600
//#define AnalogUV //ML8511
//#define HallSensor //BU52011 //Change 1: Change to use GPIO for BU52014
//#define RPR0521 //RPR0521 //Change 2: Remove This and add in the RPR-0521
//#define KMX62 //Change 3: Add Code For BH1745, KX022, BM1383GLV, KMX62
#include "mbed.h"
#include "BLEDevice.h"
#include "UARTService.h"
#include "nrf_temp.h"
#include "I2C.h"
#include <string>
#include "stdlib.h"
//#include "C12832.h"
#define MAX_REPLY_LEN (UARTService::BLE_UART_SERVICE_MAX_DATA_LEN) //Actually equal to 20
#define SENSOR_READ_INTERVAL_S (0.2F)
#define ADV_INTERVAL_MS (1000UL)
#define UART_BAUD_RATE (19200UL)
#define DEVICE_NAME ("Rohm") // This can be read AFTER connecting to the device.
#define SHORT_NAME ("BH1745") // Keep this short: max 8 chars if a 128bit UUID is also advertised.
#define DEBUG(...) { m_serial_port.printf(__VA_ARGS__); }
#define BH1745 //KX022, Accelerometer Sensor
//#define BatteryI2C
// Function Prototypes
void PBTrigger(); //Interrupt function for PB4
// Global Variables
BLEDevice m_ble;
Serial m_serial_port(p9, p11); // TX pin, RX pin
DigitalOut m_cmd_led(LED1); //Original Declaration
//PwmOut m_cmd_led(LED1); //Original Declaration
PwmOut PWM(p4);
PwmOut hornFreq(p13);
DigitalOut m_error_led(LED2);
UARTService *m_uart_service_ptr;
//InterruptIn sw4Press(p18);
InterruptIn resetCruise(p21);
DigitalIn hornEnable(p18);
InterruptIn setCruise(p20);
I2C i2c(p7,p30);
//C12832 lcd(p25, p29, p28, p1, p0);
//DigitalOut LCD_RESET(p24);
DigitalOut LED_Enable(p1);
//DigitalOut LED_EnableR(p1);
AnalogIn brake(p3); //Haven't done anything with this yet!
char BrakeIn, isClimbStop;
//General Debug
//DigitalOut Test1(p12);
//DigitalOut Test1(p13);
//DigitalOut Test2(p14);
//DigitalOut Test3(p15);
AnalogIn Hall(p5); //p3 on prior EVK, p5 on the shield
AnalogIn Speed(p2); //p3 on prior EVK, p5 on the shield
uint16_t Speed_value;
uint16_t Speed_Thres = 1000;
uint16_t VsetThresh = 1;
uint16_t ADC_value,isIncreasing;
uint16_t deltaStep = 5;
uint16_t OutputLimit = 20;//10000; //Maximum increase ADC Value per loop
uint16_t LoopDelay = 50; //Loop Time in ms
uint16_t HALL_value;
uint16_t ADC_value_old = 0;
uint16_t deltaT = 100;
uint16_t ampModeDecrease = 50;
uint16_t tick;
uint16_t pOffset = 54000;
uint16_t pGain = 150;
uint16_t ExtModeGain = 150;
uint16_t AmpModeGain = 250;
uint16_t isDecreasing, currSpeed,prevSpeed;
uint16_t climbCnt;
uint16_t climbPer = 100;
uint16_t autoClimbThresh = 110;
uint16_t isLocked = 0;
uint16_t timerX, timerY, timerZ, isLightOn;
float ADC_output;
uint16_t SpeedSampNum = 3;
uint16_t speedDelta = 24;
uint16_t Mode = 0;
//string OutSetting;
uint16_t isConnected = 0;
int32_t p, LoopID;
int16_t cruiseSet = 0;
int16_t cruiseFlag = 0;
int16_t SpeedSet=0;
int16_t speedStep = 10;
int16_t i,j;
int16_t tempSpeed;
int16_t isAmpMode = 0;
bool RepStart = true;
bool NoRepStart = false;
bool LED_Command = false;
double chromaX, chromaY, tempCIE;
double CIEx[60];
double CIEy[60];
double xPPU9_5 = 0.05660;
double yPPU9_5 = 0.06549;
double xPPU9_4 = 0.05072;
double yPPU9_4 = 0.04818;
double xPPU2_14 = 0.03221;
double yPPU2_14 = 0.02597;
double xPPU18_5 = 0.07085;
double yPPU18_5 = 0.06684;
double xPPU18_4 = 0.04707;
double yPPU18_4 = 0.04474;
double xPPU18_3 = 0.03384;
double yPPU18_3 = 0.03223;
double xPPU17_18 = 0.03440;
double yPPU17_18 = 0.03201;
double xPPU5_6 = 0.04115;
double yPPU5_6 = 0.03835;
double xPPU4_15 = 0.08183;
double yPPU4_15 = 0.07357;
double xPPU4_14 = 0.07272;
double yPPU4_14 = 0.06621;
double xPPU4_13 = 0.07797;
double yPPU4_13 = 0.07208;
double xPPU4_10 = 0.09362;
double yPPU4_10 = 0.08590;
double xPPU4_8 = 0.07635;
double yPPU4_8 = 0.07052;
double xThresh = 0.0004;
double yThresh = 0.0004;
string dataReceived = "aa";
uint8_t buf[19];
uint32_t len = 0;
string OutSetting = "";
#ifdef BH1745
int BH1745_addr_w = 0x72;
int BH1745_addr_r = 0x73;
char BH1745_CTRL2[2] = {0x42, 0x11};
char BH1745_Content_ReadData[8];
float BH1745_Red;
float BH1745_Green;
float BH1745_Blue;
float BH1745_Clear;
float SUM_BH1745_Red[10];
float SUM_BH1745_Green[10];
float SUM_BH1745_Blue[10];
float SUM_BH1745_Clear[10];
double colorSum;
int colorNum;
short int BH1745_Raw_R = 0;
short int BH1745_Raw_G = 0;
short int BH1745_Raw_B = 0;
short int BH1745_Raw_C = 0;
char BH1745_Red_Data_LSB = 0x50;
int sampleNum=0;
char message[20];
float rValue, gValue, bValue, cValue;
float colorError;
#endif
#ifdef BatteryI2C
int BMS_addr_w = 0x16; //0x2C;
int BMS_addr_r = 0x17; //0x2D;
char BMS_Content_ReadData[9];
char BMS_DeviceName = 0x21;
char BMS_Cycles = 0x17;
char BMS_SOC = 0x0D;
uint16_t BMS_Password0;
uint16_t BMS_Password1;
uint16_t BMS_Password2;
uint16_t BMS_Password3;
uint16_t BMS_Password4;
uint16_t BMS_Password5;
uint16_t BMS_Password6;
uint16_t BMS_Password7;
uint16_t BMS_Password8;
uint16_t BMS_CycleOut;
uint16_t BMS_SOCOut;
#endif
/**
* This callback is used whenever a disconnection occurs.
*/
void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
{
switch (reason) {
case Gap::REMOTE_USER_TERMINATED_CONNECTION:
DEBUG("Disconnected (REMOTE_USER_TERMINATED_CONNECTION)\n\r");
break;
case Gap::LOCAL_HOST_TERMINATED_CONNECTION:
DEBUG("Disconnected (LOCAL_HOST_TERMINATED_CONNECTION)\n\r");
break;
case Gap::CONN_INTERVAL_UNACCEPTABLE:
DEBUG("Disconnected (CONN_INTERVAL_UNACCEPTABLE)\n\r");
break;
}
isConnected = 0;
LED_Enable = 1;
wait_ms(100);
LED_Enable = 0;
DEBUG("Restarting the advertising process\n\r");
m_ble.startAdvertising();
}
/**
* This callback is used whenever the host writes data to one of our GATT characteristics.
*/
void dataWrittenCallback(const GattCharacteristicWriteCBParams *params)
{
// Ensure that initialization is finished and the host has written to the TX characteristic.
if ((m_uart_service_ptr != NULL) && (params->charHandle == m_uart_service_ptr->getTXCharacteristicHandle())) {
len = 0;
// uint16_t TempInt = 1;
/*
j = 0;
for( int i = (params->len) - 1; i > 0; i-- )
{
dataReceived[j] = params->data[j];
j++;
}
dataReceived[j] = params->data[j];
dataReceived[++j] = 0;
*/
if (params->len == 1) {
switch (params->data[0]) {
#ifdef BH1745 //
case 'r':
len = snprintf((char*) buf, MAX_REPLY_LEN, "Red= %f", BH1745_Red);
break;
case 'g':
len = snprintf((char*) buf, MAX_REPLY_LEN, "Green= %f", BH1745_Green);
break;
case 'b':
len = snprintf((char*) buf, MAX_REPLY_LEN, "Blue= %f", BH1745_Blue);
break;
case 'c':
len = snprintf((char*) buf, MAX_REPLY_LEN, "Clear= %f", BH1745_Clear);
break;
case 'x':
len = snprintf((char*) buf, MAX_REPLY_LEN, "CIE#%6.5f,%6.5f", chromaX, chromaY);
break;
/*
case '?':
if( (0.03212-0.0005<chromaX) && (chromaX<0.03212+0.0005) && (0.02598-0.0005<chromaY) && (chromaY<0.02598+0.0005) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "2-14");
}
else if( (0.07793-0.0005<chromaX) && (chromaX<0.07793+0.0005) && (0.07237-0.0005<chromaY) && (chromaY<0.07237+0.0005) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-13");
}
else if( (0.07256-0.0005<chromaX) && (chromaX<0.07256+0.0005) && (0.06636-0.0005<chromaY) && (chromaY<0.06636+0.0005) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-14");
}
else if( (0.07079-0.0005<chromaX) && (chromaX<0.07079+0.0005) && (0.06706-0.0005<chromaY) && (chromaY<0.06706+0.0005) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-5");
}
else if( (0.04708-0.0005<chromaX) && (chromaX<0.04708+0.0005) && (0.04485-0.0005<chromaY) && (chromaY<0.04485+0.0005) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-4");
}
else if( (0.03373-0.0005<chromaX) && (chromaX<0.03373+0.0005) && (0.03221-0.0005<chromaY) && (chromaY<0.03221+0.0005) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-3");
}
else if( (0.07630-0.0005<chromaX) && (chromaX<0.07630+0.0005) && (0.07077-0.0005<chromaY) && (chromaY<0.07077+0.0005) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-8");
}
else if( (0.09365-0.0005<chromaX) && (chromaX<0.09365+0.0005) && (0.08628-0.0005<chromaY) && (chromaY<0.08628+0.0005) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-10");
}
else if( (0.09483-0.0005<chromaX) && (chromaX<0.09483+0.0005) && (0.08657-0.0005<chromaY) && (chromaY<0.08657+0.0005) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-11");
}
else if( (0.08935-0.0005<chromaX) && (chromaX<0.08935+0.0005) && (0.08311-0.0005<chromaY) && (chromaY<0.08311+0.0005) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-12");
}
else if( (0.08176-0.0005<chromaX) && (chromaX<0.08176+0.0005) && (0.07384-0.0005<chromaY) && (chromaY<0.07384+0.0005) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-15");
}
else if( (0.04112-0.0005<chromaX) && (chromaX<0.04112+0.0005) && (0.03841-0.0005<chromaY) && (chromaY<0.03841+0.0005) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-6");
}
else if( (0.03577-0.0005<chromaX) && (chromaX<0.03577+0.0005) && (0.03325-0.0005<chromaY) && (chromaY<0.03325+0.0005) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-5");
}
else if( (0.03432-0.0005<chromaX) && (chromaX<0.03432+0.0005) && (0.03203-0.0005<chromaY) && (chromaY<0.03203+0.0005) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "17-18");
}
else{
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "Unregistered");
}
break;
*/
case 'l':
if(LED_Enable==0){
LED_Enable = 1;
wait_ms(100);
len = snprintf((char*) buf, MAX_REPLY_LEN, "LED ON");
}
else{
LED_Enable = 0;
wait_ms(100);
len = snprintf((char*) buf, MAX_REPLY_LEN, "LED OFF");
}
break;
#endif
default:
len = snprintf((char*) buf, MAX_REPLY_LEN, "ERROR");
break;
}
}/*
else if (params->len > 1) {
switch (params->data[0]) {
case 's':
rValue = BH1745_Red;
gValue = BH1745_Green;
bValue = BH1745_Blue;
cValue = BH1745_Clear;
break;
}
} */
else
{
len = snprintf((char*) buf, MAX_REPLY_LEN, "ERROR");
}
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
DEBUG("%d bytes received from host\n\r", params->len);
}
}
/**
* This callback is used whenever a write to a GATT characteristic causes data to be sent to the host.
*/
void dataSentCallback(unsigned count)
{
// NOTE: The count always seems to be 1 regardless of data.
DEBUG("%d bytes sent to host\n\r", count);
}
/**
* This callback is scheduled to be called periodically via a low-priority interrupt.
*/
void periodicCallback(void)
{
// uint8_t buf[MAX_REPLY_LEN];
// uint32_t len = 0;
if (m_ble.getGapState().connected) {
if(isConnected==0){
}
isConnected = 1;
}
else{
isConnected = 0;
}
}
void error(ble_error_t err, uint32_t line)
{
m_error_led = 1;
DEBUG("Error %d on line number %d\n\r", err, line);
}
//Code for writing back to Console if a PB is pressed
void PBTrigger()
{
//uint8_t buf[MAX_REPLY_LEN];
//uint32_t len = 0;
m_cmd_led = !m_cmd_led;
if(!setCruise){
cruiseFlag = 1;
cruiseSet = Speed_value*2.75;
}
if(!resetCruise){
cruiseFlag = 0;
}
m_cmd_led = cruiseFlag;
/*
if (m_ble.getGapState().connected) {
}
*/
}
int main(void)
{
ble_error_t err;
Ticker ticker;
m_serial_port.baud(UART_BAUD_RATE);
DEBUG("Initialising...\n\r");
m_cmd_led = 0;
m_error_led = 0;
//ticker.attach(periodicCallback, SENSOR_READ_INTERVAL_S);
//sw4Press.fall(&PBTrigger);
setCruise.fall(&PBTrigger);
resetCruise.fall(&PBTrigger);
//Start BTLE Initialization Section
m_ble.init();
m_ble.onDisconnection(disconnectionCallback);
m_ble.onDataWritten(dataWrittenCallback);
m_ble.onDataSent(dataSentCallback);
// Set the TX power in dBm units.
// Possible values (in decreasing order): 4, 0, -4, -8, -12, -16, -20.
err = m_ble.setTxPower(4);
if (BLE_ERROR_NONE != err) {
error(err, __LINE__);
}
// Setup advertising (GAP stuff).
err = m_ble.setDeviceName(DEVICE_NAME);
if (BLE_ERROR_NONE != err) {
error(err, __LINE__);
}
err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
if (BLE_ERROR_NONE != err) {
error(err, __LINE__);
}
m_ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
(const uint8_t *)SHORT_NAME,
(sizeof(SHORT_NAME) - 1));
if (BLE_ERROR_NONE != err) {
error(err, __LINE__);
}
err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
(const uint8_t *)UARTServiceUUID_reversed,
sizeof(UARTServiceUUID_reversed));
if (BLE_ERROR_NONE != err) {
error(err, __LINE__);
}
m_ble.setAdvertisingInterval(Gap::MSEC_TO_ADVERTISEMENT_DURATION_UNITS(ADV_INTERVAL_MS));
m_ble.startAdvertising();
// Create a UARTService object (GATT stuff).
UARTService uartService(m_ble);
m_uart_service_ptr = &uartService;
#ifdef BH1745
i2c.write(BH1745_addr_w, &BH1745_CTRL2[0], 2, false);
#endif
#ifdef BatteryI2C
i2c.write(BMS_addr_w, &BMS_DeviceName, 1, RepStart);
i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 8, NoRepStart);
//BMS_Password = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]);
BMS_Password0 = (BMS_Content_ReadData[0]);
BMS_Password1 = (BMS_Content_ReadData[1]);
BMS_Password2 = (BMS_Content_ReadData[2]);
BMS_Password3 = (BMS_Content_ReadData[3]);
BMS_Password4 = (BMS_Content_ReadData[4]);
BMS_Password5 = (BMS_Content_ReadData[5]);
BMS_Password6 = (BMS_Content_ReadData[6]);
BMS_Password7 = (BMS_Content_ReadData[7]);
BMS_Password8 = (BMS_Content_ReadData[8]);
i2c.write(BMS_addr_w, &BMS_Cycles, 1, RepStart);
i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 2, NoRepStart);
BMS_CycleOut = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]);
i2c.write(BMS_addr_w, &BMS_SOC, 1, RepStart);
i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 2, NoRepStart);
BMS_SOCOut = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]);
#endif
//Initialize PWM Output
PWM.period_us(1000);
hornFreq.period_us(1250);
//m_cmd_led.period_ms(10);
ADC_value_old = 0;
p = 0;
SpeedSet = 1;
tick = deltaT;
SpeedSampNum = 5;
LED_Enable = 1;
wait_ms(100);
wait_ms(100);
LED_Enable = 0;
wait_ms(100);
wait_ms(100);
LED_Enable = 1;
wait_ms(100);
wait_ms(100);
LED_Enable = 0;
wait_ms(100);
wait_ms(100);
LED_Enable = 1;
wait_ms(100);
wait_ms(100);
LED_Enable = 0;
wait_ms(100);
wait_ms(100);
while (true) {
//m_ble.waitForEvent();
if(m_ble.getGapState().connected){
if(!isConnected){
LED_Enable = 1;
wait_ms(100);
wait_ms(100);
LED_Enable = 0;
wait_ms(100);
wait_ms(100);
LED_Enable = 1;
wait_ms(100);
wait_ms(100);
LED_Enable = 0;
wait_ms(100);
wait_ms(100);
LED_Enable = 1;
wait_ms(100);
wait_ms(100);
LED_Enable = 0;
wait_ms(100);
wait_ms(100);
}
isConnected = 1;
//Read KX022 Portion from the IC
i2c.write(BH1745_addr_w, &BH1745_Red_Data_LSB, 1, RepStart);
i2c.read(BH1745_addr_r, &BH1745_Content_ReadData[0], 8, NoRepStart);
//reconfigure the data (taken from arduino code)
BH1745_Raw_R = (BH1745_Content_ReadData[1]<<8) | (BH1745_Content_ReadData[0]);
BH1745_Raw_G = (BH1745_Content_ReadData[3]<<8) | (BH1745_Content_ReadData[2]);
BH1745_Raw_B = (BH1745_Content_ReadData[5]<<8) | (BH1745_Content_ReadData[4]);
BH1745_Raw_C = (BH1745_Content_ReadData[7]<<8) | (BH1745_Content_ReadData[6]);
BH1745_Red = (float)BH1745_Raw_R / 16384;
BH1745_Green = (float)BH1745_Raw_G / 16384;
BH1745_Blue = (float)BH1745_Raw_B / 16384;
BH1745_Clear = (float)BH1745_Raw_C / 16384;
if(BH1745_Clear < 0.00001){
wait_ms(100);
wait_ms(100);
wait_ms(100);
wait_ms(100);
wait_ms(100);
wait_ms(100);
wait_ms(100);
wait_ms(100);
LED_Enable = 1;
wait_ms(100);
wait_ms(100);
for(i=0; i<60; i++){
//---------------------------- KX022 ----------------------------
//Read KX022 Portion from the IC
i2c.write(BH1745_addr_w, &BH1745_Red_Data_LSB, 1, RepStart);
i2c.read(BH1745_addr_r, &BH1745_Content_ReadData[0], 8, NoRepStart);
//reconfigure the data (taken from arduino code)
BH1745_Raw_R = (BH1745_Content_ReadData[1]<<8) | (BH1745_Content_ReadData[0]);
BH1745_Raw_G = (BH1745_Content_ReadData[3]<<8) | (BH1745_Content_ReadData[2]);
BH1745_Raw_B = (BH1745_Content_ReadData[5]<<8) | (BH1745_Content_ReadData[4]);
BH1745_Raw_C = (BH1745_Content_ReadData[7]<<8) | (BH1745_Content_ReadData[6]);
LED_Enable = 0;
BH1745_Red = (float)BH1745_Raw_R / 16384;
BH1745_Green = (float)BH1745_Raw_G / 16384;
BH1745_Blue = (float)BH1745_Raw_B / 16384;
BH1745_Clear = (float)BH1745_Raw_C / 16384;
CIEx[i] = 0.232*BH1745_Red + 0.610*BH1745_Green + 0.055*BH1745_Blue;
if( BH1745_Clear < (0.2*BH1745_Green) ){
if( BH1745_Blue < (0.45*BH1745_Green) ){
CIEy[i] = 0.083*BH1745_Red + 0.860*BH1745_Green;
}
else{
CIEy[i] = 0.070*BH1745_Red + 0.709*BH1745_Green;
}
}
else if( BH1745_Clear < (0.50*BH1745_Green) ){
CIEy[i] = 0.110*BH1745_Red + 0.712*BH1745_Green;
}
else{
CIEy[i] = 0.073*BH1745_Red + 0.864*BH1745_Green;
}
for(j=i; j>0; j--){
tempCIE = CIEx[j];
if(tempCIE<CIEx[j-1]){
CIEx[j] = CIEx[j-1];
CIEx[j-1] = tempCIE;
}
}
for(j=i; j>0; j--){
tempCIE = CIEy[j];
if(tempCIE<CIEy[j-1]){
CIEy[j] = CIEy[j-1];
CIEy[j-1] = tempCIE;
}
}
}
chromaX = (CIEx[28]+CIEx[29]+CIEx[30]+CIEx[31]+CIEx[32])/5.0;
chromaY = (CIEy[28]+CIEy[29]+CIEy[30]+CIEy[31]+CIEy[32])/5.0;
if( (xPPU2_14-xThresh<chromaX) && (chromaX<xPPU2_14+xThresh) && (yPPU2_14-yThresh<chromaY) && (chromaY<yPPU2_14+yThresh) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "2-14");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
}
else if( (xPPU18_5-xThresh<chromaX) && (chromaX<xPPU18_5+xThresh) && (yPPU18_5-yThresh<chromaY) && (chromaY<yPPU18_5+yThresh) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-5");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
}
else if( (xPPU18_4-xThresh<chromaX) && (chromaX<xPPU18_4+xThresh) && (yPPU18_4-yThresh<chromaY) && (chromaY<yPPU18_4+yThresh) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-4");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
}
else if( (xPPU18_3-xThresh<chromaX) && (chromaX<xPPU18_3+xThresh) && (yPPU18_3-yThresh<chromaY) && (chromaY<yPPU18_3+yThresh) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-3");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
}
else if( (xPPU17_18-xThresh<chromaX) && (chromaX<xPPU17_18+xThresh) && (yPPU17_18-yThresh<chromaY) && (chromaY<yPPU17_18+yThresh) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "17-18");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
}
else if( (xPPU5_6-xThresh<chromaX) && (chromaX<xPPU5_6+xThresh) && (yPPU5_6-yThresh<chromaY) && (chromaY<yPPU5_6+yThresh) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-6");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
}
else if( (xPPU4_15-xThresh<chromaX) && (chromaX<xPPU4_15+xThresh) && (yPPU4_15-yThresh<chromaY) && (chromaY<yPPU4_15+yThresh) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-15");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
}
else if( (xPPU4_14-xThresh<chromaX) && (chromaX<xPPU4_14+xThresh) && (yPPU4_14-yThresh<chromaY) && (chromaY<yPPU4_14+yThresh) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-14");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
}
else if( (xPPU4_13-xThresh<chromaX) && (chromaX<xPPU4_13+xThresh) && (yPPU4_13-yThresh<chromaY) && (chromaY<yPPU4_13+yThresh) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-13");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
}
else if( (xPPU4_10-xThresh<chromaX) && (chromaX<xPPU4_10+xThresh) && (yPPU4_10-yThresh<chromaY) && (chromaY<yPPU4_10+yThresh) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-10");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
}
else if( (xPPU4_8-xThresh<chromaX) && (chromaX<xPPU4_8+xThresh) && (yPPU4_8-yThresh<chromaY) && (chromaY<yPPU4_8+yThresh) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-8");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
}
else if( (xPPU9_4-xThresh<chromaX) && (chromaX<xPPU9_4+xThresh) && (yPPU9_4-yThresh<chromaY) && (chromaY<yPPU9_4+yThresh) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "9-4");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
}
else if( (xPPU9_5-xThresh<chromaX) && (chromaX<xPPU9_5+xThresh) && (yPPU9_5-yThresh<chromaY) && (chromaY<yPPU9_5+yThresh) ){
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "9-5");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
}
else{
len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "Unregistered");
m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
}
}
}
else{
isConnected = 0;
}
}
}