Francis Lee
test
Dependencies: BLE_API mbed nRF51822
Diff: main.cpp
- Revision:
- 0:a394a04bd0b7
- Child:
- 1:88e3620627f9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Feb 26 22:53:44 2016 +0000
@@ -0,0 +1,765 @@
+/*
+ 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;
+ }
+ }
+}