Francis Lee / Mbed 2 deprecated IRIS

test

Dependencies:   BLE_API mbed nRF51822

main.cpp@1:88e3620627f9, 2016-02-26 (annotated)

Committer:
sk84life85
Date:
Fri Feb 26 23:27:37 2016 +0000
Revision:
1:88e3620627f9
Parent:
0:a394a04bd0b7 
test

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 1:88e3620627f9 126 double xPPU9_5 = 0.05660;
sk84life85 1:88e3620627f9 127 double yPPU9_5 = 0.06549;
sk84life85 1:88e3620627f9 128
sk84life85 1:88e3620627f9 129
sk84life85 1:88e3620627f9 130 double xPPU9_4 = 0.05072;
sk84life85 1:88e3620627f9 131 double yPPU9_4 = 0.04818;
sk84life85 1:88e3620627f9 132
sk84life85 0:a394a04bd0b7 133 double xPPU2_14 = 0.03221;
sk84life85 0:a394a04bd0b7 134 double yPPU2_14 = 0.02597;
sk84life85 0:a394a04bd0b7 135
sk84life85 0:a394a04bd0b7 136 double xPPU18_5 = 0.07085;
sk84life85 0:a394a04bd0b7 137 double yPPU18_5 = 0.06684;
sk84life85 0:a394a04bd0b7 138
sk84life85 0:a394a04bd0b7 139 double xPPU18_4 = 0.04707;
sk84life85 0:a394a04bd0b7 140 double yPPU18_4 = 0.04474;
sk84life85 0:a394a04bd0b7 141
sk84life85 0:a394a04bd0b7 142 double xPPU18_3 = 0.03384;
sk84life85 0:a394a04bd0b7 143 double yPPU18_3 = 0.03223;
sk84life85 0:a394a04bd0b7 144
sk84life85 0:a394a04bd0b7 145 double xPPU17_18 = 0.03440;
sk84life85 0:a394a04bd0b7 146 double yPPU17_18 = 0.03201;
sk84life85 0:a394a04bd0b7 147
sk84life85 0:a394a04bd0b7 148 double xPPU5_6 = 0.04115;
sk84life85 0:a394a04bd0b7 149 double yPPU5_6 = 0.03835;
sk84life85 0:a394a04bd0b7 150
sk84life85 0:a394a04bd0b7 151 double xPPU4_15 = 0.08183;
sk84life85 0:a394a04bd0b7 152 double yPPU4_15 = 0.07357;
sk84life85 0:a394a04bd0b7 153
sk84life85 0:a394a04bd0b7 154 double xPPU4_14 = 0.07272;
sk84life85 0:a394a04bd0b7 155 double yPPU4_14 = 0.06621;
sk84life85 0:a394a04bd0b7 156
sk84life85 0:a394a04bd0b7 157 double xPPU4_13 = 0.07797;
sk84life85 0:a394a04bd0b7 158 double yPPU4_13 = 0.07208;
sk84life85 0:a394a04bd0b7 159
sk84life85 0:a394a04bd0b7 160 double xPPU4_10 = 0.09362;
sk84life85 0:a394a04bd0b7 161 double yPPU4_10 = 0.08590;
sk84life85 0:a394a04bd0b7 162
sk84life85 0:a394a04bd0b7 163 double xPPU4_8 = 0.07635;
sk84life85 0:a394a04bd0b7 164 double yPPU4_8 = 0.07052;
sk84life85 0:a394a04bd0b7 165
sk84life85 0:a394a04bd0b7 166
sk84life85 0:a394a04bd0b7 167 double xThresh = 0.0004;
sk84life85 0:a394a04bd0b7 168 double yThresh = 0.0004;
sk84life85 0:a394a04bd0b7 169
sk84life85 0:a394a04bd0b7 170 string dataReceived = "aa";
sk84life85 0:a394a04bd0b7 171 uint8_t buf[19];
sk84life85 0:a394a04bd0b7 172 uint32_t len = 0;
sk84life85 0:a394a04bd0b7 173 string OutSetting = "";
sk84life85 0:a394a04bd0b7 174
sk84life85 0:a394a04bd0b7 175
sk84life85 0:a394a04bd0b7 176 #ifdef BH1745
sk84life85 0:a394a04bd0b7 177 int BH1745_addr_w = 0x72;
sk84life85 0:a394a04bd0b7 178 int BH1745_addr_r = 0x73;
sk84life85 0:a394a04bd0b7 179
sk84life85 0:a394a04bd0b7 180 char BH1745_CTRL2[2] = {0x42, 0x11};
sk84life85 0:a394a04bd0b7 181
sk84life85 0:a394a04bd0b7 182 char BH1745_Content_ReadData[8];
sk84life85 0:a394a04bd0b7 183
sk84life85 0:a394a04bd0b7 184 float BH1745_Red;
sk84life85 0:a394a04bd0b7 185 float BH1745_Green;
sk84life85 0:a394a04bd0b7 186 float BH1745_Blue;
sk84life85 0:a394a04bd0b7 187 float BH1745_Clear;
sk84life85 0:a394a04bd0b7 188
sk84life85 0:a394a04bd0b7 189 float SUM_BH1745_Red[10];
sk84life85 0:a394a04bd0b7 190 float SUM_BH1745_Green[10];
sk84life85 0:a394a04bd0b7 191 float SUM_BH1745_Blue[10];
sk84life85 0:a394a04bd0b7 192 float SUM_BH1745_Clear[10];
sk84life85 0:a394a04bd0b7 193
sk84life85 0:a394a04bd0b7 194 double colorSum;
sk84life85 0:a394a04bd0b7 195 int colorNum;
sk84life85 0:a394a04bd0b7 196
sk84life85 0:a394a04bd0b7 197 short int BH1745_Raw_R = 0;
sk84life85 0:a394a04bd0b7 198 short int BH1745_Raw_G = 0;
sk84life85 0:a394a04bd0b7 199 short int BH1745_Raw_B = 0;
sk84life85 0:a394a04bd0b7 200 short int BH1745_Raw_C = 0;
sk84life85 0:a394a04bd0b7 201
sk84life85 0:a394a04bd0b7 202 char BH1745_Red_Data_LSB = 0x50;
sk84life85 0:a394a04bd0b7 203
sk84life85 0:a394a04bd0b7 204
sk84life85 0:a394a04bd0b7 205 int sampleNum=0;
sk84life85 0:a394a04bd0b7 206 char message[20];
sk84life85 0:a394a04bd0b7 207 float rValue, gValue, bValue, cValue;
sk84life85 0:a394a04bd0b7 208
sk84life85 0:a394a04bd0b7 209 float colorError;
sk84life85 0:a394a04bd0b7 210
sk84life85 0:a394a04bd0b7 211 #endif
sk84life85 0:a394a04bd0b7 212
sk84life85 0:a394a04bd0b7 213 #ifdef BatteryI2C
sk84life85 0:a394a04bd0b7 214 int BMS_addr_w = 0x16; //0x2C;
sk84life85 0:a394a04bd0b7 215 int BMS_addr_r = 0x17; //0x2D;
sk84life85 0:a394a04bd0b7 216
sk84life85 0:a394a04bd0b7 217 char BMS_Content_ReadData[9];
sk84life85 0:a394a04bd0b7 218 char BMS_DeviceName = 0x21;
sk84life85 0:a394a04bd0b7 219 char BMS_Cycles = 0x17;
sk84life85 0:a394a04bd0b7 220 char BMS_SOC = 0x0D;
sk84life85 0:a394a04bd0b7 221
sk84life85 0:a394a04bd0b7 222 uint16_t BMS_Password0;
sk84life85 0:a394a04bd0b7 223 uint16_t BMS_Password1;
sk84life85 0:a394a04bd0b7 224 uint16_t BMS_Password2;
sk84life85 0:a394a04bd0b7 225 uint16_t BMS_Password3;
sk84life85 0:a394a04bd0b7 226 uint16_t BMS_Password4;
sk84life85 0:a394a04bd0b7 227 uint16_t BMS_Password5;
sk84life85 0:a394a04bd0b7 228 uint16_t BMS_Password6;
sk84life85 0:a394a04bd0b7 229 uint16_t BMS_Password7;
sk84life85 0:a394a04bd0b7 230 uint16_t BMS_Password8;
sk84life85 0:a394a04bd0b7 231 uint16_t BMS_CycleOut;
sk84life85 0:a394a04bd0b7 232 uint16_t BMS_SOCOut;
sk84life85 0:a394a04bd0b7 233
sk84life85 0:a394a04bd0b7 234 #endif
sk84life85 0:a394a04bd0b7 235
sk84life85 0:a394a04bd0b7 236 /**
sk84life85 0:a394a04bd0b7 237 * This callback is used whenever a disconnection occurs.
sk84life85 0:a394a04bd0b7 238 */
sk84life85 0:a394a04bd0b7 239 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
sk84life85 0:a394a04bd0b7 240 {
sk84life85 0:a394a04bd0b7 241 switch (reason) {
sk84life85 0:a394a04bd0b7 242 case Gap::REMOTE_USER_TERMINATED_CONNECTION:
sk84life85 0:a394a04bd0b7 243 DEBUG("Disconnected (REMOTE_USER_TERMINATED_CONNECTION)\n\r");
sk84life85 0:a394a04bd0b7 244 break;
sk84life85 0:a394a04bd0b7 245 case Gap::LOCAL_HOST_TERMINATED_CONNECTION:
sk84life85 0:a394a04bd0b7 246 DEBUG("Disconnected (LOCAL_HOST_TERMINATED_CONNECTION)\n\r");
sk84life85 0:a394a04bd0b7 247 break;
sk84life85 0:a394a04bd0b7 248 case Gap::CONN_INTERVAL_UNACCEPTABLE:
sk84life85 0:a394a04bd0b7 249 DEBUG("Disconnected (CONN_INTERVAL_UNACCEPTABLE)\n\r");
sk84life85 0:a394a04bd0b7 250 break;
sk84life85 0:a394a04bd0b7 251 }
sk84life85 0:a394a04bd0b7 252 isConnected = 0;
sk84life85 0:a394a04bd0b7 253
sk84life85 0:a394a04bd0b7 254 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 255 wait_ms(100);
sk84life85 0:a394a04bd0b7 256 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 257
sk84life85 0:a394a04bd0b7 258 DEBUG("Restarting the advertising process\n\r");
sk84life85 0:a394a04bd0b7 259 m_ble.startAdvertising();
sk84life85 0:a394a04bd0b7 260 }
sk84life85 0:a394a04bd0b7 261
sk84life85 0:a394a04bd0b7 262 /**
sk84life85 0:a394a04bd0b7 263 * This callback is used whenever the host writes data to one of our GATT characteristics.
sk84life85 0:a394a04bd0b7 264 */
sk84life85 0:a394a04bd0b7 265 void dataWrittenCallback(const GattCharacteristicWriteCBParams *params)
sk84life85 0:a394a04bd0b7 266 {
sk84life85 0:a394a04bd0b7 267 // Ensure that initialization is finished and the host has written to the TX characteristic.
sk84life85 0:a394a04bd0b7 268 if ((m_uart_service_ptr != NULL) && (params->charHandle == m_uart_service_ptr->getTXCharacteristicHandle())) {
sk84life85 0:a394a04bd0b7 269 len = 0;
sk84life85 0:a394a04bd0b7 270 // uint16_t TempInt = 1;
sk84life85 0:a394a04bd0b7 271
sk84life85 0:a394a04bd0b7 272 /*
sk84life85 0:a394a04bd0b7 273 j = 0;
sk84life85 0:a394a04bd0b7 274 for( int i = (params->len) - 1; i > 0; i-- )
sk84life85 0:a394a04bd0b7 275 {
sk84life85 0:a394a04bd0b7 276 dataReceived[j] = params->data[j];
sk84life85 0:a394a04bd0b7 277 j++;
sk84life85 0:a394a04bd0b7 278 }
sk84life85 0:a394a04bd0b7 279 dataReceived[j] = params->data[j];
sk84life85 0:a394a04bd0b7 280 dataReceived[++j] = 0;
sk84life85 0:a394a04bd0b7 281 */
sk84life85 0:a394a04bd0b7 282
sk84life85 0:a394a04bd0b7 283 if (params->len == 1) {
sk84life85 0:a394a04bd0b7 284 switch (params->data[0]) {
sk84life85 0:a394a04bd0b7 285 #ifdef BH1745 //
sk84life85 0:a394a04bd0b7 286 case 'r':
sk84life85 0:a394a04bd0b7 287 len = snprintf((char*) buf, MAX_REPLY_LEN, "Red= %f", BH1745_Red);
sk84life85 0:a394a04bd0b7 288 break;
sk84life85 0:a394a04bd0b7 289 case 'g':
sk84life85 0:a394a04bd0b7 290 len = snprintf((char*) buf, MAX_REPLY_LEN, "Green= %f", BH1745_Green);
sk84life85 0:a394a04bd0b7 291 break;
sk84life85 0:a394a04bd0b7 292 case 'b':
sk84life85 0:a394a04bd0b7 293 len = snprintf((char*) buf, MAX_REPLY_LEN, "Blue= %f", BH1745_Blue);
sk84life85 0:a394a04bd0b7 294 break;
sk84life85 0:a394a04bd0b7 295 case 'c':
sk84life85 0:a394a04bd0b7 296 len = snprintf((char*) buf, MAX_REPLY_LEN, "Clear= %f", BH1745_Clear);
sk84life85 0:a394a04bd0b7 297 break;
sk84life85 0:a394a04bd0b7 298 case 'x':
sk84life85 0:a394a04bd0b7 299 len = snprintf((char*) buf, MAX_REPLY_LEN, "CIE#%6.5f,%6.5f", chromaX, chromaY);
sk84life85 0:a394a04bd0b7 300 break;
sk84life85 0:a394a04bd0b7 301 /*
sk84life85 0:a394a04bd0b7 302 case '?':
sk84life85 0:a394a04bd0b7 303 if( (0.03212-0.0005<chromaX) && (chromaX<0.03212+0.0005) && (0.02598-0.0005<chromaY) && (chromaY<0.02598+0.0005) ){
sk84life85 0:a394a04bd0b7 304 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 305 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 306 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "2-14");
sk84life85 0:a394a04bd0b7 307 }
sk84life85 0:a394a04bd0b7 308 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 309 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 310 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 311 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-13");
sk84life85 0:a394a04bd0b7 312 }
sk84life85 0:a394a04bd0b7 313 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 314 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 315 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 316 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-14");
sk84life85 0:a394a04bd0b7 317 }
sk84life85 0:a394a04bd0b7 318 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 319 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 320 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 321 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-5");
sk84life85 0:a394a04bd0b7 322 }
sk84life85 0:a394a04bd0b7 323 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 324 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 325 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 326 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-4");
sk84life85 0:a394a04bd0b7 327 }
sk84life85 0:a394a04bd0b7 328 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 329 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 330 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 331 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-3");
sk84life85 0:a394a04bd0b7 332 }
sk84life85 0:a394a04bd0b7 333 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 334 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 335 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 336 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-8");
sk84life85 0:a394a04bd0b7 337 }
sk84life85 0:a394a04bd0b7 338 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 339 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 340 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 341 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-10");
sk84life85 0:a394a04bd0b7 342 }
sk84life85 0:a394a04bd0b7 343 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 344 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 345 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 346 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-11");
sk84life85 0:a394a04bd0b7 347 }
sk84life85 0:a394a04bd0b7 348 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 349 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 350 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 351 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-12");
sk84life85 0:a394a04bd0b7 352 }
sk84life85 0:a394a04bd0b7 353 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 354 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 355 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 356 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-15");
sk84life85 0:a394a04bd0b7 357 }
sk84life85 0:a394a04bd0b7 358 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 359 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 360 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 361 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-6");
sk84life85 0:a394a04bd0b7 362 }
sk84life85 0:a394a04bd0b7 363 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 364 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 365 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 366 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-5");
sk84life85 0:a394a04bd0b7 367 }
sk84life85 0:a394a04bd0b7 368 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 369 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "PPU");
sk84life85 0:a394a04bd0b7 370 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 371 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "17-18");
sk84life85 0:a394a04bd0b7 372 }
sk84life85 0:a394a04bd0b7 373 else{
sk84life85 0:a394a04bd0b7 374 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "Unregistered");
sk84life85 0:a394a04bd0b7 375 }
sk84life85 0:a394a04bd0b7 376 break;
sk84life85 0:a394a04bd0b7 377 */
sk84life85 0:a394a04bd0b7 378
sk84life85 0:a394a04bd0b7 379 case 'l':
sk84life85 0:a394a04bd0b7 380 if(LED_Enable==0){
sk84life85 0:a394a04bd0b7 381 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 382 wait_ms(100);
sk84life85 0:a394a04bd0b7 383 len = snprintf((char*) buf, MAX_REPLY_LEN, "LED ON");
sk84life85 0:a394a04bd0b7 384 }
sk84life85 0:a394a04bd0b7 385 else{
sk84life85 0:a394a04bd0b7 386 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 387 wait_ms(100);
sk84life85 0:a394a04bd0b7 388 len = snprintf((char*) buf, MAX_REPLY_LEN, "LED OFF");
sk84life85 0:a394a04bd0b7 389 }
sk84life85 0:a394a04bd0b7 390
sk84life85 0:a394a04bd0b7 391 break;
sk84life85 0:a394a04bd0b7 392 #endif
sk84life85 0:a394a04bd0b7 393
sk84life85 0:a394a04bd0b7 394 default:
sk84life85 0:a394a04bd0b7 395 len = snprintf((char*) buf, MAX_REPLY_LEN, "ERROR");
sk84life85 0:a394a04bd0b7 396 break;
sk84life85 0:a394a04bd0b7 397 }
sk84life85 0:a394a04bd0b7 398 }/*
sk84life85 0:a394a04bd0b7 399 else if (params->len > 1) {
sk84life85 0:a394a04bd0b7 400 switch (params->data[0]) {
sk84life85 0:a394a04bd0b7 401 case 's':
sk84life85 0:a394a04bd0b7 402
sk84life85 0:a394a04bd0b7 403 rValue = BH1745_Red;
sk84life85 0:a394a04bd0b7 404 gValue = BH1745_Green;
sk84life85 0:a394a04bd0b7 405 bValue = BH1745_Blue;
sk84life85 0:a394a04bd0b7 406 cValue = BH1745_Clear;
sk84life85 0:a394a04bd0b7 407
sk84life85 0:a394a04bd0b7 408 break;
sk84life85 0:a394a04bd0b7 409 }
sk84life85 0:a394a04bd0b7 410 } */
sk84life85 0:a394a04bd0b7 411 else
sk84life85 0:a394a04bd0b7 412 {
sk84life85 0:a394a04bd0b7 413 len = snprintf((char*) buf, MAX_REPLY_LEN, "ERROR");
sk84life85 0:a394a04bd0b7 414 }
sk84life85 0:a394a04bd0b7 415
sk84life85 0:a394a04bd0b7 416 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 417 DEBUG("%d bytes received from host\n\r", params->len);
sk84life85 0:a394a04bd0b7 418 }
sk84life85 0:a394a04bd0b7 419 }
sk84life85 0:a394a04bd0b7 420
sk84life85 0:a394a04bd0b7 421
sk84life85 0:a394a04bd0b7 422 /**
sk84life85 0:a394a04bd0b7 423 * This callback is used whenever a write to a GATT characteristic causes data to be sent to the host.
sk84life85 0:a394a04bd0b7 424 */
sk84life85 0:a394a04bd0b7 425 void dataSentCallback(unsigned count)
sk84life85 0:a394a04bd0b7 426 {
sk84life85 0:a394a04bd0b7 427 // NOTE: The count always seems to be 1 regardless of data.
sk84life85 0:a394a04bd0b7 428 DEBUG("%d bytes sent to host\n\r", count);
sk84life85 0:a394a04bd0b7 429 }
sk84life85 0:a394a04bd0b7 430
sk84life85 0:a394a04bd0b7 431
sk84life85 0:a394a04bd0b7 432 /**
sk84life85 0:a394a04bd0b7 433 * This callback is scheduled to be called periodically via a low-priority interrupt.
sk84life85 0:a394a04bd0b7 434 */
sk84life85 0:a394a04bd0b7 435 void periodicCallback(void)
sk84life85 0:a394a04bd0b7 436 {
sk84life85 0:a394a04bd0b7 437
sk84life85 0:a394a04bd0b7 438 // uint8_t buf[MAX_REPLY_LEN];
sk84life85 0:a394a04bd0b7 439 // uint32_t len = 0;
sk84life85 0:a394a04bd0b7 440
sk84life85 0:a394a04bd0b7 441
sk84life85 0:a394a04bd0b7 442
sk84life85 0:a394a04bd0b7 443 if (m_ble.getGapState().connected) {
sk84life85 0:a394a04bd0b7 444 if(isConnected==0){
sk84life85 0:a394a04bd0b7 445 }
sk84life85 0:a394a04bd0b7 446 isConnected = 1;
sk84life85 0:a394a04bd0b7 447 }
sk84life85 0:a394a04bd0b7 448 else{
sk84life85 0:a394a04bd0b7 449 isConnected = 0;
sk84life85 0:a394a04bd0b7 450 }
sk84life85 0:a394a04bd0b7 451
sk84life85 0:a394a04bd0b7 452 }
sk84life85 0:a394a04bd0b7 453
sk84life85 0:a394a04bd0b7 454
sk84life85 0:a394a04bd0b7 455 void error(ble_error_t err, uint32_t line)
sk84life85 0:a394a04bd0b7 456 {
sk84life85 0:a394a04bd0b7 457 m_error_led = 1;
sk84life85 0:a394a04bd0b7 458 DEBUG("Error %d on line number %d\n\r", err, line);
sk84life85 0:a394a04bd0b7 459 }
sk84life85 0:a394a04bd0b7 460
sk84life85 0:a394a04bd0b7 461 //Code for writing back to Console if a PB is pressed
sk84life85 0:a394a04bd0b7 462 void PBTrigger()
sk84life85 0:a394a04bd0b7 463 {
sk84life85 0:a394a04bd0b7 464 //uint8_t buf[MAX_REPLY_LEN];
sk84life85 0:a394a04bd0b7 465 //uint32_t len = 0;
sk84life85 0:a394a04bd0b7 466
sk84life85 0:a394a04bd0b7 467 m_cmd_led = !m_cmd_led;
sk84life85 0:a394a04bd0b7 468
sk84life85 0:a394a04bd0b7 469 if(!setCruise){
sk84life85 0:a394a04bd0b7 470 cruiseFlag = 1;
sk84life85 0:a394a04bd0b7 471 cruiseSet = Speed_value*2.75;
sk84life85 0:a394a04bd0b7 472 }
sk84life85 0:a394a04bd0b7 473 if(!resetCruise){
sk84life85 0:a394a04bd0b7 474 cruiseFlag = 0;
sk84life85 0:a394a04bd0b7 475 }
sk84life85 0:a394a04bd0b7 476 m_cmd_led = cruiseFlag;
sk84life85 0:a394a04bd0b7 477
sk84life85 0:a394a04bd0b7 478
sk84life85 0:a394a04bd0b7 479 /*
sk84life85 0:a394a04bd0b7 480 if (m_ble.getGapState().connected) {
sk84life85 0:a394a04bd0b7 481 }
sk84life85 0:a394a04bd0b7 482 */
sk84life85 0:a394a04bd0b7 483 }
sk84life85 0:a394a04bd0b7 484
sk84life85 0:a394a04bd0b7 485 int main(void)
sk84life85 0:a394a04bd0b7 486 {
sk84life85 0:a394a04bd0b7 487 ble_error_t err;
sk84life85 0:a394a04bd0b7 488 Ticker ticker;
sk84life85 0:a394a04bd0b7 489
sk84life85 0:a394a04bd0b7 490 m_serial_port.baud(UART_BAUD_RATE);
sk84life85 0:a394a04bd0b7 491
sk84life85 0:a394a04bd0b7 492 DEBUG("Initialising...\n\r");
sk84life85 0:a394a04bd0b7 493
sk84life85 0:a394a04bd0b7 494 m_cmd_led = 0;
sk84life85 0:a394a04bd0b7 495 m_error_led = 0;
sk84life85 0:a394a04bd0b7 496
sk84life85 0:a394a04bd0b7 497 //ticker.attach(periodicCallback, SENSOR_READ_INTERVAL_S);
sk84life85 0:a394a04bd0b7 498
sk84life85 0:a394a04bd0b7 499 //sw4Press.fall(&PBTrigger);
sk84life85 0:a394a04bd0b7 500
sk84life85 0:a394a04bd0b7 501 setCruise.fall(&PBTrigger);
sk84life85 0:a394a04bd0b7 502 resetCruise.fall(&PBTrigger);
sk84life85 0:a394a04bd0b7 503
sk84life85 0:a394a04bd0b7 504 //Start BTLE Initialization Section
sk84life85 0:a394a04bd0b7 505 m_ble.init();
sk84life85 0:a394a04bd0b7 506 m_ble.onDisconnection(disconnectionCallback);
sk84life85 0:a394a04bd0b7 507 m_ble.onDataWritten(dataWrittenCallback);
sk84life85 0:a394a04bd0b7 508 m_ble.onDataSent(dataSentCallback);
sk84life85 0:a394a04bd0b7 509
sk84life85 0:a394a04bd0b7 510 // Set the TX power in dBm units.
sk84life85 0:a394a04bd0b7 511 // Possible values (in decreasing order): 4, 0, -4, -8, -12, -16, -20.
sk84life85 0:a394a04bd0b7 512 err = m_ble.setTxPower(4);
sk84life85 0:a394a04bd0b7 513 if (BLE_ERROR_NONE != err) {
sk84life85 0:a394a04bd0b7 514 error(err, __LINE__);
sk84life85 0:a394a04bd0b7 515 }
sk84life85 0:a394a04bd0b7 516
sk84life85 0:a394a04bd0b7 517 // Setup advertising (GAP stuff).
sk84life85 0:a394a04bd0b7 518 err = m_ble.setDeviceName(DEVICE_NAME);
sk84life85 0:a394a04bd0b7 519 if (BLE_ERROR_NONE != err) {
sk84life85 0:a394a04bd0b7 520 error(err, __LINE__);
sk84life85 0:a394a04bd0b7 521 }
sk84life85 0:a394a04bd0b7 522
sk84life85 0:a394a04bd0b7 523 err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
sk84life85 0:a394a04bd0b7 524 if (BLE_ERROR_NONE != err) {
sk84life85 0:a394a04bd0b7 525 error(err, __LINE__);
sk84life85 0:a394a04bd0b7 526 }
sk84life85 0:a394a04bd0b7 527
sk84life85 0:a394a04bd0b7 528 m_ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
sk84life85 0:a394a04bd0b7 529
sk84life85 0:a394a04bd0b7 530 err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
sk84life85 0:a394a04bd0b7 531 (const uint8_t *)SHORT_NAME,
sk84life85 0:a394a04bd0b7 532 (sizeof(SHORT_NAME) - 1));
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 err = m_ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
sk84life85 0:a394a04bd0b7 538 (const uint8_t *)UARTServiceUUID_reversed,
sk84life85 0:a394a04bd0b7 539 sizeof(UARTServiceUUID_reversed));
sk84life85 0:a394a04bd0b7 540 if (BLE_ERROR_NONE != err) {
sk84life85 0:a394a04bd0b7 541 error(err, __LINE__);
sk84life85 0:a394a04bd0b7 542 }
sk84life85 0:a394a04bd0b7 543
sk84life85 0:a394a04bd0b7 544 m_ble.setAdvertisingInterval(Gap::MSEC_TO_ADVERTISEMENT_DURATION_UNITS(ADV_INTERVAL_MS));
sk84life85 0:a394a04bd0b7 545 m_ble.startAdvertising();
sk84life85 0:a394a04bd0b7 546
sk84life85 0:a394a04bd0b7 547 // Create a UARTService object (GATT stuff).
sk84life85 0:a394a04bd0b7 548 UARTService uartService(m_ble);
sk84life85 0:a394a04bd0b7 549 m_uart_service_ptr = &uartService;
sk84life85 0:a394a04bd0b7 550
sk84life85 0:a394a04bd0b7 551 #ifdef BH1745
sk84life85 0:a394a04bd0b7 552 i2c.write(BH1745_addr_w, &BH1745_CTRL2[0], 2, false);
sk84life85 0:a394a04bd0b7 553 #endif
sk84life85 0:a394a04bd0b7 554
sk84life85 0:a394a04bd0b7 555 #ifdef BatteryI2C
sk84life85 0:a394a04bd0b7 556 i2c.write(BMS_addr_w, &BMS_DeviceName, 1, RepStart);
sk84life85 0:a394a04bd0b7 557 i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 8, NoRepStart);
sk84life85 0:a394a04bd0b7 558
sk84life85 0:a394a04bd0b7 559 //BMS_Password = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]);
sk84life85 0:a394a04bd0b7 560 BMS_Password0 = (BMS_Content_ReadData[0]);
sk84life85 0:a394a04bd0b7 561 BMS_Password1 = (BMS_Content_ReadData[1]);
sk84life85 0:a394a04bd0b7 562 BMS_Password2 = (BMS_Content_ReadData[2]);
sk84life85 0:a394a04bd0b7 563 BMS_Password3 = (BMS_Content_ReadData[3]);
sk84life85 0:a394a04bd0b7 564 BMS_Password4 = (BMS_Content_ReadData[4]);
sk84life85 0:a394a04bd0b7 565 BMS_Password5 = (BMS_Content_ReadData[5]);
sk84life85 0:a394a04bd0b7 566 BMS_Password6 = (BMS_Content_ReadData[6]);
sk84life85 0:a394a04bd0b7 567 BMS_Password7 = (BMS_Content_ReadData[7]);
sk84life85 0:a394a04bd0b7 568 BMS_Password8 = (BMS_Content_ReadData[8]);
sk84life85 0:a394a04bd0b7 569 i2c.write(BMS_addr_w, &BMS_Cycles, 1, RepStart);
sk84life85 0:a394a04bd0b7 570 i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 2, NoRepStart);
sk84life85 0:a394a04bd0b7 571 BMS_CycleOut = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]);
sk84life85 0:a394a04bd0b7 572
sk84life85 0:a394a04bd0b7 573 i2c.write(BMS_addr_w, &BMS_SOC, 1, RepStart);
sk84life85 0:a394a04bd0b7 574 i2c.read(BMS_addr_r, &BMS_Content_ReadData[0], 2, NoRepStart);
sk84life85 0:a394a04bd0b7 575 BMS_SOCOut = (BMS_Content_ReadData[1]<<8) | (BMS_Content_ReadData[0]);
sk84life85 0:a394a04bd0b7 576 #endif
sk84life85 0:a394a04bd0b7 577
sk84life85 0:a394a04bd0b7 578 //Initialize PWM Output
sk84life85 0:a394a04bd0b7 579 PWM.period_us(1000);
sk84life85 0:a394a04bd0b7 580 hornFreq.period_us(1250);
sk84life85 0:a394a04bd0b7 581 //m_cmd_led.period_ms(10);
sk84life85 0:a394a04bd0b7 582 ADC_value_old = 0;
sk84life85 0:a394a04bd0b7 583
sk84life85 0:a394a04bd0b7 584 p = 0;
sk84life85 0:a394a04bd0b7 585 SpeedSet = 1;
sk84life85 0:a394a04bd0b7 586 tick = deltaT;
sk84life85 0:a394a04bd0b7 587 SpeedSampNum = 5;
sk84life85 0:a394a04bd0b7 588
sk84life85 0:a394a04bd0b7 589 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 590 wait_ms(100);
sk84life85 0:a394a04bd0b7 591 wait_ms(100);
sk84life85 0:a394a04bd0b7 592 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 593 wait_ms(100);
sk84life85 0:a394a04bd0b7 594 wait_ms(100);
sk84life85 0:a394a04bd0b7 595 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 596 wait_ms(100);
sk84life85 0:a394a04bd0b7 597 wait_ms(100);
sk84life85 0:a394a04bd0b7 598 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 599 wait_ms(100);
sk84life85 0:a394a04bd0b7 600 wait_ms(100);
sk84life85 0:a394a04bd0b7 601 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 602 wait_ms(100);
sk84life85 0:a394a04bd0b7 603 wait_ms(100);
sk84life85 0:a394a04bd0b7 604 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 605 wait_ms(100);
sk84life85 0:a394a04bd0b7 606 wait_ms(100);
sk84life85 0:a394a04bd0b7 607
sk84life85 0:a394a04bd0b7 608 while (true) {
sk84life85 0:a394a04bd0b7 609 //m_ble.waitForEvent();
sk84life85 0:a394a04bd0b7 610 if(m_ble.getGapState().connected){
sk84life85 0:a394a04bd0b7 611 if(!isConnected){
sk84life85 0:a394a04bd0b7 612 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 613 wait_ms(100);
sk84life85 0:a394a04bd0b7 614 wait_ms(100);
sk84life85 0:a394a04bd0b7 615 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 616 wait_ms(100);
sk84life85 0:a394a04bd0b7 617 wait_ms(100);
sk84life85 0:a394a04bd0b7 618 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 619 wait_ms(100);
sk84life85 0:a394a04bd0b7 620 wait_ms(100);
sk84life85 0:a394a04bd0b7 621 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 622 wait_ms(100);
sk84life85 0:a394a04bd0b7 623 wait_ms(100);
sk84life85 0:a394a04bd0b7 624 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 625 wait_ms(100);
sk84life85 0:a394a04bd0b7 626 wait_ms(100);
sk84life85 0:a394a04bd0b7 627 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 628 wait_ms(100);
sk84life85 0:a394a04bd0b7 629 wait_ms(100);
sk84life85 0:a394a04bd0b7 630 }
sk84life85 0:a394a04bd0b7 631 isConnected = 1;
sk84life85 0:a394a04bd0b7 632
sk84life85 0:a394a04bd0b7 633 //Read KX022 Portion from the IC
sk84life85 0:a394a04bd0b7 634 i2c.write(BH1745_addr_w, &BH1745_Red_Data_LSB, 1, RepStart);
sk84life85 0:a394a04bd0b7 635 i2c.read(BH1745_addr_r, &BH1745_Content_ReadData[0], 8, NoRepStart);
sk84life85 0:a394a04bd0b7 636
sk84life85 0:a394a04bd0b7 637 //reconfigure the data (taken from arduino code)
sk84life85 0:a394a04bd0b7 638 BH1745_Raw_R = (BH1745_Content_ReadData[1]<<8) | (BH1745_Content_ReadData[0]);
sk84life85 0:a394a04bd0b7 639 BH1745_Raw_G = (BH1745_Content_ReadData[3]<<8) | (BH1745_Content_ReadData[2]);
sk84life85 0:a394a04bd0b7 640 BH1745_Raw_B = (BH1745_Content_ReadData[5]<<8) | (BH1745_Content_ReadData[4]);
sk84life85 0:a394a04bd0b7 641 BH1745_Raw_C = (BH1745_Content_ReadData[7]<<8) | (BH1745_Content_ReadData[6]);
sk84life85 0:a394a04bd0b7 642
sk84life85 0:a394a04bd0b7 643 BH1745_Red = (float)BH1745_Raw_R / 16384;
sk84life85 0:a394a04bd0b7 644 BH1745_Green = (float)BH1745_Raw_G / 16384;
sk84life85 0:a394a04bd0b7 645 BH1745_Blue = (float)BH1745_Raw_B / 16384;
sk84life85 0:a394a04bd0b7 646 BH1745_Clear = (float)BH1745_Raw_C / 16384;
sk84life85 0:a394a04bd0b7 647
sk84life85 0:a394a04bd0b7 648 if(BH1745_Clear < 0.00001){
sk84life85 0:a394a04bd0b7 649
sk84life85 0:a394a04bd0b7 650 wait_ms(100);
sk84life85 0:a394a04bd0b7 651 wait_ms(100);
sk84life85 0:a394a04bd0b7 652 wait_ms(100);
sk84life85 0:a394a04bd0b7 653 wait_ms(100);
sk84life85 0:a394a04bd0b7 654 wait_ms(100);
sk84life85 0:a394a04bd0b7 655 wait_ms(100);
sk84life85 0:a394a04bd0b7 656 wait_ms(100);
sk84life85 0:a394a04bd0b7 657 wait_ms(100);
sk84life85 0:a394a04bd0b7 658 LED_Enable = 1;
sk84life85 0:a394a04bd0b7 659 wait_ms(100);
sk84life85 0:a394a04bd0b7 660 wait_ms(100);
sk84life85 0:a394a04bd0b7 661
sk84life85 0:a394a04bd0b7 662 for(i=0; i<60; i++){
sk84life85 0:a394a04bd0b7 663
sk84life85 0:a394a04bd0b7 664 //---------------------------- KX022 ----------------------------
sk84life85 0:a394a04bd0b7 665 //Read KX022 Portion from the IC
sk84life85 0:a394a04bd0b7 666 i2c.write(BH1745_addr_w, &BH1745_Red_Data_LSB, 1, RepStart);
sk84life85 0:a394a04bd0b7 667 i2c.read(BH1745_addr_r, &BH1745_Content_ReadData[0], 8, NoRepStart);
sk84life85 0:a394a04bd0b7 668
sk84life85 0:a394a04bd0b7 669 //reconfigure the data (taken from arduino code)
sk84life85 0:a394a04bd0b7 670 BH1745_Raw_R = (BH1745_Content_ReadData[1]<<8) | (BH1745_Content_ReadData[0]);
sk84life85 0:a394a04bd0b7 671 BH1745_Raw_G = (BH1745_Content_ReadData[3]<<8) | (BH1745_Content_ReadData[2]);
sk84life85 0:a394a04bd0b7 672 BH1745_Raw_B = (BH1745_Content_ReadData[5]<<8) | (BH1745_Content_ReadData[4]);
sk84life85 0:a394a04bd0b7 673 BH1745_Raw_C = (BH1745_Content_ReadData[7]<<8) | (BH1745_Content_ReadData[6]);
sk84life85 0:a394a04bd0b7 674
sk84life85 0:a394a04bd0b7 675 LED_Enable = 0;
sk84life85 0:a394a04bd0b7 676
sk84life85 0:a394a04bd0b7 677 BH1745_Red = (float)BH1745_Raw_R / 16384;
sk84life85 0:a394a04bd0b7 678 BH1745_Green = (float)BH1745_Raw_G / 16384;
sk84life85 0:a394a04bd0b7 679 BH1745_Blue = (float)BH1745_Raw_B / 16384;
sk84life85 0:a394a04bd0b7 680 BH1745_Clear = (float)BH1745_Raw_C / 16384;
sk84life85 0:a394a04bd0b7 681
sk84life85 0:a394a04bd0b7 682 CIEx[i] = 0.232*BH1745_Red + 0.610*BH1745_Green + 0.055*BH1745_Blue;
sk84life85 0:a394a04bd0b7 683 if( BH1745_Clear < (0.2*BH1745_Green) ){
sk84life85 0:a394a04bd0b7 684 if( BH1745_Blue < (0.45*BH1745_Green) ){
sk84life85 0:a394a04bd0b7 685 CIEy[i] = 0.083*BH1745_Red + 0.860*BH1745_Green;
sk84life85 0:a394a04bd0b7 686 }
sk84life85 0:a394a04bd0b7 687 else{
sk84life85 0:a394a04bd0b7 688 CIEy[i] = 0.070*BH1745_Red + 0.709*BH1745_Green;
sk84life85 0:a394a04bd0b7 689 }
sk84life85 0:a394a04bd0b7 690 }
sk84life85 0:a394a04bd0b7 691 else if( BH1745_Clear < (0.50*BH1745_Green) ){
sk84life85 0:a394a04bd0b7 692 CIEy[i] = 0.110*BH1745_Red + 0.712*BH1745_Green;
sk84life85 0:a394a04bd0b7 693 }
sk84life85 0:a394a04bd0b7 694 else{
sk84life85 0:a394a04bd0b7 695 CIEy[i] = 0.073*BH1745_Red + 0.864*BH1745_Green;
sk84life85 0:a394a04bd0b7 696 }
sk84life85 0:a394a04bd0b7 697 for(j=i; j>0; j--){
sk84life85 0:a394a04bd0b7 698 tempCIE = CIEx[j];
sk84life85 0:a394a04bd0b7 699 if(tempCIE<CIEx[j-1]){
sk84life85 0:a394a04bd0b7 700 CIEx[j] = CIEx[j-1];
sk84life85 0:a394a04bd0b7 701 CIEx[j-1] = tempCIE;
sk84life85 0:a394a04bd0b7 702 }
sk84life85 0:a394a04bd0b7 703 }
sk84life85 0:a394a04bd0b7 704 for(j=i; j>0; j--){
sk84life85 0:a394a04bd0b7 705 tempCIE = CIEy[j];
sk84life85 0:a394a04bd0b7 706 if(tempCIE<CIEy[j-1]){
sk84life85 0:a394a04bd0b7 707 CIEy[j] = CIEy[j-1];
sk84life85 0:a394a04bd0b7 708 CIEy[j-1] = tempCIE;
sk84life85 0:a394a04bd0b7 709 }
sk84life85 0:a394a04bd0b7 710 }
sk84life85 0:a394a04bd0b7 711 }
sk84life85 0:a394a04bd0b7 712
sk84life85 0:a394a04bd0b7 713
sk84life85 0:a394a04bd0b7 714 chromaX = (CIEx[28]+CIEx[29]+CIEx[30]+CIEx[31]+CIEx[32])/5.0;
sk84life85 0:a394a04bd0b7 715 chromaY = (CIEy[28]+CIEy[29]+CIEy[30]+CIEy[31]+CIEy[32])/5.0;
sk84life85 0:a394a04bd0b7 716
sk84life85 0:a394a04bd0b7 717 if( (xPPU2_14-xThresh<chromaX) && (chromaX<xPPU2_14+xThresh) && (yPPU2_14-yThresh<chromaY) && (chromaY<yPPU2_14+yThresh) ){
sk84life85 0:a394a04bd0b7 718 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "2-14");
sk84life85 0:a394a04bd0b7 719 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 720 }
sk84life85 0:a394a04bd0b7 721 else if( (xPPU18_5-xThresh<chromaX) && (chromaX<xPPU18_5+xThresh) && (yPPU18_5-yThresh<chromaY) && (chromaY<yPPU18_5+yThresh) ){
sk84life85 0:a394a04bd0b7 722 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-5");
sk84life85 0:a394a04bd0b7 723 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 724 }
sk84life85 0:a394a04bd0b7 725 else if( (xPPU18_4-xThresh<chromaX) && (chromaX<xPPU18_4+xThresh) && (yPPU18_4-yThresh<chromaY) && (chromaY<yPPU18_4+yThresh) ){
sk84life85 0:a394a04bd0b7 726 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-4");
sk84life85 0:a394a04bd0b7 727 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 728 }
sk84life85 0:a394a04bd0b7 729 else if( (xPPU18_3-xThresh<chromaX) && (chromaX<xPPU18_3+xThresh) && (yPPU18_3-yThresh<chromaY) && (chromaY<yPPU18_3+yThresh) ){
sk84life85 0:a394a04bd0b7 730 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "18-3");
sk84life85 0:a394a04bd0b7 731 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 732 }
sk84life85 0:a394a04bd0b7 733 else if( (xPPU17_18-xThresh<chromaX) && (chromaX<xPPU17_18+xThresh) && (yPPU17_18-yThresh<chromaY) && (chromaY<yPPU17_18+yThresh) ){
sk84life85 0:a394a04bd0b7 734 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "17-18");
sk84life85 0:a394a04bd0b7 735 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 736 }
sk84life85 0:a394a04bd0b7 737 else if( (xPPU5_6-xThresh<chromaX) && (chromaX<xPPU5_6+xThresh) && (yPPU5_6-yThresh<chromaY) && (chromaY<yPPU5_6+yThresh) ){
sk84life85 0:a394a04bd0b7 738 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "5-6");
sk84life85 0:a394a04bd0b7 739 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 740 }
sk84life85 0:a394a04bd0b7 741 else if( (xPPU4_15-xThresh<chromaX) && (chromaX<xPPU4_15+xThresh) && (yPPU4_15-yThresh<chromaY) && (chromaY<yPPU4_15+yThresh) ){
sk84life85 0:a394a04bd0b7 742 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-15");
sk84life85 0:a394a04bd0b7 743 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 744 }
sk84life85 0:a394a04bd0b7 745 else if( (xPPU4_14-xThresh<chromaX) && (chromaX<xPPU4_14+xThresh) && (yPPU4_14-yThresh<chromaY) && (chromaY<yPPU4_14+yThresh) ){
sk84life85 0:a394a04bd0b7 746 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-14");
sk84life85 0:a394a04bd0b7 747 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 748 }
sk84life85 0:a394a04bd0b7 749 else if( (xPPU4_13-xThresh<chromaX) && (chromaX<xPPU4_13+xThresh) && (yPPU4_13-yThresh<chromaY) && (chromaY<yPPU4_13+yThresh) ){
sk84life85 0:a394a04bd0b7 750 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-13");
sk84life85 0:a394a04bd0b7 751 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 752 }
sk84life85 0:a394a04bd0b7 753 else if( (xPPU4_10-xThresh<chromaX) && (chromaX<xPPU4_10+xThresh) && (yPPU4_10-yThresh<chromaY) && (chromaY<yPPU4_10+yThresh) ){
sk84life85 0:a394a04bd0b7 754 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-10");
sk84life85 0:a394a04bd0b7 755 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 756 }
sk84life85 0:a394a04bd0b7 757 else if( (xPPU4_8-xThresh<chromaX) && (chromaX<xPPU4_8+xThresh) && (yPPU4_8-yThresh<chromaY) && (chromaY<yPPU4_8+yThresh) ){
sk84life85 0:a394a04bd0b7 758 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "4-8");
sk84life85 0:a394a04bd0b7 759 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 760 }
sk84life85 1:88e3620627f9 761 else if( (xPPU9_4-xThresh<chromaX) && (chromaX<xPPU9_4+xThresh) && (yPPU9_4-yThresh<chromaY) && (chromaY<yPPU9_4+yThresh) ){
sk84life85 1:88e3620627f9 762 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "9-4");
sk84life85 1:88e3620627f9 763 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 1:88e3620627f9 764 }
sk84life85 1:88e3620627f9 765 else if( (xPPU9_5-xThresh<chromaX) && (chromaX<xPPU9_5+xThresh) && (yPPU9_5-yThresh<chromaY) && (chromaY<yPPU9_5+yThresh) ){
sk84life85 1:88e3620627f9 766 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "9-5");
sk84life85 1:88e3620627f9 767 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 1:88e3620627f9 768 }
sk84life85 0:a394a04bd0b7 769 else{
sk84life85 0:a394a04bd0b7 770 len = snprintf((char*) buf, MAX_REPLY_LEN, "%s", "Unregistered");
sk84life85 0:a394a04bd0b7 771 m_ble.updateCharacteristicValue(m_uart_service_ptr->getRXCharacteristicHandle(), buf, len);
sk84life85 0:a394a04bd0b7 772 }
sk84life85 0:a394a04bd0b7 773 }
sk84life85 0:a394a04bd0b7 774
sk84life85 0:a394a04bd0b7 775 }
sk84life85 0:a394a04bd0b7 776 else{
sk84life85 0:a394a04bd0b7 777 isConnected = 0;
sk84life85 0:a394a04bd0b7 778 }
sk84life85 0:a394a04bd0b7 779 }
sk84life85 0:a394a04bd0b7 780 }