Francis Lee / Mbed 2 deprecated IRIS

test

Dependencies:   BLE_API mbed nRF51822

main.cpp@0:a394a04bd0b7, 2016-02-26 (annotated)

Committer:
sk84life85
Date:
Fri Feb 26 22:53:44 2016 +0000
Revision:
0:a394a04bd0b7
Child:
1:88e3620627f9
Revision 1.1;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sk84life85 0:a394a04bd0b7 1 /*
sk84life85 0:a394a04bd0b7 2 SAD: 0x72
sk84life85 0:a394a04bd0b7 3 TASK: WRITE 0x12 to register 0x42 then start reading.
sk84life85 0:a394a04bd0b7 4
sk84life85 0:a394a04bd0b7 5
sk84life85 0:a394a04bd0b7 6 mbed Microcontroller Library
sk84life85 0:a394a04bd0b7 7 * Copyright (c) 2006-2013 ARM Limited
sk84life85 0:a394a04bd0b7 8 *
sk84life85 0:a394a04bd0b7 9 * Licensed under the Apache License, Version 2.0 (the "License");
sk84life85 0:a394a04bd0b7 10 * you may not use this file except in compliance with the License.
sk84life85 0:a394a04bd0b7 11 * You may obtain a copy of the License at
sk84life85 0:a394a04bd0b7 12 *
sk84life85 0:a394a04bd0b7 13 * http://www.apache.org/licenses/LICENSE-2.0
sk84life85 0:a394a04bd0b7 14 *
sk84life85 0:a394a04bd0b7 15 * Unless required by applicable law or agreed to in writing, software
sk84life85 0:a394a04bd0b7 16 * distributed under the License is distributed on an "AS IS" BASIS,
sk84life85 0:a394a04bd0b7 17 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
sk84life85 0:a394a04bd0b7 18 * See the License for the specific language governing permissions and
sk84life85 0:a394a04bd0b7 19 * limitations under the License.
sk84life85 0:a394a04bd0b7 20 */
sk84life85 0:a394a04bd0b7 21
sk84life85 0:a394a04bd0b7 22 //#define AnalogTemp //BDE0600
sk84life85 0:a394a04bd0b7 23 //#define AnalogUV //ML8511
sk84life85 0:a394a04bd0b7 24 //#define HallSensor //BU52011 //Change 1: Change to use GPIO for BU52014
sk84life85 0:a394a04bd0b7 25 //#define RPR0521 //RPR0521 //Change 2: Remove This and add in the RPR-0521
sk84life85 0:a394a04bd0b7 26 //#define KMX62 //Change 3: Add Code For BH1745, KX022, BM1383GLV, KMX62
sk84life85 0:a394a04bd0b7 27
sk84life85 0:a394a04bd0b7 28 #include "mbed.h"
sk84life85 0:a394a04bd0b7 29 #include "BLEDevice.h"
sk84life85 0:a394a04bd0b7 30 #include "UARTService.h"
sk84life85 0:a394a04bd0b7 31 #include "nrf_temp.h"
sk84life85 0:a394a04bd0b7 32 #include "I2C.h"
sk84life85 0:a394a04bd0b7 33 #include <string>
sk84life85 0:a394a04bd0b7 34 #include "stdlib.h"
sk84life85 0:a394a04bd0b7 35 //#include "C12832.h"
sk84life85 0:a394a04bd0b7 36
sk84life85 0:a394a04bd0b7 37 #define MAX_REPLY_LEN (UARTService::BLE_UART_SERVICE_MAX_DATA_LEN) //Actually equal to 20
sk84life85 0:a394a04bd0b7 38 #define SENSOR_READ_INTERVAL_S (0.2F)
sk84life85 0:a394a04bd0b7 39 #define ADV_INTERVAL_MS (1000UL)
sk84life85 0:a394a04bd0b7 40 #define UART_BAUD_RATE (19200UL)
sk84life85 0:a394a04bd0b7 41 #define DEVICE_NAME ("Rohm") // This can be read AFTER connecting to the device.
sk84life85 0:a394a04bd0b7 42 #define SHORT_NAME ("BH1745") // Keep this short: max 8 chars if a 128bit UUID is also advertised.
sk84life85 0:a394a04bd0b7 43 #define DEBUG(...) { m_serial_port.printf(__VA_ARGS__); }
sk84life85 0:a394a04bd0b7 44
sk84life85 0:a394a04bd0b7 45 #define BH1745 //KX022, Accelerometer Sensor
sk84life85 0:a394a04bd0b7 46 //#define BatteryI2C
sk84life85 0:a394a04bd0b7 47
sk84life85 0:a394a04bd0b7 48 // Function Prototypes
sk84life85 0:a394a04bd0b7 49 void PBTrigger(); //Interrupt function for PB4
sk84life85 0:a394a04bd0b7 50
sk84life85 0:a394a04bd0b7 51 // Global Variables
sk84life85 0:a394a04bd0b7 52 BLEDevice m_ble;
sk84life85 0:a394a04bd0b7 53 Serial m_serial_port(p9, p11); // TX pin, RX pin
sk84life85 0:a394a04bd0b7 54 DigitalOut m_cmd_led(LED1); //Original Declaration
sk84life85 0:a394a04bd0b7 55 //PwmOut m_cmd_led(LED1); //Original Declaration
sk84life85 0:a394a04bd0b7 56 PwmOut PWM(p4);
sk84life85 0:a394a04bd0b7 57 PwmOut hornFreq(p13);
sk84life85 0:a394a04bd0b7 58 DigitalOut m_error_led(LED2);
sk84life85 0:a394a04bd0b7 59 UARTService *m_uart_service_ptr;
sk84life85 0:a394a04bd0b7 60 //InterruptIn sw4Press(p18);
sk84life85 0:a394a04bd0b7 61 InterruptIn resetCruise(p21);
sk84life85 0:a394a04bd0b7 62 DigitalIn hornEnable(p18);
sk84life85 0:a394a04bd0b7 63 InterruptIn setCruise(p20);
sk84life85 0:a394a04bd0b7 64 I2C i2c(p7,p30);
sk84life85 0:a394a04bd0b7 65
sk84life85 0:a394a04bd0b7 66 //C12832 lcd(p25, p29, p28, p1, p0);
sk84life85 0:a394a04bd0b7 67 //DigitalOut LCD_RESET(p24);
sk84life85 0:a394a04bd0b7 68 DigitalOut LED_Enable(p1);
sk84life85 0:a394a04bd0b7 69 //DigitalOut LED_EnableR(p1);
sk84life85 0:a394a04bd0b7 70
sk84life85 0:a394a04bd0b7 71 AnalogIn brake(p3); //Haven't done anything with this yet!
sk84life85 0:a394a04bd0b7 72 char BrakeIn, isClimbStop;
sk84life85 0:a394a04bd0b7 73
sk84life85 0:a394a04bd0b7 74 //General Debug
sk84life85 0:a394a04bd0b7 75 //DigitalOut Test1(p12);
sk84life85 0:a394a04bd0b7 76 //DigitalOut Test1(p13);
sk84life85 0:a394a04bd0b7 77 //DigitalOut Test2(p14);
sk84life85 0:a394a04bd0b7 78 //DigitalOut Test3(p15);
sk84life85 0:a394a04bd0b7 79
sk84life85 0:a394a04bd0b7 80
sk84life85 0:a394a04bd0b7 81 AnalogIn Hall(p5); //p3 on prior EVK, p5 on the shield
sk84life85 0:a394a04bd0b7 82 AnalogIn Speed(p2); //p3 on prior EVK, p5 on the shield
sk84life85 0:a394a04bd0b7 83 uint16_t Speed_value;
sk84life85 0:a394a04bd0b7 84 uint16_t Speed_Thres = 1000;
sk84life85 0:a394a04bd0b7 85 uint16_t VsetThresh = 1;
sk84life85 0:a394a04bd0b7 86 uint16_t ADC_value,isIncreasing;
sk84life85 0:a394a04bd0b7 87 uint16_t deltaStep = 5;
sk84life85 0:a394a04bd0b7 88 uint16_t OutputLimit = 20;//10000; //Maximum increase ADC Value per loop
sk84life85 0:a394a04bd0b7 89 uint16_t LoopDelay = 50; //Loop Time in ms
sk84life85 0:a394a04bd0b7 90 uint16_t HALL_value;
sk84life85 0:a394a04bd0b7 91 uint16_t ADC_value_old = 0;
sk84life85 0:a394a04bd0b7 92 uint16_t deltaT = 100;
sk84life85 0:a394a04bd0b7 93 uint16_t ampModeDecrease = 50;
sk84life85 0:a394a04bd0b7 94 uint16_t tick;
sk84life85 0:a394a04bd0b7 95 uint16_t pOffset = 54000;
sk84life85 0:a394a04bd0b7 96 uint16_t pGain = 150;
sk84life85 0:a394a04bd0b7 97 uint16_t ExtModeGain = 150;
sk84life85 0:a394a04bd0b7 98 uint16_t AmpModeGain = 250;
sk84life85 0:a394a04bd0b7 99 uint16_t isDecreasing, currSpeed,prevSpeed;
sk84life85 0:a394a04bd0b7 100 uint16_t climbCnt;
sk84life85 0:a394a04bd0b7 101 uint16_t climbPer = 100;
sk84life85 0:a394a04bd0b7 102 uint16_t autoClimbThresh = 110;
sk84life85 0:a394a04bd0b7 103 uint16_t isLocked = 0;
sk84life85 0:a394a04bd0b7 104 uint16_t timerX, timerY, timerZ, isLightOn;
sk84life85 0:a394a04bd0b7 105 float ADC_output;
sk84life85 0:a394a04bd0b7 106 uint16_t SpeedSampNum = 3;
sk84life85 0:a394a04bd0b7 107 uint16_t speedDelta = 24;
sk84life85 0:a394a04bd0b7 108 uint16_t Mode = 0;
sk84life85 0:a394a04bd0b7 109 //string OutSetting;
sk84life85 0:a394a04bd0b7 110 uint16_t isConnected = 0;
sk84life85 0:a394a04bd0b7 111 int32_t p, LoopID;
sk84life85 0:a394a04bd0b7 112 int16_t cruiseSet = 0;
sk84life85 0:a394a04bd0b7 113 int16_t cruiseFlag = 0;
sk84life85 0:a394a04bd0b7 114 int16_t SpeedSet=0;
sk84life85 0:a394a04bd0b7 115 int16_t speedStep = 10;
sk84life85 0:a394a04bd0b7 116 int16_t i,j;
sk84life85 0:a394a04bd0b7 117 int16_t tempSpeed;
sk84life85 0:a394a04bd0b7 118 int16_t isAmpMode = 0;
sk84life85 0:a394a04bd0b7 119 bool RepStart = true;
sk84life85 0:a394a04bd0b7 120 bool NoRepStart = false;
sk84life85 0:a394a04bd0b7 121 bool LED_Command = false;
sk84life85 0:a394a04bd0b7 122 double chromaX, chromaY, tempCIE;
sk84life85 0:a394a04bd0b7 123 double CIEx[60];
sk84life85 0:a394a04bd0b7 124 double CIEy[60];
sk84life85 0:a394a04bd0b7 125
sk84life85 0:a394a04bd0b7 126 double xPPU2_14 = 0.03221;
sk84life85 0:a394a04bd0b7 127 double yPPU2_14 = 0.02597;
sk84life85 0:a394a04bd0b7 128
sk84life85 0:a394a04bd0b7 129 double xPPU18_5 = 0.07085;
sk84life85 0:a394a04bd0b7 130 double yPPU18_5 = 0.06684;
sk84life85 0:a394a04bd0b7 131
sk84life85 0:a394a04bd0b7 132 double xPPU18_4 = 0.04707;
sk84life85 0:a394a04bd0b7 133 double yPPU18_4 = 0.04474;
sk84life85 0:a394a04bd0b7 134
sk84life85 0:a394a04bd0b7 135 double xPPU18_3 = 0.03384;
sk84life85 0:a394a04bd0b7 136 double yPPU18_3 = 0.03223;
sk84life85 0:a394a04bd0b7 137
sk84life85 0:a394a04bd0b7 138 double xPPU17_18 = 0.03440;
sk84life85 0:a394a04bd0b7 139 double yPPU17_18 = 0.03201;
sk84life85 0:a394a04bd0b7 140
sk84life85 0:a394a04bd0b7 141 double xPPU5_6 = 0.04115;
sk84life85 0:a394a04bd0b7 142 double yPPU5_6 = 0.03835;
sk84life85 0:a394a04bd0b7 143
sk84life85 0:a394a04bd0b7 144 double xPPU4_15 = 0.08183;
sk84life85 0:a394a04bd0b7 145 double yPPU4_15 = 0.07357;
sk84life85 0:a394a04bd0b7 146
sk84life85 0:a394a04bd0b7 147 double xPPU4_14 = 0.07272;
sk84life85 0:a394a04bd0b7 148 double yPPU4_14 = 0.06621;
sk84life85 0:a394a04bd0b7 149
sk84life85 0:a394a04bd0b7 150 double xPPU4_13 = 0.07797;
sk84life85 0:a394a04bd0b7 151 double yPPU4_13 = 0.07208;
sk84life85 0:a394a04bd0b7 152
sk84life85 0:a394a04bd0b7 153 double xPPU4_10 = 0.09362;
sk84life85 0:a394a04bd0b7 154 double yPPU4_10 = 0.08590;
sk84life85 0:a394a04bd0b7 155
sk84life85 0:a394a04bd0b7 156 double xPPU4_8 = 0.07635;
sk84life85 0:a394a04bd0b7 157 double yPPU4_8 = 0.07052;
sk84life85 0:a394a04bd0b7 158
sk84life85 0:a394a04bd0b7 159
sk84life85 0:a394a04bd0b7 160 double xThresh = 0.0004;
sk84life85 0:a394a04bd0b7 161 double yThresh = 0.0004;
sk84life85 0:a394a04bd0b7 162
sk84life85 0:a394a04bd0b7 163 string dataReceived = "aa";
sk84life85 0:a394a04bd0b7 164 uint8_t buf[19];
sk84life85 0:a394a04bd0b7 165 uint32_t len = 0;
sk84life85 0:a394a04bd0b7 166 string OutSetting = "";
sk84life85 0:a394a04bd0b7 167
sk84life85 0:a394a04bd0b7 168
sk84life85 0:a394a04bd0b7 169 #ifdef BH1745
sk84life85 0:a394a04bd0b7 170 int BH1745_addr_w = 0x72;
sk84life85 0:a394a04bd0b7 171 int BH1745_addr_r = 0x73;
sk84life85 0:a394a04bd0b7 172
sk84life85 0:a394a04bd0b7 173 char BH1745_CTRL2[2] = {0x42, 0x11};
sk84life85 0:a394a04bd0b7 174
sk84life85 0:a394a04bd0b7 175 char BH1745_Content_ReadData[8];
sk84life85 0:a394a04bd0b7 176
sk84life85 0:a394a04bd0b7 177 float BH1745_Red;
sk84life85 0:a394a04bd0b7 178 float BH1745_Green;
sk84life85 0:a394a04bd0b7 179 float BH1745_Blue;
sk84life85 0:a394a04bd0b7 180 float BH1745_Clear;
sk84life85 0:a394a04bd0b7 181
sk84life85 0:a394a04bd0b7 182 float SUM_BH1745_Red[10];
sk84life85 0:a394a04bd0b7 183 float SUM_BH1745_Green[10];
sk84life85 0:a394a04bd0b7 184 float SUM_BH1745_Blue[10];
sk84life85 0:a394a04bd0b7 185 float SUM_BH1745_Clear[10];
sk84life85 0:a394a04bd0b7 186
sk84life85 0:a394a04bd0b7 187 double colorSum;
sk84life85 0:a394a04bd0b7 188 int colorNum;
sk84life85 0:a394a04bd0b7 189
sk84life85 0:a394a04bd0b7 190 short int BH1745_Raw_R = 0;
sk84life85 0:a394a04bd0b7 191 short int BH1745_Raw_G = 0;
sk84life85 0:a394a04bd0b7 192 short int BH1745_Raw_B = 0;
sk84life85 0:a394a04bd0b7 193 short int BH1745_Raw_C = 0;
sk84life85 0:a394a04bd0b7 194
sk84life85 0:a394a04bd0b7 195 char BH1745_Red_Data_LSB = 0x50;
sk84life85 0:a394a04bd0b7 196
sk84life85 0:a394a04bd0b7 197
sk84life85 0:a394a04bd0b7 198 int sampleNum=0;
sk84life85 0:a394a04bd0b7 199 char message[20];
sk84life85 0:a394a04bd0b7 200 float rValue, gValue, bValue, cValue;
sk84life85 0:a394a04bd0b7 201
sk84life85 0:a394a04bd0b7 202 float colorError;
sk84life85 0:a394a04bd0b7 203
sk84life85 0:a394a04bd0b7 204 #endif
sk84life85 0:a394a04bd0b7 205
sk84life85 0:a394a04bd0b7 206 #ifdef BatteryI2C
sk84life85 0:a394a04bd0b7 207 int BMS_addr_w = 0x16; //0x2C;
sk84life85 0:a394a04bd0b7 208 int BMS_addr_r = 0x17; //0x2D;
sk84life85 0:a394a04bd0b7 209
sk84life85 0:a394a04bd0b7 210 char BMS_Content_ReadData[9];
sk84life85 0:a394a04bd0b7 211 char BMS_DeviceName = 0x21;
sk84life85 0:a394a04bd0b7 212 char BMS_Cycles = 0x17;
sk84life85 0:a394a04bd0b7 213 char BMS_SOC = 0x0D;
sk84life85 0:a394a04bd0b7 214
sk84life85 0:a394a04bd0b7 215 uint16_t BMS_Password0;
sk84life85 0:a394a04bd0b7 216 uint16_t BMS_Password1;
sk84life85 0:a394a04bd0b7 217 uint16_t BMS_Password2;
sk84life85 0:a394a04bd0b7 218 uint16_t BMS_Password3;
sk84life85 0:a394a04bd0b7 219 uint16_t BMS_Password4;
sk84life85 0:a394a04bd0b7 220 uint16_t BMS_Password5;
sk84life85 0:a394a04bd0b7 221 uint16_t BMS_Password6;
sk84life85 0:a394a04bd0b7 222 uint16_t BMS_Password7;
sk84life85 0:a394a04bd0b7 223 uint16_t BMS_Password8;
sk84life85 0:a394a04bd0b7 224 uint16_t BMS_CycleOut;
sk84life85 0:a394a04bd0b7 225 uint16_t BMS_SOCOut;
sk84life85 0:a394a04bd0b7 226
sk84life85 0:a394a04bd0b7 227 #endif
sk84life85 0:a394a04bd0b7 228
sk84life85 0:a394a04bd0b7 229 /**
sk84life85 0:a394a04bd0b7 230 * This callback is used whenever a disconnection occurs.
sk84life85 0:a394a04bd0b7 231 */
sk84life85 0:a394a04bd0b7 232 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
sk84life85 0:a394a04bd0b7 233 {
sk84life85 0:a394a04bd0b7 234 switch (reason) {
sk84life85 0:a394a04bd0b7 235 case Gap::REMOTE_USER_TERMINATED_CONNECTION:
sk84life85 0:a394a04bd0b7 236 DEBUG("Disconnected (REMOTE_USER_TERMINATED_CONNECTION)\n\r");
sk84life85 0:a394a04bd0b7 237 break;
sk84life85 0:a394a04bd0b7 238 case Gap::LOCAL_HOST_TERMINATED_CONNECTION:
sk84life85 0:a394a04bd0b7 239 DEBUG("Disconnected (LOCAL_HOST_TERMINATED_CONNECTION)\n\r");
sk84life85 0:a394a04bd0b7 240 break;
sk84life85 0:a394a04bd0b7 241 case Gap::CONN_INTERVAL_UNACCEPTABLE:
sk84life85 0:a394a04bd0b7 242 DEBUG("Disconnected (CONN_INTERVAL_UNACCEPTABLE)\n\r");
sk84life85 0:a394a04bd0b7 243 break;
sk84life85 0:a394a04bd0b7 244 }
sk84life85 0:a394a04bd0b7 245 isConnected = 0;
sk84life85 0:a394a04bd0b7 246
sk84life85 0:a394a04bd0b7 247 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 248 wait_ms(100);
sk84life85 0:a394a04bd0b7 249 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 250
sk84life85 0:a394a04bd0b7 251 DEBUG("Restarting the advertising process\n\r");
sk84life85 0:a394a04bd0b7 252 m_ble.startAdvertising();
sk84life85 0:a394a04bd0b7 253 }
sk84life85 0:a394a04bd0b7 254
sk84life85 0:a394a04bd0b7 255 /**
sk84life85 0:a394a04bd0b7 256 * This callback is used whenever the host writes data to one of our GATT characteristics.
sk84life85 0:a394a04bd0b7 257 */
sk84life85 0:a394a04bd0b7 258 void dataWrittenCallback(const GattCharacteristicWriteCBParams *params)
sk84life85 0:a394a04bd0b7 259 {
sk84life85 0:a394a04bd0b7 260 // Ensure that initialization is finished and the host has written to the TX characteristic.
sk84life85 0:a394a04bd0b7 261 if ((m_uart_service_ptr != NULL) && (params->charHandle == m_uart_service_ptr->getTXCharacteristicHandle())) {
sk84life85 0:a394a04bd0b7 262 len = 0;
sk84life85 0:a394a04bd0b7 263 // uint16_t TempInt = 1;
sk84life85 0:a394a04bd0b7 264
sk84life85 0:a394a04bd0b7 265 /*
sk84life85 0:a394a04bd0b7 266 j = 0;
sk84life85 0:a394a04bd0b7 267 for( int i = (params->len) - 1; i > 0; i-- )
sk84life85 0:a394a04bd0b7 268 {
sk84life85 0:a394a04bd0b7 269 dataReceived[j] = params->data[j];
sk84life85 0:a394a04bd0b7 270 j++;
sk84life85 0:a394a04bd0b7 271 }
sk84life85 0:a394a04bd0b7 272 dataReceived[j] = params->data[j];
sk84life85 0:a394a04bd0b7 273 dataReceived[++j] = 0;
sk84life85 0:a394a04bd0b7 274 */
sk84life85 0:a394a04bd0b7 275
sk84life85 0:a394a04bd0b7 276 if (params->len == 1) {
sk84life85 0:a394a04bd0b7 277 switch (params->data[0]) {
sk84life85 0:a394a04bd0b7 278 #ifdef BH1745 //
sk84life85 0:a394a04bd0b7 279 case 'r':
sk84life85 0:a394a04bd0b7 280 len = snprintf((char*) buf, MAX_REPLY_LEN, "Red= %f", BH1745_Red);
sk84life85 0:a394a04bd0b7 281 break;
sk84life85 0:a394a04bd0b7 282 case 'g':
sk84life85 0:a394a04bd0b7 283 len = snprintf((char*) buf, MAX_REPLY_LEN, "Green= %f", BH1745_Green);
sk84life85 0:a394a04bd0b7 284 break;
sk84life85 0:a394a04bd0b7 285 case 'b':
sk84life85 0:a394a04bd0b7 286 len = snprintf((char*) buf, MAX_REPLY_LEN, "Blue= %f", BH1745_Blue);
sk84life85 0:a394a04bd0b7 287 break;
sk84life85 0:a394a04bd0b7 288 case 'c':
sk84life85 0:a394a04bd0b7 289 len = snprintf((char*) buf, MAX_REPLY_LEN, "Clear= %f", BH1745_Clear);
sk84life85 0:a394a04bd0b7 290 break;
sk84life85 0:a394a04bd0b7 291 case 'x':
sk84life85 0:a394a04bd0b7 292 len = snprintf((char*) buf, MAX_REPLY_LEN, "CIE#%6.5f,%6.5f", chromaX, chromaY);
sk84life85 0:a394a04bd0b7 293 break;
sk84life85 0:a394a04bd0b7 294 /*
sk84life85 0:a394a04bd0b7 295 case '?':
sk84life85 0:a394a04bd0b7 296 if( (0.03212-0.0005<chromaX) && (chromaX<0.03212+0.0005) && (0.02598-0.0005<chromaY) && (chromaY<0.02598+0.0005) ){
sk84life85 0:a394a04bd0b7 297 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 298 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 299 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "2-14");
sk84life85 0:a394a04bd0b7 300 }
sk84life85 0:a394a04bd0b7 301 else if( (0.07793-0.0005<chromaX) && (chromaX<0.07793+0.0005) && (0.07237-0.0005<chromaY) && (chromaY<0.07237+0.0005) ){
sk84life85 0:a394a04bd0b7 302 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 303 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 304 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-13");
sk84life85 0:a394a04bd0b7 305 }
sk84life85 0:a394a04bd0b7 306 else if( (0.07256-0.0005<chromaX) && (chromaX<0.07256+0.0005) && (0.06636-0.0005<chromaY) && (chromaY<0.06636+0.0005) ){
sk84life85 0:a394a04bd0b7 307 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 308 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 309 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-14");
sk84life85 0:a394a04bd0b7 310 }
sk84life85 0:a394a04bd0b7 311 else if( (0.07079-0.0005<chromaX) && (chromaX<0.07079+0.0005) && (0.06706-0.0005<chromaY) && (chromaY<0.06706+0.0005) ){
sk84life85 0:a394a04bd0b7 312 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 313 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 314 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-5");
sk84life85 0:a394a04bd0b7 315 }
sk84life85 0:a394a04bd0b7 316 else if( (0.04708-0.0005<chromaX) && (chromaX<0.04708+0.0005) && (0.04485-0.0005<chromaY) && (chromaY<0.04485+0.0005) ){
sk84life85 0:a394a04bd0b7 317 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 318 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 319 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-4");
sk84life85 0:a394a04bd0b7 320 }
sk84life85 0:a394a04bd0b7 321 else if( (0.03373-0.0005<chromaX) && (chromaX<0.03373+0.0005) && (0.03221-0.0005<chromaY) && (chromaY<0.03221+0.0005) ){
sk84life85 0:a394a04bd0b7 322 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 323 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 324 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-3");
sk84life85 0:a394a04bd0b7 325 }
sk84life85 0:a394a04bd0b7 326 else if( (0.07630-0.0005<chromaX) && (chromaX<0.07630+0.0005) && (0.07077-0.0005<chromaY) && (chromaY<0.07077+0.0005) ){
sk84life85 0:a394a04bd0b7 327 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 328 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 329 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-8");
sk84life85 0:a394a04bd0b7 330 }
sk84life85 0:a394a04bd0b7 331 else if( (0.09365-0.0005<chromaX) && (chromaX<0.09365+0.0005) && (0.08628-0.0005<chromaY) && (chromaY<0.08628+0.0005) ){
sk84life85 0:a394a04bd0b7 332 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 333 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 334 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-10");
sk84life85 0:a394a04bd0b7 335 }
sk84life85 0:a394a04bd0b7 336 else if( (0.09483-0.0005<chromaX) && (chromaX<0.09483+0.0005) && (0.08657-0.0005<chromaY) && (chromaY<0.08657+0.0005) ){
sk84life85 0:a394a04bd0b7 337 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 338 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 339 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-11");
sk84life85 0:a394a04bd0b7 340 }
sk84life85 0:a394a04bd0b7 341 else if( (0.08935-0.0005<chromaX) && (chromaX<0.08935+0.0005) && (0.08311-0.0005<chromaY) && (chromaY<0.08311+0.0005) ){
sk84life85 0:a394a04bd0b7 342 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 343 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 344 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-12");
sk84life85 0:a394a04bd0b7 345 }
sk84life85 0:a394a04bd0b7 346 else if( (0.08176-0.0005<chromaX) && (chromaX<0.08176+0.0005) && (0.07384-0.0005<chromaY) && (chromaY<0.07384+0.0005) ){
sk84life85 0:a394a04bd0b7 347 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 348 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 349 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-15");
sk84life85 0:a394a04bd0b7 350 }
sk84life85 0:a394a04bd0b7 351 else if( (0.04112-0.0005<chromaX) && (chromaX<0.04112+0.0005) && (0.03841-0.0005<chromaY) && (chromaY<0.03841+0.0005) ){
sk84life85 0:a394a04bd0b7 352 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 353 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 354 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-6");
sk84life85 0:a394a04bd0b7 355 }
sk84life85 0:a394a04bd0b7 356 else if( (0.03577-0.0005<chromaX) && (chromaX<0.03577+0.0005) && (0.03325-0.0005<chromaY) && (chromaY<0.03325+0.0005) ){
sk84life85 0:a394a04bd0b7 357 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 358 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 359 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-5");
sk84life85 0:a394a04bd0b7 360 }
sk84life85 0:a394a04bd0b7 361 else if( (0.03432-0.0005<chromaX) && (chromaX<0.03432+0.0005) && (0.03203-0.0005<chromaY) && (chromaY<0.03203+0.0005) ){
sk84life85 0:a394a04bd0b7 362 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 363 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 364 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "17-18");
sk84life85 0:a394a04bd0b7 365 }
sk84life85 0:a394a04bd0b7 366 else{
sk84life85 0:a394a04bd0b7 367 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "Unregistered");
sk84life85 0:a394a04bd0b7 368 }
sk84life85 0:a394a04bd0b7 369 break;
sk84life85 0:a394a04bd0b7 370 */
sk84life85 0:a394a04bd0b7 371
sk84life85 0:a394a04bd0b7 372 case 'l':
sk84life85 0:a394a04bd0b7 373 if(LED_Enable==0){
sk84life85 0:a394a04bd0b7 374 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 375 wait_ms(100);
sk84life85 0:a394a04bd0b7 376 len = snprintf((char*) buf, MAX_REPLY_LEN, "LED ON");
sk84life85 0:a394a04bd0b7 377 }
sk84life85 0:a394a04bd0b7 378 else{
sk84life85 0:a394a04bd0b7 379 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 380 wait_ms(100);
sk84life85 0:a394a04bd0b7 381 len = snprintf((char*) buf, MAX_REPLY_LEN, "LED OFF");
sk84life85 0:a394a04bd0b7 382 }
sk84life85 0:a394a04bd0b7 383
sk84life85 0:a394a04bd0b7 384 break;
sk84life85 0:a394a04bd0b7 385 #endif
sk84life85 0:a394a04bd0b7 386
sk84life85 0:a394a04bd0b7 387 default:
sk84life85 0:a394a04bd0b7 388 len = snprintf((char*) buf, MAX_REPLY_LEN, "ERROR");
sk84life85 0:a394a04bd0b7 389 break;
sk84life85 0:a394a04bd0b7 390 }
sk84life85 0:a394a04bd0b7 391 }/*
sk84life85 0:a394a04bd0b7 392 else if (params->len > 1) {
sk84life85 0:a394a04bd0b7 393 switch (params->data[0]) {
sk84life85 0:a394a04bd0b7 394 case 's':
sk84life85 0:a394a04bd0b7 395
sk84life85 0:a394a04bd0b7 396 rValue = BH1745_Red;
sk84life85 0:a394a04bd0b7 397 gValue = BH1745_Green;
sk84life85 0:a394a04bd0b7 398 bValue = BH1745_Blue;
sk84life85 0:a394a04bd0b7 399 cValue = BH1745_Clear;
sk84life85 0:a394a04bd0b7 400
sk84life85 0:a394a04bd0b7 401 break;
sk84life85 0:a394a04bd0b7 402 }
sk84life85 0:a394a04bd0b7 403 } */
sk84life85 0:a394a04bd0b7 404 else
sk84life85 0:a394a04bd0b7 405 {
sk84life85 0:a394a04bd0b7 406 len = snprintf((char*) buf, MAX_REPLY_LEN, "ERROR");
sk84life85 0:a394a04bd0b7 407 }
sk84life85 0:a394a04bd0b7 408
sk84life85 0:a394a04bd0b7 409 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 410 DEBUG("%d bytes received from host\n\r", params->len);
sk84life85 0:a394a04bd0b7 411 }
sk84life85 0:a394a04bd0b7 412 }
sk84life85 0:a394a04bd0b7 413
sk84life85 0:a394a04bd0b7 414
sk84life85 0:a394a04bd0b7 415 /**
sk84life85 0:a394a04bd0b7 416 * This callback is used whenever a write to a GATT characteristic causes data to be sent to the host.
sk84life85 0:a394a04bd0b7 417 */
sk84life85 0:a394a04bd0b7 418 void dataSentCallback(unsigned count)
sk84life85 0:a394a04bd0b7 419 {
sk84life85 0:a394a04bd0b7 420 // NOTE: The count always seems to be 1 regardless of data.
sk84life85 0:a394a04bd0b7 421 DEBUG("%d bytes sent to host\n\r", count);
sk84life85 0:a394a04bd0b7 422 }
sk84life85 0:a394a04bd0b7 423
sk84life85 0:a394a04bd0b7 424
sk84life85 0:a394a04bd0b7 425 /**
sk84life85 0:a394a04bd0b7 426 * This callback is scheduled to be called periodically via a low-priority interrupt.
sk84life85 0:a394a04bd0b7 427 */
sk84life85 0:a394a04bd0b7 428 void periodicCallback(void)
sk84life85 0:a394a04bd0b7 429 {
sk84life85 0:a394a04bd0b7 430
sk84life85 0:a394a04bd0b7 431 // uint8_t buf[MAX_REPLY_LEN];
sk84life85 0:a394a04bd0b7 432 // uint32_t len = 0;
sk84life85 0:a394a04bd0b7 433
sk84life85 0:a394a04bd0b7 434
sk84life85 0:a394a04bd0b7 435
sk84life85 0:a394a04bd0b7 436 if (m_ble.getGapState().connected) {
sk84life85 0:a394a04bd0b7 437 if(isConnected==0){
sk84life85 0:a394a04bd0b7 438 }
sk84life85 0:a394a04bd0b7 439 isConnected = 1;
sk84life85 0:a394a04bd0b7 440 }
sk84life85 0:a394a04bd0b7 441 else{
sk84life85 0:a394a04bd0b7 442 isConnected = 0;
sk84life85 0:a394a04bd0b7 443 }
sk84life85 0:a394a04bd0b7 444
sk84life85 0:a394a04bd0b7 445 }
sk84life85 0:a394a04bd0b7 446
sk84life85 0:a394a04bd0b7 447
sk84life85 0:a394a04bd0b7 448 void error(ble_error_t err, uint32_t line)
sk84life85 0:a394a04bd0b7 449 {
sk84life85 0:a394a04bd0b7 450 m_error_led = 1;
sk84life85 0:a394a04bd0b7 451 DEBUG("Error %d on line number %d\n\r", err, line);
sk84life85 0:a394a04bd0b7 452 }
sk84life85 0:a394a04bd0b7 453
sk84life85 0:a394a04bd0b7 454 //Code for writing back to Console if a PB is pressed
sk84life85 0:a394a04bd0b7 455 void PBTrigger()
sk84life85 0:a394a04bd0b7 456 {
sk84life85 0:a394a04bd0b7 457 //uint8_t buf[MAX_REPLY_LEN];
sk84life85 0:a394a04bd0b7 458 //uint32_t len = 0;
sk84life85 0:a394a04bd0b7 459
sk84life85 0:a394a04bd0b7 460 m_cmd_led = !m_cmd_led;
sk84life85 0:a394a04bd0b7 461
sk84life85 0:a394a04bd0b7 462 if(!setCruise){
sk84life85 0:a394a04bd0b7 463 cruiseFlag = 1;
sk84life85 0:a394a04bd0b7 464 cruiseSet = Speed_value*2.75;
sk84life85 0:a394a04bd0b7 465 }
sk84life85 0:a394a04bd0b7 466 if(!resetCruise){
sk84life85 0:a394a04bd0b7 467 cruiseFlag = 0;
sk84life85 0:a394a04bd0b7 468 }
sk84life85 0:a394a04bd0b7 469 m_cmd_led = cruiseFlag;
sk84life85 0:a394a04bd0b7 470
sk84life85 0:a394a04bd0b7 471
sk84life85 0:a394a04bd0b7 472 /*
sk84life85 0:a394a04bd0b7 473 if (m_ble.getGapState().connected) {
sk84life85 0:a394a04bd0b7 474 }
sk84life85 0:a394a04bd0b7 475 */
sk84life85 0:a394a04bd0b7 476 }
sk84life85 0:a394a04bd0b7 477
sk84life85 0:a394a04bd0b7 478 int main(void)
sk84life85 0:a394a04bd0b7 479 {
sk84life85 0:a394a04bd0b7 480 ble_error_t err;
sk84life85 0:a394a04bd0b7 481 Ticker ticker;
sk84life85 0:a394a04bd0b7 482
sk84life85 0:a394a04bd0b7 483 m_serial_port.baud(UART_BAUD_RATE);
sk84life85 0:a394a04bd0b7 484
sk84life85 0:a394a04bd0b7 485 DEBUG("Initialising...\n\r");
sk84life85 0:a394a04bd0b7 486
sk84life85 0:a394a04bd0b7 487 m_cmd_led = 0;
sk84life85 0:a394a04bd0b7 488 m_error_led = 0;
sk84life85 0:a394a04bd0b7 489
sk84life85 0:a394a04bd0b7 490 //ticker.attach(periodicCallback, SENSOR_READ_INTERVAL_S);
sk84life85 0:a394a04bd0b7 491
sk84life85 0:a394a04bd0b7 492 //sw4Press.fall(&PBTrigger);
sk84life85 0:a394a04bd0b7 493
sk84life85 0:a394a04bd0b7 494 setCruise.fall(&PBTrigger);
sk84life85 0:a394a04bd0b7 495 resetCruise.fall(&PBTrigger);
sk84life85 0:a394a04bd0b7 496
sk84life85 0:a394a04bd0b7 497 //Start BTLE Initialization Section
sk84life85 0:a394a04bd0b7 498 m_ble.init();
sk84life85 0:a394a04bd0b7 499 m_ble.onDisconnection(disconnectionCallback);
sk84life85 0:a394a04bd0b7 500 m_ble.onDataWritten(dataWrittenCallback);
sk84life85 0:a394a04bd0b7 501 m_ble.onDataSent(dataSentCallback);
sk84life85 0:a394a04bd0b7 502
sk84life85 0:a394a04bd0b7 503 // Set the TX power in dBm units.
sk84life85 0:a394a04bd0b7 504 // Possible values (in decreasing order): 4, 0, -4, -8, -12, -16, -20.
sk84life85 0:a394a04bd0b7 505 err = m_ble.setTxPower(4);
sk84life85 0:a394a04bd0b7 506 if (BLE_ERROR_NONE != err) {
sk84life85 0:a394a04bd0b7 507 error(err, __LINE__);
sk84life85 0:a394a04bd0b7 508 }
sk84life85 0:a394a04bd0b7 509
sk84life85 0:a394a04bd0b7 510 // Setup advertising (GAP stuff).
sk84life85 0:a394a04bd0b7 511 err = m_ble.setDeviceName(DEVICE_NAME);
sk84life85 0:a394a04bd0b7 512 if (BLE_ERROR_NONE != err) {
sk84life85 0:a394a04bd0b7 513 error(err, __LINE__);
sk84life85 0:a394a04bd0b7 514 }
sk84life85 0:a394a04bd0b7 515
sk84life85 0:a394a04bd0b7 516 err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
sk84life85 0:a394a04bd0b7 517 if (BLE_ERROR_NONE != err) {
sk84life85 0:a394a04bd0b7 518 error(err, __LINE__);
sk84life85 0:a394a04bd0b7 519 }
sk84life85 0:a394a04bd0b7 520
sk84life85 0:a394a04bd0b7 521 m_ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
sk84life85 0:a394a04bd0b7 522
sk84life85 0:a394a04bd0b7 523 err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
sk84life85 0:a394a04bd0b7 524 (const uint8_t *)SHORT_NAME,
sk84life85 0:a394a04bd0b7 525 (sizeof(SHORT_NAME) - 1));
sk84life85 0:a394a04bd0b7 526 if (BLE_ERROR_NONE != err) {
sk84life85 0:a394a04bd0b7 527 error(err, __LINE__);
sk84life85 0:a394a04bd0b7 528 }
sk84life85 0:a394a04bd0b7 529
sk84life85 0:a394a04bd0b7 530 err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
sk84life85 0:a394a04bd0b7 531 (const uint8_t *)UARTServiceUUID_reversed,
sk84life85 0:a394a04bd0b7 532 sizeof(UARTServiceUUID_reversed));
sk84life85 0:a394a04bd0b7 533 if (BLE_ERROR_NONE != err) {
sk84life85 0:a394a04bd0b7 534 error(err, __LINE__);
sk84life85 0:a394a04bd0b7 535 }
sk84life85 0:a394a04bd0b7 536
sk84life85 0:a394a04bd0b7 537 m_ble.setAdvertisingInterval(Gap::MSEC_TO_ADVERTISEMENT_DURATION_UNITS(ADV_INTERVAL_MS));
sk84life85 0:a394a04bd0b7 538 m_ble.startAdvertising();
sk84life85 0:a394a04bd0b7 539
sk84life85 0:a394a04bd0b7 540 // Create a UARTService object (GATT stuff).
sk84life85 0:a394a04bd0b7 541 UARTService uartService(m_ble);
sk84life85 0:a394a04bd0b7 542 m_uart_service_ptr = &uartService;
sk84life85 0:a394a04bd0b7 543
sk84life85 0:a394a04bd0b7 544 #ifdef BH1745
sk84life85 0:a394a04bd0b7 545 i2c.write(BH1745_addr_w, &BH1745_CTRL2[0], 2, false);
sk84life85 0:a394a04bd0b7 546 #endif
sk84life85 0:a394a04bd0b7 547
sk84life85 0:a394a04bd0b7 548 #ifdef BatteryI2C
sk84life85 0:a394a04bd0b7 549 i2c.write(BMS_addr_w, &BMS_DeviceName, 1, RepStart);
sk84life85 0:a394a04bd0b7 550 i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 8, NoRepStart);
sk84life85 0:a394a04bd0b7 551
sk84life85 0:a394a04bd0b7 552 //BMS_Password = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]);
sk84life85 0:a394a04bd0b7 553 BMS_Password0 = (BMS_Content_ReadData[0]);
sk84life85 0:a394a04bd0b7 554 BMS_Password1 = (BMS_Content_ReadData[1]);
sk84life85 0:a394a04bd0b7 555 BMS_Password2 = (BMS_Content_ReadData[2]);
sk84life85 0:a394a04bd0b7 556 BMS_Password3 = (BMS_Content_ReadData[3]);
sk84life85 0:a394a04bd0b7 557 BMS_Password4 = (BMS_Content_ReadData[4]);
sk84life85 0:a394a04bd0b7 558 BMS_Password5 = (BMS_Content_ReadData[5]);
sk84life85 0:a394a04bd0b7 559 BMS_Password6 = (BMS_Content_ReadData[6]);
sk84life85 0:a394a04bd0b7 560 BMS_Password7 = (BMS_Content_ReadData[7]);
sk84life85 0:a394a04bd0b7 561 BMS_Password8 = (BMS_Content_ReadData[8]);
sk84life85 0:a394a04bd0b7 562 i2c.write(BMS_addr_w, &BMS_Cycles, 1, RepStart);
sk84life85 0:a394a04bd0b7 563 i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 2, NoRepStart);
sk84life85 0:a394a04bd0b7 564 BMS_CycleOut = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]);
sk84life85 0:a394a04bd0b7 565
sk84life85 0:a394a04bd0b7 566 i2c.write(BMS_addr_w, &BMS_SOC, 1, RepStart);
sk84life85 0:a394a04bd0b7 567 i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 2, NoRepStart);
sk84life85 0:a394a04bd0b7 568 BMS_SOCOut = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]);
sk84life85 0:a394a04bd0b7 569 #endif
sk84life85 0:a394a04bd0b7 570
sk84life85 0:a394a04bd0b7 571 //Initialize PWM Output
sk84life85 0:a394a04bd0b7 572 PWM.period_us(1000);
sk84life85 0:a394a04bd0b7 573 hornFreq.period_us(1250);
sk84life85 0:a394a04bd0b7 574 //m_cmd_led.period_ms(10);
sk84life85 0:a394a04bd0b7 575 ADC_value_old = 0;
sk84life85 0:a394a04bd0b7 576
sk84life85 0:a394a04bd0b7 577 p = 0;
sk84life85 0:a394a04bd0b7 578 SpeedSet = 1;
sk84life85 0:a394a04bd0b7 579 tick = deltaT;
sk84life85 0:a394a04bd0b7 580 SpeedSampNum = 5;
sk84life85 0:a394a04bd0b7 581
sk84life85 0:a394a04bd0b7 582 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 583 wait_ms(100);
sk84life85 0:a394a04bd0b7 584 wait_ms(100);
sk84life85 0:a394a04bd0b7 585 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 586 wait_ms(100);
sk84life85 0:a394a04bd0b7 587 wait_ms(100);
sk84life85 0:a394a04bd0b7 588 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 589 wait_ms(100);
sk84life85 0:a394a04bd0b7 590 wait_ms(100);
sk84life85 0:a394a04bd0b7 591 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 592 wait_ms(100);
sk84life85 0:a394a04bd0b7 593 wait_ms(100);
sk84life85 0:a394a04bd0b7 594 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 595 wait_ms(100);
sk84life85 0:a394a04bd0b7 596 wait_ms(100);
sk84life85 0:a394a04bd0b7 597 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 598 wait_ms(100);
sk84life85 0:a394a04bd0b7 599 wait_ms(100);
sk84life85 0:a394a04bd0b7 600
sk84life85 0:a394a04bd0b7 601 while (true) {
sk84life85 0:a394a04bd0b7 602 //m_ble.waitForEvent();
sk84life85 0:a394a04bd0b7 603 if(m_ble.getGapState().connected){
sk84life85 0:a394a04bd0b7 604 if(!isConnected){
sk84life85 0:a394a04bd0b7 605 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 606 wait_ms(100);
sk84life85 0:a394a04bd0b7 607 wait_ms(100);
sk84life85 0:a394a04bd0b7 608 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 609 wait_ms(100);
sk84life85 0:a394a04bd0b7 610 wait_ms(100);
sk84life85 0:a394a04bd0b7 611 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 612 wait_ms(100);
sk84life85 0:a394a04bd0b7 613 wait_ms(100);
sk84life85 0:a394a04bd0b7 614 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 615 wait_ms(100);
sk84life85 0:a394a04bd0b7 616 wait_ms(100);
sk84life85 0:a394a04bd0b7 617 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 618 wait_ms(100);
sk84life85 0:a394a04bd0b7 619 wait_ms(100);
sk84life85 0:a394a04bd0b7 620 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 621 wait_ms(100);
sk84life85 0:a394a04bd0b7 622 wait_ms(100);
sk84life85 0:a394a04bd0b7 623 }
sk84life85 0:a394a04bd0b7 624 isConnected = 1;
sk84life85 0:a394a04bd0b7 625
sk84life85 0:a394a04bd0b7 626 //Read KX022 Portion from the IC
sk84life85 0:a394a04bd0b7 627 i2c.write(BH1745_addr_w, &BH1745_Red_Data_LSB, 1, RepStart);
sk84life85 0:a394a04bd0b7 628 i2c.read(BH1745_addr_r, &BH1745_Content_ReadData[0], 8, NoRepStart);
sk84life85 0:a394a04bd0b7 629
sk84life85 0:a394a04bd0b7 630 //reconfigure the data (taken from arduino code)
sk84life85 0:a394a04bd0b7 631 BH1745_Raw_R = (BH1745_Content_ReadData[1]<<8) | (BH1745_Content_ReadData[0]);
sk84life85 0:a394a04bd0b7 632 BH1745_Raw_G = (BH1745_Content_ReadData[3]<<8) | (BH1745_Content_ReadData[2]);
sk84life85 0:a394a04bd0b7 633 BH1745_Raw_B = (BH1745_Content_ReadData[5]<<8) | (BH1745_Content_ReadData[4]);
sk84life85 0:a394a04bd0b7 634 BH1745_Raw_C = (BH1745_Content_ReadData[7]<<8) | (BH1745_Content_ReadData[6]);
sk84life85 0:a394a04bd0b7 635
sk84life85 0:a394a04bd0b7 636 BH1745_Red = (float)BH1745_Raw_R / 16384;
sk84life85 0:a394a04bd0b7 637 BH1745_Green = (float)BH1745_Raw_G / 16384;
sk84life85 0:a394a04bd0b7 638 BH1745_Blue = (float)BH1745_Raw_B / 16384;
sk84life85 0:a394a04bd0b7 639 BH1745_Clear = (float)BH1745_Raw_C / 16384;
sk84life85 0:a394a04bd0b7 640
sk84life85 0:a394a04bd0b7 641 if(BH1745_Clear < 0.00001){
sk84life85 0:a394a04bd0b7 642
sk84life85 0:a394a04bd0b7 643 wait_ms(100);
sk84life85 0:a394a04bd0b7 644 wait_ms(100);
sk84life85 0:a394a04bd0b7 645 wait_ms(100);
sk84life85 0:a394a04bd0b7 646 wait_ms(100);
sk84life85 0:a394a04bd0b7 647 wait_ms(100);
sk84life85 0:a394a04bd0b7 648 wait_ms(100);
sk84life85 0:a394a04bd0b7 649 wait_ms(100);
sk84life85 0:a394a04bd0b7 650 wait_ms(100);
sk84life85 0:a394a04bd0b7 651 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 652 wait_ms(100);
sk84life85 0:a394a04bd0b7 653 wait_ms(100);
sk84life85 0:a394a04bd0b7 654
sk84life85 0:a394a04bd0b7 655 for(i=0; i<60; i++){
sk84life85 0:a394a04bd0b7 656
sk84life85 0:a394a04bd0b7 657 //---------------------------- KX022 ----------------------------
sk84life85 0:a394a04bd0b7 658 //Read KX022 Portion from the IC
sk84life85 0:a394a04bd0b7 659 i2c.write(BH1745_addr_w, &BH1745_Red_Data_LSB, 1, RepStart);
sk84life85 0:a394a04bd0b7 660 i2c.read(BH1745_addr_r, &BH1745_Content_ReadData[0], 8, NoRepStart);
sk84life85 0:a394a04bd0b7 661
sk84life85 0:a394a04bd0b7 662 //reconfigure the data (taken from arduino code)
sk84life85 0:a394a04bd0b7 663 BH1745_Raw_R = (BH1745_Content_ReadData[1]<<8) | (BH1745_Content_ReadData[0]);
sk84life85 0:a394a04bd0b7 664 BH1745_Raw_G = (BH1745_Content_ReadData[3]<<8) | (BH1745_Content_ReadData[2]);
sk84life85 0:a394a04bd0b7 665 BH1745_Raw_B = (BH1745_Content_ReadData[5]<<8) | (BH1745_Content_ReadData[4]);
sk84life85 0:a394a04bd0b7 666 BH1745_Raw_C = (BH1745_Content_ReadData[7]<<8) | (BH1745_Content_ReadData[6]);
sk84life85 0:a394a04bd0b7 667
sk84life85 0:a394a04bd0b7 668 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 669
sk84life85 0:a394a04bd0b7 670 BH1745_Red = (float)BH1745_Raw_R / 16384;
sk84life85 0:a394a04bd0b7 671 BH1745_Green = (float)BH1745_Raw_G / 16384;
sk84life85 0:a394a04bd0b7 672 BH1745_Blue = (float)BH1745_Raw_B / 16384;
sk84life85 0:a394a04bd0b7 673 BH1745_Clear = (float)BH1745_Raw_C / 16384;
sk84life85 0:a394a04bd0b7 674
sk84life85 0:a394a04bd0b7 675 CIEx[i] = 0.232*BH1745_Red + 0.610*BH1745_Green + 0.055*BH1745_Blue;
sk84life85 0:a394a04bd0b7 676 if( BH1745_Clear < (0.2*BH1745_Green) ){
sk84life85 0:a394a04bd0b7 677 if( BH1745_Blue < (0.45*BH1745_Green) ){
sk84life85 0:a394a04bd0b7 678 CIEy[i] = 0.083*BH1745_Red + 0.860*BH1745_Green;
sk84life85 0:a394a04bd0b7 679 }
sk84life85 0:a394a04bd0b7 680 else{
sk84life85 0:a394a04bd0b7 681 CIEy[i] = 0.070*BH1745_Red + 0.709*BH1745_Green;
sk84life85 0:a394a04bd0b7 682 }
sk84life85 0:a394a04bd0b7 683 }
sk84life85 0:a394a04bd0b7 684 else if( BH1745_Clear < (0.50*BH1745_Green) ){
sk84life85 0:a394a04bd0b7 685 CIEy[i] = 0.110*BH1745_Red + 0.712*BH1745_Green;
sk84life85 0:a394a04bd0b7 686 }
sk84life85 0:a394a04bd0b7 687 else{
sk84life85 0:a394a04bd0b7 688 CIEy[i] = 0.073*BH1745_Red + 0.864*BH1745_Green;
sk84life85 0:a394a04bd0b7 689 }
sk84life85 0:a394a04bd0b7 690 for(j=i; j>0; j--){
sk84life85 0:a394a04bd0b7 691 tempCIE = CIEx[j];
sk84life85 0:a394a04bd0b7 692 if(tempCIE<CIEx[j-1]){
sk84life85 0:a394a04bd0b7 693 CIEx[j] = CIEx[j-1];
sk84life85 0:a394a04bd0b7 694 CIEx[j-1] = tempCIE;
sk84life85 0:a394a04bd0b7 695 }
sk84life85 0:a394a04bd0b7 696 }
sk84life85 0:a394a04bd0b7 697 for(j=i; j>0; j--){
sk84life85 0:a394a04bd0b7 698 tempCIE = CIEy[j];
sk84life85 0:a394a04bd0b7 699 if(tempCIE<CIEy[j-1]){
sk84life85 0:a394a04bd0b7 700 CIEy[j] = CIEy[j-1];
sk84life85 0:a394a04bd0b7 701 CIEy[j-1] = tempCIE;
sk84life85 0:a394a04bd0b7 702 }
sk84life85 0:a394a04bd0b7 703 }
sk84life85 0:a394a04bd0b7 704 }
sk84life85 0:a394a04bd0b7 705
sk84life85 0:a394a04bd0b7 706
sk84life85 0:a394a04bd0b7 707 chromaX = (CIEx[28]+CIEx[29]+CIEx[30]+CIEx[31]+CIEx[32])/5.0;
sk84life85 0:a394a04bd0b7 708 chromaY = (CIEy[28]+CIEy[29]+CIEy[30]+CIEy[31]+CIEy[32])/5.0;
sk84life85 0:a394a04bd0b7 709
sk84life85 0:a394a04bd0b7 710 if( (xPPU2_14-xThresh<chromaX) && (chromaX<xPPU2_14+xThresh) && (yPPU2_14-yThresh<chromaY) && (chromaY<yPPU2_14+yThresh) ){
sk84life85 0:a394a04bd0b7 711 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "2-14");
sk84life85 0:a394a04bd0b7 712 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 713 }
sk84life85 0:a394a04bd0b7 714 else if( (xPPU18_5-xThresh<chromaX) && (chromaX<xPPU18_5+xThresh) && (yPPU18_5-yThresh<chromaY) && (chromaY<yPPU18_5+yThresh) ){
sk84life85 0:a394a04bd0b7 715 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-5");
sk84life85 0:a394a04bd0b7 716 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 717 }
sk84life85 0:a394a04bd0b7 718 else if( (xPPU18_4-xThresh<chromaX) && (chromaX<xPPU18_4+xThresh) && (yPPU18_4-yThresh<chromaY) && (chromaY<yPPU18_4+yThresh) ){
sk84life85 0:a394a04bd0b7 719 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-4");
sk84life85 0:a394a04bd0b7 720 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 721 }
sk84life85 0:a394a04bd0b7 722 else if( (xPPU18_3-xThresh<chromaX) && (chromaX<xPPU18_3+xThresh) && (yPPU18_3-yThresh<chromaY) && (chromaY<yPPU18_3+yThresh) ){
sk84life85 0:a394a04bd0b7 723 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-3");
sk84life85 0:a394a04bd0b7 724 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 725 }
sk84life85 0:a394a04bd0b7 726 else if( (xPPU17_18-xThresh<chromaX) && (chromaX<xPPU17_18+xThresh) && (yPPU17_18-yThresh<chromaY) && (chromaY<yPPU17_18+yThresh) ){
sk84life85 0:a394a04bd0b7 727 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "17-18");
sk84life85 0:a394a04bd0b7 728 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 729 }
sk84life85 0:a394a04bd0b7 730 else if( (xPPU5_6-xThresh<chromaX) && (chromaX<xPPU5_6+xThresh) && (yPPU5_6-yThresh<chromaY) && (chromaY<yPPU5_6+yThresh) ){
sk84life85 0:a394a04bd0b7 731 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-6");
sk84life85 0:a394a04bd0b7 732 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 733 }
sk84life85 0:a394a04bd0b7 734 else if( (xPPU4_15-xThresh<chromaX) && (chromaX<xPPU4_15+xThresh) && (yPPU4_15-yThresh<chromaY) && (chromaY<yPPU4_15+yThresh) ){
sk84life85 0:a394a04bd0b7 735 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-15");
sk84life85 0:a394a04bd0b7 736 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 737 }
sk84life85 0:a394a04bd0b7 738 else if( (xPPU4_14-xThresh<chromaX) && (chromaX<xPPU4_14+xThresh) && (yPPU4_14-yThresh<chromaY) && (chromaY<yPPU4_14+yThresh) ){
sk84life85 0:a394a04bd0b7 739 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-14");
sk84life85 0:a394a04bd0b7 740 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 741 }
sk84life85 0:a394a04bd0b7 742 else if( (xPPU4_13-xThresh<chromaX) && (chromaX<xPPU4_13+xThresh) && (yPPU4_13-yThresh<chromaY) && (chromaY<yPPU4_13+yThresh) ){
sk84life85 0:a394a04bd0b7 743 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-13");
sk84life85 0:a394a04bd0b7 744 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 745 }
sk84life85 0:a394a04bd0b7 746 else if( (xPPU4_10-xThresh<chromaX) && (chromaX<xPPU4_10+xThresh) && (yPPU4_10-yThresh<chromaY) && (chromaY<yPPU4_10+yThresh) ){
sk84life85 0:a394a04bd0b7 747 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-10");
sk84life85 0:a394a04bd0b7 748 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 749 }
sk84life85 0:a394a04bd0b7 750 else if( (xPPU4_8-xThresh<chromaX) && (chromaX<xPPU4_8+xThresh) && (yPPU4_8-yThresh<chromaY) && (chromaY<yPPU4_8+yThresh) ){
sk84life85 0:a394a04bd0b7 751 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-8");
sk84life85 0:a394a04bd0b7 752 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 753 }
sk84life85 0:a394a04bd0b7 754 else{
sk84life85 0:a394a04bd0b7 755 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "Unregistered");
sk84life85 0:a394a04bd0b7 756 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 757 }
sk84life85 0:a394a04bd0b7 758 }
sk84life85 0:a394a04bd0b7 759
sk84life85 0:a394a04bd0b7 760 }
sk84life85 0:a394a04bd0b7 761 else{
sk84life85 0:a394a04bd0b7 762 isConnected = 0;
sk84life85 0:a394a04bd0b7 763 }
sk84life85 0:a394a04bd0b7 764 }
sk84life85 0:a394a04bd0b7 765 }