main.cpp

00001 /* 
00002     SAD: 0x72
00003     TASK: WRITE 0x12 to register 0x42 then start reading.
00004     
00005     
00006 mbed Microcontroller Library
00007 * Copyright (c) 2006-2013 ARM Limited
00008 *
00009 * Licensed under the Apache License, Version 2.0 (the "License");
00010 * you may not use this file except in compliance with the License.
00011 * You may obtain a copy of the License at
00012 *
00013 *     http://www.apache.org/licenses/LICENSE-2.0
00014 *
00015 * Unless required by applicable law or agreed to in writing, software
00016 * distributed under the License is distributed on an "AS IS" BASIS,
00017 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00018 * See the License for the specific language governing permissions and
00019 * limitations under the License.
00020 */
00021 
00022 //#define AnalogTemp  //BDE0600
00023 //#define AnalogUV    //ML8511
00024 //#define HallSensor  //BU52011    //Change 1: Change to use GPIO for BU52014
00025 //#define RPR0521     //RPR0521    //Change 2: Remove This and add in the RPR-0521
00026 //#define KMX62                    //Change 3: Add Code For BH1745, KX022, BM1383GLV, KMX62
00027                                 
00028 #include "mbed.h"
00029 #include "BLEDevice.h"
00030 #include "UARTService.h"
00031 #include "nrf_temp.h"
00032 #include "I2C.h"
00033 #include <string>
00034 #include "stdlib.h"
00035 //#include "C12832.h"
00036 
00037 #define MAX_REPLY_LEN           (UARTService::BLE_UART_SERVICE_MAX_DATA_LEN)    //Actually equal to 20
00038 #define SENSOR_READ_INTERVAL_S  (0.2F) 
00039 #define ADV_INTERVAL_MS         (1000UL)
00040 #define UART_BAUD_RATE          (19200UL)
00041 #define DEVICE_NAME             ("Rohm") // This can be read AFTER connecting to the device.
00042 #define SHORT_NAME              ("BH1745")    // Keep this short: max 8 chars if a 128bit UUID is also advertised.
00043 #define DEBUG(...)              { m_serial_port.printf(__VA_ARGS__); }
00044 
00045 #define BH1745                                   //KX022, Accelerometer Sensor
00046 //#define BatteryI2C
00047 
00048 // Function Prototypes
00049 void PBTrigger();   //Interrupt function for PB4
00050 
00051 // Global Variables
00052 BLEDevice   m_ble;
00053 Serial      m_serial_port(p9, p11);  // TX pin, RX pin
00054 DigitalOut  m_cmd_led(LED1);  //Original Declaration
00055 //PwmOut      m_cmd_led(LED1);  //Original Declaration
00056 PwmOut      PWM(p4);
00057 PwmOut      hornFreq(p13);
00058 DigitalOut  m_error_led(LED2);
00059 UARTService *m_uart_service_ptr; 
00060 //InterruptIn   sw4Press(p18); 
00061 InterruptIn   resetCruise(p21); 
00062 DigitalIn   hornEnable(p18);  
00063 InterruptIn setCruise(p20); 
00064 I2C         i2c(p7,p30);
00065 
00066 //C12832  lcd(p25, p29, p28, p1, p0);
00067 //DigitalOut  LCD_RESET(p24);
00068 DigitalOut    LED_Enable(p1);
00069 //DigitalOut    LED_EnableR(p1);
00070 
00071 AnalogIn   brake(p3);      //Haven't done anything with this yet!
00072 char        BrakeIn, isClimbStop;
00073 
00074 //General Debug
00075 //DigitalOut  Test1(p12);
00076 //DigitalOut  Test1(p13);
00077 //DigitalOut  Test2(p14);
00078 //DigitalOut  Test3(p15);
00079 
00080 
00081 AnalogIn    Hall(p5);      //p3 on prior EVK, p5 on the shield
00082 AnalogIn    Speed(p2);      //p3 on prior EVK, p5 on the shield
00083 uint16_t    Speed_value;
00084 uint16_t    Speed_Thres = 1000;
00085 uint16_t    VsetThresh  = 1;
00086 uint16_t    ADC_value,isIncreasing;
00087 uint16_t    deltaStep = 5;
00088 uint16_t    OutputLimit = 20;//10000;   //Maximum increase ADC Value per loop
00089 uint16_t    LoopDelay = 50;    //Loop Time in ms
00090 uint16_t    HALL_value;
00091 uint16_t    ADC_value_old = 0;
00092 uint16_t    deltaT = 100;
00093 uint16_t    ampModeDecrease = 50;
00094 uint16_t    tick;
00095 uint16_t  pOffset = 54000;
00096 uint16_t  pGain   = 150;
00097 uint16_t  ExtModeGain = 150;
00098 uint16_t  AmpModeGain = 250;
00099 uint16_t isDecreasing, currSpeed,prevSpeed;
00100 uint16_t climbCnt;
00101 uint16_t climbPer = 100;
00102 uint16_t autoClimbThresh = 110;  
00103 uint16_t isLocked = 0;
00104 uint16_t timerX, timerY, timerZ, isLightOn;
00105 float       ADC_output;
00106 uint16_t SpeedSampNum = 3;
00107 uint16_t speedDelta = 24;
00108 uint16_t Mode = 0;
00109 //string      OutSetting;
00110 uint16_t isConnected = 0; 
00111 int32_t       p, LoopID;
00112 int16_t       cruiseSet = 0;
00113 int16_t       cruiseFlag = 0;
00114 int16_t       SpeedSet=0;
00115 int16_t       speedStep = 10;
00116 int16_t       i,j;
00117 int16_t       tempSpeed; 
00118 int16_t       isAmpMode = 0;
00119 bool        RepStart = true;
00120 bool        NoRepStart = false;
00121 bool        LED_Command = false;
00122 double      chromaX, chromaY, tempCIE;
00123 double      CIEx[60];
00124 double      CIEy[60];
00125 
00126 double      xPPU9_5  = 0.05660;
00127 double      yPPU9_5  = 0.06549;
00128 
00129 
00130 double      xPPU9_4  = 0.05072;
00131 double      yPPU9_4  = 0.04818;
00132 
00133 double      xPPU2_14 = 0.03221;
00134 double      yPPU2_14 = 0.02597;
00135 
00136 double      xPPU18_5 = 0.07085;
00137 double      yPPU18_5 = 0.06684;
00138 
00139 double      xPPU18_4 = 0.04707;
00140 double      yPPU18_4 = 0.04474;
00141                        
00142 double      xPPU18_3 = 0.03384;
00143 double      yPPU18_3 = 0.03223;
00144 
00145 double      xPPU17_18 = 0.03440;
00146 double      yPPU17_18 = 0.03201;
00147 
00148 double      xPPU5_6 = 0.04115;
00149 double      yPPU5_6 = 0.03835;
00150 
00151 double      xPPU4_15 = 0.08183;
00152 double      yPPU4_15 = 0.07357;
00153 
00154 double      xPPU4_14 = 0.07272;
00155 double      yPPU4_14 = 0.06621;
00156 
00157 double      xPPU4_13 = 0.07797;
00158 double      yPPU4_13 = 0.07208;
00159 
00160 double      xPPU4_10 = 0.09362;
00161 double      yPPU4_10 = 0.08590;
00162 
00163 double      xPPU4_8 = 0.07635;
00164 double      yPPU4_8 = 0.07052;
00165 
00166             
00167 double      xThresh =  0.0004;
00168 double      yThresh =  0.0004;
00169                     
00170 string  dataReceived = "aa";
00171         uint8_t  buf[19];
00172         uint32_t len = 0;
00173         string  OutSetting = "";
00174 
00175 
00176 #ifdef BH1745
00177 int         BH1745_addr_w = 0x72;
00178 int         BH1745_addr_r = 0x73;
00179 
00180 char        BH1745_CTRL2[2] = {0x42, 0x11};  
00181 
00182 char        BH1745_Content_ReadData[8];          
00183 
00184 float       BH1745_Red;
00185 float       BH1745_Green;
00186 float       BH1745_Blue;
00187 float       BH1745_Clear;
00188 
00189 float       SUM_BH1745_Red[10];
00190 float       SUM_BH1745_Green[10];
00191 float       SUM_BH1745_Blue[10];
00192 float       SUM_BH1745_Clear[10];
00193 
00194 double      colorSum;
00195 int         colorNum;
00196 
00197 short int   BH1745_Raw_R = 0;
00198 short int   BH1745_Raw_G = 0;
00199 short int   BH1745_Raw_B = 0; 
00200 short int   BH1745_Raw_C = 0; 
00201 
00202 char        BH1745_Red_Data_LSB = 0x50;    
00203 
00204 
00205             int sampleNum=0;
00206             char message[20];
00207             float rValue, gValue, bValue, cValue;
00208             
00209             float colorError;
00210         
00211 #endif
00212 
00213 #ifdef BatteryI2C  
00214 int         BMS_addr_w = 0x16;  //0x2C;
00215 int         BMS_addr_r = 0x17;  //0x2D;
00216 
00217 char        BMS_Content_ReadData[9];
00218 char        BMS_DeviceName = 0x21;
00219 char        BMS_Cycles = 0x17; 
00220 char        BMS_SOC = 0x0D;
00221 
00222 uint16_t    BMS_Password0;
00223 uint16_t    BMS_Password1;
00224 uint16_t    BMS_Password2;
00225 uint16_t    BMS_Password3;
00226 uint16_t    BMS_Password4;
00227 uint16_t    BMS_Password5;
00228 uint16_t    BMS_Password6;
00229 uint16_t    BMS_Password7;
00230 uint16_t    BMS_Password8;
00231 uint16_t    BMS_CycleOut;
00232 uint16_t    BMS_SOCOut; 
00233 
00234 #endif
00235 
00236 /**
00237 * This callback is used whenever a disconnection occurs.
00238 */
00239 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
00240 {
00241     switch (reason) {
00242     case Gap::REMOTE_USER_TERMINATED_CONNECTION:
00243         DEBUG("Disconnected (REMOTE_USER_TERMINATED_CONNECTION)\n\r");
00244         break;
00245     case Gap::LOCAL_HOST_TERMINATED_CONNECTION:
00246         DEBUG("Disconnected (LOCAL_HOST_TERMINATED_CONNECTION)\n\r");
00247         break;
00248     case Gap::CONN_INTERVAL_UNACCEPTABLE:
00249         DEBUG("Disconnected (CONN_INTERVAL_UNACCEPTABLE)\n\r");
00250         break;
00251     }
00252     isConnected = 0; 
00253     
00254                                 LED_Enable = 1;  
00255                                 wait_ms(100);   
00256                                 LED_Enable = 0;  
00257                                 
00258     DEBUG("Restarting the advertising process\n\r");
00259     m_ble.startAdvertising();
00260 }
00261 
00262 /**
00263 * This callback is used whenever the host writes data to one of our GATT characteristics.
00264 */
00265 void dataWrittenCallback(const GattCharacteristicWriteCBParams *params)
00266 {
00267     // Ensure that initialization is finished and the host has written to the TX characteristic.
00268     if ((m_uart_service_ptr != NULL) && (params->charHandle == m_uart_service_ptr->getTXCharacteristicHandle())) { 
00269         len = 0; 
00270 //        uint16_t TempInt = 1; 
00271  
00272                    /*
00273                     j = 0;
00274                     for( int i = (params->len) - 1; i > 0; i-- )
00275                     { 
00276                         dataReceived[j] = params->data[j];
00277                         j++;
00278                     } 
00279                     dataReceived[j] = params->data[j];
00280                     dataReceived[++j] = 0;
00281                     */
00282                 
00283         if (params->len == 1) {
00284             switch (params->data[0]) {  
00285                 #ifdef BH1745 // 
00286                 case 'r':  
00287                     len = snprintf((char*) buf, MAX_REPLY_LEN, "Red= %f", BH1745_Red); 
00288                     break;
00289                 case 'g':  
00290                     len = snprintf((char*) buf, MAX_REPLY_LEN, "Green= %f", BH1745_Green); 
00291                     break; 
00292                 case 'b':  
00293                     len = snprintf((char*) buf, MAX_REPLY_LEN, "Blue= %f", BH1745_Blue); 
00294                     break; 
00295                 case 'c':  
00296                     len = snprintf((char*) buf, MAX_REPLY_LEN, "Clear= %f", BH1745_Clear); 
00297                     break;  
00298                 case 'x':  
00299                     len = snprintf((char*) buf, MAX_REPLY_LEN, "CIE#%6.5f,%6.5f", chromaX, chromaY);  
00300                     break;   
00301                     /*
00302                 case '?':  
00303                     if( (0.03212-0.0005<chromaX) && (chromaX<0.03212+0.0005) && (0.02598-0.0005<chromaY) && (chromaY<0.02598+0.0005) ){
00304                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");  
00305                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00306                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "2-14");  
00307                     }
00308                     else if( (0.07793-0.0005<chromaX) && (chromaX<0.07793+0.0005) && (0.07237-0.0005<chromaY) && (chromaY<0.07237+0.0005) ){
00309                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");  
00310                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00311                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-13");  
00312                     }
00313                     else if( (0.07256-0.0005<chromaX) && (chromaX<0.07256+0.0005) && (0.06636-0.0005<chromaY) && (chromaY<0.06636+0.0005) ){
00314                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");  
00315                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00316                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-14");  
00317                     }
00318                     else if( (0.07079-0.0005<chromaX) && (chromaX<0.07079+0.0005) && (0.06706-0.0005<chromaY) && (chromaY<0.06706+0.0005) ){
00319                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");  
00320                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00321                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-5");  
00322                     }
00323                     else if( (0.04708-0.0005<chromaX) && (chromaX<0.04708+0.0005) && (0.04485-0.0005<chromaY) && (chromaY<0.04485+0.0005) ){
00324                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");  
00325                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00326                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-4");  
00327                     }
00328                     else if( (0.03373-0.0005<chromaX) && (chromaX<0.03373+0.0005) && (0.03221-0.0005<chromaY) && (chromaY<0.03221+0.0005) ){
00329                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");  
00330                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00331                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-3");  
00332                     }
00333                     else if( (0.07630-0.0005<chromaX) && (chromaX<0.07630+0.0005) && (0.07077-0.0005<chromaY) && (chromaY<0.07077+0.0005) ){
00334                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");  
00335                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00336                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-8");  
00337                     }
00338                     else if( (0.09365-0.0005<chromaX) && (chromaX<0.09365+0.0005) && (0.08628-0.0005<chromaY) && (chromaY<0.08628+0.0005) ){
00339                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");  
00340                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00341                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-10");  
00342                     }
00343                     else if( (0.09483-0.0005<chromaX) && (chromaX<0.09483+0.0005) && (0.08657-0.0005<chromaY) && (chromaY<0.08657+0.0005) ){
00344                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");  
00345                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00346                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-11");  
00347                     }
00348                     else if( (0.08935-0.0005<chromaX) && (chromaX<0.08935+0.0005) && (0.08311-0.0005<chromaY) && (chromaY<0.08311+0.0005) ){
00349                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");  
00350                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00351                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-12");  
00352                     }
00353                     else if( (0.08176-0.0005<chromaX) && (chromaX<0.08176+0.0005) && (0.07384-0.0005<chromaY) && (chromaY<0.07384+0.0005) ){
00354                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");  
00355                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00356                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-15");  
00357                     }
00358                     else if( (0.04112-0.0005<chromaX) && (chromaX<0.04112+0.0005) && (0.03841-0.0005<chromaY) && (chromaY<0.03841+0.0005) ){
00359                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");  
00360                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00361                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-6");  
00362                     }
00363                     else if( (0.03577-0.0005<chromaX) && (chromaX<0.03577+0.0005) && (0.03325-0.0005<chromaY) && (chromaY<0.03325+0.0005) ){
00364                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");  
00365                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00366                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-5");  
00367                     }
00368                     else if( (0.03432-0.0005<chromaX) && (chromaX<0.03432+0.0005) && (0.03203-0.0005<chromaY) && (chromaY<0.03203+0.0005) ){
00369                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");  
00370                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00371                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "17-18");  
00372                     }
00373                     else{
00374                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "Unregistered");  
00375                     }
00376                     break;   
00377                     */
00378                     
00379                 case 'l': 
00380                     if(LED_Enable==0){
00381                                 LED_Enable = 1;  
00382                                 wait_ms(100);   
00383                         len = snprintf((char*) buf, MAX_REPLY_LEN, "LED ON"); 
00384                     }
00385                     else{
00386                                 LED_Enable = 0;  
00387                                 wait_ms(100);   
00388                         len = snprintf((char*) buf, MAX_REPLY_LEN, "LED OFF"); 
00389                     }
00390                     
00391                     break;  
00392                 #endif   
00393  
00394                 default:
00395                     len = snprintf((char*) buf, MAX_REPLY_LEN, "ERROR");
00396                     break;
00397             }
00398         }/*
00399         else if (params->len > 1) {
00400             switch (params->data[0]) {
00401                 case 's':   
00402                         
00403                         rValue = BH1745_Red;
00404                         gValue = BH1745_Green;
00405                         bValue = BH1745_Blue;
00406                         cValue = BH1745_Clear; 
00407                          
00408                     break; 
00409             }
00410         } */
00411         else
00412         {
00413             len = snprintf((char*) buf, MAX_REPLY_LEN, "ERROR");
00414         }
00415 
00416         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00417         DEBUG("%d bytes received from host\n\r", params->len);
00418     }
00419 }
00420 
00421 
00422 /**
00423 * This callback is used whenever a write to a GATT characteristic causes data to be sent to the host.
00424 */
00425 void dataSentCallback(unsigned count)
00426 {
00427     // NOTE: The count always seems to be 1 regardless of data.
00428     DEBUG("%d bytes sent to host\n\r", count);
00429 }
00430 
00431 
00432 /**
00433 * This callback is scheduled to be called periodically via a low-priority interrupt.
00434 */
00435 void periodicCallback(void)
00436 {
00437      
00438  //   uint8_t  buf[MAX_REPLY_LEN];
00439 //    uint32_t len = 0;
00440     
00441 
00442          
00443     if (m_ble.getGapState().connected) { 
00444             if(isConnected==0){ 
00445         }
00446         isConnected = 1;  
00447     } 
00448     else{
00449         isConnected = 0; 
00450     }
00451     
00452 }
00453 
00454 
00455 void error(ble_error_t err, uint32_t line)
00456 {
00457     m_error_led = 1;
00458     DEBUG("Error %d on line number %d\n\r", err, line);
00459 }
00460 
00461 //Code for writing back to Console if a PB is pressed
00462 void PBTrigger()
00463 {
00464     //uint8_t  buf[MAX_REPLY_LEN];
00465     //uint32_t len = 0;
00466     
00467     m_cmd_led = !m_cmd_led;
00468   
00469     if(!setCruise){  
00470         cruiseFlag = 1;
00471         cruiseSet = Speed_value*2.75;
00472     }
00473     if(!resetCruise){
00474         cruiseFlag = 0;
00475     }
00476     m_cmd_led = cruiseFlag;
00477     
00478 
00479     /*    
00480     if (m_ble.getGapState().connected) {
00481     }
00482     */
00483 }
00484 
00485 int main(void)
00486 {
00487     ble_error_t err;
00488     Ticker      ticker;
00489 
00490     m_serial_port.baud(UART_BAUD_RATE);
00491 
00492     DEBUG("Initialising...\n\r");
00493 
00494     m_cmd_led      = 0;
00495     m_error_led    = 0;
00496 
00497     //ticker.attach(periodicCallback, SENSOR_READ_INTERVAL_S);
00498 
00499     //sw4Press.fall(&PBTrigger);  
00500     
00501     setCruise.fall(&PBTrigger);  
00502     resetCruise.fall(&PBTrigger);  
00503 
00504     //Start BTLE Initialization Section
00505     m_ble.init();
00506     m_ble.onDisconnection(disconnectionCallback);
00507     m_ble.onDataWritten(dataWrittenCallback);
00508     m_ble.onDataSent(dataSentCallback);
00509 
00510     // Set the TX power in dBm units.
00511     // Possible values (in decreasing order): 4, 0, -4, -8, -12, -16, -20.
00512     err = m_ble.setTxPower(4);
00513     if (BLE_ERROR_NONE != err) {
00514         error(err, __LINE__);
00515     }
00516 
00517     // Setup advertising (GAP stuff).
00518     err = m_ble.setDeviceName(DEVICE_NAME);
00519     if (BLE_ERROR_NONE != err) {
00520         error(err, __LINE__);
00521     }
00522 
00523     err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00524     if (BLE_ERROR_NONE != err) {
00525         error(err, __LINE__);
00526     }
00527 
00528     m_ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00529 
00530     err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00531                                                 (const uint8_t *)SHORT_NAME,
00532                                                 (sizeof(SHORT_NAME) - 1));
00533     if (BLE_ERROR_NONE != err) {
00534         error(err, __LINE__);
00535     }
00536 
00537     err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
00538                                                 (const uint8_t *)UARTServiceUUID_reversed,
00539                                                 sizeof(UARTServiceUUID_reversed));
00540     if (BLE_ERROR_NONE != err) {
00541         error(err, __LINE__);
00542     }
00543 
00544     m_ble.setAdvertisingInterval(Gap::MSEC_TO_ADVERTISEMENT_DURATION_UNITS(ADV_INTERVAL_MS));
00545     m_ble.startAdvertising();
00546 
00547     // Create a UARTService object (GATT stuff).
00548     UARTService uartService(m_ble);
00549     m_uart_service_ptr = &uartService;
00550  
00551     #ifdef BH1745
00552     i2c.write(BH1745_addr_w, &BH1745_CTRL2[0], 2, false); 
00553     #endif
00554 
00555     #ifdef BatteryI2C  
00556         i2c.write(BMS_addr_w, &BMS_DeviceName, 1, RepStart);
00557         i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 8, NoRepStart);
00558     
00559         //BMS_Password = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]);
00560         BMS_Password0 = (BMS_Content_ReadData[0]);
00561         BMS_Password1 = (BMS_Content_ReadData[1]);
00562         BMS_Password2 = (BMS_Content_ReadData[2]);
00563         BMS_Password3 = (BMS_Content_ReadData[3]);
00564         BMS_Password4 = (BMS_Content_ReadData[4]);
00565         BMS_Password5 = (BMS_Content_ReadData[5]);
00566         BMS_Password6 = (BMS_Content_ReadData[6]);
00567         BMS_Password7 = (BMS_Content_ReadData[7]);
00568         BMS_Password8 = (BMS_Content_ReadData[8]); 
00569         i2c.write(BMS_addr_w, &BMS_Cycles, 1, RepStart);
00570         i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 2, NoRepStart);
00571         BMS_CycleOut = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]); 
00572         
00573         i2c.write(BMS_addr_w, &BMS_SOC, 1, RepStart);
00574         i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 2, NoRepStart);
00575         BMS_SOCOut = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]);
00576     #endif
00577         
00578     //Initialize PWM Output
00579     PWM.period_us(1000);
00580     hornFreq.period_us(1250);
00581     //m_cmd_led.period_ms(10);
00582     ADC_value_old = 0;
00583  
00584     p = 0; 
00585     SpeedSet = 1;
00586     tick = deltaT;
00587     SpeedSampNum = 5;
00588     
00589                     LED_Enable = 1;  
00590                     wait_ms(100); 
00591                     wait_ms(100);  
00592                     LED_Enable = 0;  
00593                     wait_ms(100); 
00594                     wait_ms(100);   
00595                     LED_Enable = 1;  
00596                     wait_ms(100); 
00597                     wait_ms(100);  
00598                     LED_Enable = 0;  
00599                     wait_ms(100); 
00600                     wait_ms(100);   
00601                     LED_Enable = 1;  
00602                     wait_ms(100); 
00603                     wait_ms(100);  
00604                     LED_Enable = 0;  
00605                     wait_ms(100); 
00606                     wait_ms(100);   
00607 
00608     while (true) {
00609         //m_ble.waitForEvent();  
00610         if(m_ble.getGapState().connected){
00611                 if(!isConnected){
00612                     LED_Enable = 1;  
00613                     wait_ms(100); 
00614                     wait_ms(100);  
00615                     LED_Enable = 0;  
00616                     wait_ms(100); 
00617                     wait_ms(100);   
00618                     LED_Enable = 1;  
00619                     wait_ms(100); 
00620                     wait_ms(100);  
00621                     LED_Enable = 0;  
00622                     wait_ms(100); 
00623                     wait_ms(100);   
00624                     LED_Enable = 1;  
00625                     wait_ms(100); 
00626                     wait_ms(100);  
00627                     LED_Enable = 0;  
00628                     wait_ms(100); 
00629                     wait_ms(100);   
00630                 }
00631                 isConnected = 1;
00632                 
00633                 //Read KX022 Portion from the IC
00634                 i2c.write(BH1745_addr_w, &BH1745_Red_Data_LSB, 1, RepStart);
00635                 i2c.read(BH1745_addr_r, &BH1745_Content_ReadData[0], 8, NoRepStart);
00636                     
00637                 //reconfigure the data (taken from arduino code)
00638                 BH1745_Raw_R = (BH1745_Content_ReadData[1]<<8) | (BH1745_Content_ReadData[0]);
00639                 BH1745_Raw_G = (BH1745_Content_ReadData[3]<<8) | (BH1745_Content_ReadData[2]);
00640                 BH1745_Raw_B = (BH1745_Content_ReadData[5]<<8) | (BH1745_Content_ReadData[4]);  
00641                 BH1745_Raw_C = (BH1745_Content_ReadData[7]<<8) | (BH1745_Content_ReadData[6]);       
00642           
00643                 BH1745_Red = (float)BH1745_Raw_R / 16384; 
00644                 BH1745_Green = (float)BH1745_Raw_G / 16384; 
00645                 BH1745_Blue = (float)BH1745_Raw_B / 16384; 
00646                 BH1745_Clear = (float)BH1745_Raw_C / 16384; 
00647                 
00648                 if(BH1745_Clear < 0.00001){
00649                     
00650                     wait_ms(100); 
00651                     wait_ms(100); 
00652                     wait_ms(100); 
00653                     wait_ms(100);   
00654                     wait_ms(100); 
00655                     wait_ms(100); 
00656                     wait_ms(100); 
00657                     wait_ms(100);   
00658                     LED_Enable = 1;  
00659                     wait_ms(100); 
00660                     wait_ms(100);  
00661                     
00662                     for(i=0; i<60; i++){
00663                         
00664                         //---------------------------- KX022 ----------------------------
00665                         //Read KX022 Portion from the IC
00666                         i2c.write(BH1745_addr_w, &BH1745_Red_Data_LSB, 1, RepStart);
00667                         i2c.read(BH1745_addr_r, &BH1745_Content_ReadData[0], 8, NoRepStart);
00668                             
00669                         //reconfigure the data (taken from arduino code)
00670                         BH1745_Raw_R = (BH1745_Content_ReadData[1]<<8) | (BH1745_Content_ReadData[0]);
00671                         BH1745_Raw_G = (BH1745_Content_ReadData[3]<<8) | (BH1745_Content_ReadData[2]);
00672                         BH1745_Raw_B = (BH1745_Content_ReadData[5]<<8) | (BH1745_Content_ReadData[4]);  
00673                         BH1745_Raw_C = (BH1745_Content_ReadData[7]<<8) | (BH1745_Content_ReadData[6]);       
00674                       
00675                         LED_Enable = 0;        
00676                     
00677                         BH1745_Red = (float)BH1745_Raw_R / 16384; 
00678                         BH1745_Green = (float)BH1745_Raw_G / 16384; 
00679                         BH1745_Blue = (float)BH1745_Raw_B / 16384; 
00680                         BH1745_Clear = (float)BH1745_Raw_C / 16384; 
00681                       
00682                         CIEx[i] = 0.232*BH1745_Red + 0.610*BH1745_Green + 0.055*BH1745_Blue;
00683                         if( BH1745_Clear < (0.2*BH1745_Green) ){
00684                             if( BH1745_Blue < (0.45*BH1745_Green) ){
00685                                 CIEy[i] = 0.083*BH1745_Red + 0.860*BH1745_Green;
00686                             }
00687                             else{
00688                                 CIEy[i] = 0.070*BH1745_Red + 0.709*BH1745_Green;
00689                             }
00690                         }
00691                         else if( BH1745_Clear < (0.50*BH1745_Green) ){
00692                                 CIEy[i] = 0.110*BH1745_Red + 0.712*BH1745_Green;
00693                         }
00694                         else{
00695                             CIEy[i] = 0.073*BH1745_Red + 0.864*BH1745_Green;
00696                         }
00697                         for(j=i; j>0; j--){
00698                             tempCIE = CIEx[j];
00699                             if(tempCIE<CIEx[j-1]){
00700                                 CIEx[j] = CIEx[j-1];
00701                                 CIEx[j-1] = tempCIE;
00702                             }
00703                         }
00704                         for(j=i; j>0; j--){
00705                             tempCIE = CIEy[j];
00706                             if(tempCIE<CIEy[j-1]){
00707                                 CIEy[j] = CIEy[j-1];
00708                                 CIEy[j-1] = tempCIE;
00709                             }
00710                         }
00711                     }   
00712                         
00713                     
00714                     chromaX = (CIEx[28]+CIEx[29]+CIEx[30]+CIEx[31]+CIEx[32])/5.0;
00715                     chromaY = (CIEy[28]+CIEy[29]+CIEy[30]+CIEy[31]+CIEy[32])/5.0;
00716                       
00717                     if( (xPPU2_14-xThresh<chromaX) && (chromaX<xPPU2_14+xThresh) && (yPPU2_14-yThresh<chromaY) && (chromaY<yPPU2_14+yThresh) ){
00718                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "2-14");  
00719                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00720                     }
00721                     else if( (xPPU18_5-xThresh<chromaX) && (chromaX<xPPU18_5+xThresh) && (yPPU18_5-yThresh<chromaY) && (chromaY<yPPU18_5+yThresh) ){
00722                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-5");  
00723                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00724                     }  
00725                     else if( (xPPU18_4-xThresh<chromaX) && (chromaX<xPPU18_4+xThresh) && (yPPU18_4-yThresh<chromaY) && (chromaY<yPPU18_4+yThresh) ){
00726                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-4");  
00727                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00728                     } 
00729                     else if( (xPPU18_3-xThresh<chromaX) && (chromaX<xPPU18_3+xThresh) && (yPPU18_3-yThresh<chromaY) && (chromaY<yPPU18_3+yThresh) ){
00730                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-3");  
00731                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00732                     } 
00733                     else if( (xPPU17_18-xThresh<chromaX) && (chromaX<xPPU17_18+xThresh) && (yPPU17_18-yThresh<chromaY) && (chromaY<yPPU17_18+yThresh) ){
00734                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "17-18");  
00735                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00736                     } 
00737                     else if( (xPPU5_6-xThresh<chromaX) && (chromaX<xPPU5_6+xThresh) && (yPPU5_6-yThresh<chromaY) && (chromaY<yPPU5_6+yThresh) ){
00738                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-6");  
00739                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00740                     }
00741                     else if( (xPPU4_15-xThresh<chromaX) && (chromaX<xPPU4_15+xThresh) && (yPPU4_15-yThresh<chromaY) && (chromaY<yPPU4_15+yThresh) ){
00742                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-15");  
00743                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00744                     }  
00745                     else if( (xPPU4_14-xThresh<chromaX) && (chromaX<xPPU4_14+xThresh) && (yPPU4_14-yThresh<chromaY) && (chromaY<yPPU4_14+yThresh) ){
00746                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-14");  
00747                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00748                     }
00749                     else if( (xPPU4_13-xThresh<chromaX) && (chromaX<xPPU4_13+xThresh) && (yPPU4_13-yThresh<chromaY) && (chromaY<yPPU4_13+yThresh) ){
00750                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-13");  
00751                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00752                     } 
00753                     else if( (xPPU4_10-xThresh<chromaX) && (chromaX<xPPU4_10+xThresh) && (yPPU4_10-yThresh<chromaY) && (chromaY<yPPU4_10+yThresh) ){
00754                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-10");  
00755                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00756                     }
00757                     else if( (xPPU4_8-xThresh<chromaX) && (chromaX<xPPU4_8+xThresh) && (yPPU4_8-yThresh<chromaY) && (chromaY<yPPU4_8+yThresh) ){
00758                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-8");  
00759                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00760                     }                      
00761                     else if( (xPPU9_4-xThresh<chromaX) && (chromaX<xPPU9_4+xThresh) && (yPPU9_4-yThresh<chromaY) && (chromaY<yPPU9_4+yThresh) ){
00762                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "9-4");  
00763                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00764                     }       
00765                     else if( (xPPU9_5-xThresh<chromaX) && (chromaX<xPPU9_5+xThresh) && (yPPU9_5-yThresh<chromaY) && (chromaY<yPPU9_5+yThresh) ){
00766                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "9-5");  
00767                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00768                     }        
00769                     else{
00770                         len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "Unregistered");  
00771                         m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
00772                     }
00773                 }
00774                 
00775         }
00776         else{
00777                 isConnected = 0;
00778         }
00779     }
00780 }