test
Dependencies: BLE_API mbed nRF51822
main.cpp
- Committer:
- sk84life85
- Date:
- 2016-02-26
- Revision:
- 0:a394a04bd0b7
- Child:
- 1:88e3620627f9
File content as of revision 0:a394a04bd0b7:
/* 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 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{ len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "Unregistered"); m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len); } } } else{ isConnected = 0; } } }