Masaya Shimoyamada
/
Inp_Fiber_Logo_FOTA_reduce_msg_bytes
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: BLE_API mbed nRF51822
Fork of
Inp_Fiber_Logo_FOTA
by 
inupathy
main.cpp@5:b3fe9050a777, 2017-05-30 (annotated)
- Committer:
- jojisdogakane
- Date:
- Tue May 30 07:54:02 2017 +0000
- Revision:
- 5:b3fe9050a777
- Parent:
- 4:a0150e0f285b
- Child:
- 6:35f536162183
Ando share
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| jojisdogakane | 0:2f6112d75752 | 1 | #include "mbed.h" |
| jojisdogakane | 0:2f6112d75752 | 2 | #include "neopixel.h" |
| jojisdogakane | 0:2f6112d75752 | 3 | #include "nrf_soc.h" |
| jojisdogakane | 0:2f6112d75752 | 4 | #include "common.h" |
| jojisdogakane | 0:2f6112d75752 | 5 | #include "ble/BLE.h" |
| jojisdogakane | 0:2f6112d75752 | 6 | #include "ble/services/HeartRateService.h" |
| jojisdogakane | 0:2f6112d75752 | 7 | #include "ble/services/BatteryService.h" |
| jojisdogakane | 0:2f6112d75752 | 8 | #include "ble/services/DeviceInformationService.h" |
| jojisdogakane | 0:2f6112d75752 | 9 | #include "UARTService.h" |
| ayaando | 1:f28bdfaf3005 | 10 | #include "ble_gap.h" |
| ayaando | 2:221df6d014f5 | 11 | #include "ble/services/DFUService.h" // ★OTA用 |
| ayaando | 2:221df6d014f5 | 12 | |
| jojisdogakane | 0:2f6112d75752 | 13 | #define RAINBOW_RATIO 1 |
| jojisdogakane | 0:2f6112d75752 | 14 | #define RCFILTER 0.9 |
| jojisdogakane | 0:2f6112d75752 | 15 | #define RCFILTER2 0.9 |
| jojisdogakane | 4:a0150e0f285b | 16 | #define NEOPIXEL_COUNT 6 |
| jojisdogakane | 0:2f6112d75752 | 17 | #define MIN_LIGHT 0 |
| jojisdogakane | 0:2f6112d75752 | 18 | #define PNNX_FACTOR 0.1 // 0.06 |
| jojisdogakane | 4:a0150e0f285b | 19 | #define MAXARRAY 15 //15 // 心拍平均値計算用の配列サイズ //20 |
| jojisdogakane | 4:a0150e0f285b | 20 | #define MAXHRVARRAY 10 //30 // 脈間ゆらぎ平均値計算用の配列サイズ |
| jojisdogakane | 0:2f6112d75752 | 21 | #define MIN_INTERVAL 200 |
| jojisdogakane | 0:2f6112d75752 | 22 | #define INTERVAL 10 // Loopの間隔 通常時推奨1 通信時推奨２０ |
| jojisdogakane | 0:2f6112d75752 | 23 | #define MAXCOLORARRAY 1 //(MIN_INTERVAL / INTERVAL) // 色平均値計算用の配列サイズ (MIN_INTERVAL / 7) |
| jojisdogakane | 0:2f6112d75752 | 24 | #define MAXCONARRAY 10 |
| jojisdogakane | 0:2f6112d75752 | 25 | #define MAXSTRESSARRAY 3 |
| jojisdogakane | 5:b3fe9050a777 | 26 | #define DIM_LED 0.5 //0.2 |
| jojisdogakane | 0:2f6112d75752 | 27 | //#define DIM_LED 1.0 |
| jojisdogakane | 0:2f6112d75752 | 28 | |
| jojisdogakane | 0:2f6112d75752 | 29 | // 心拍数の最低値、最高値 |
| jojisdogakane | 0:2f6112d75752 | 30 | // 人間の場合は60～150、標準80 |
| jojisdogakane | 0:2f6112d75752 | 31 | // 犬の場合は40～200、標準60 |
| jojisdogakane | 0:2f6112d75752 | 32 | #define HR_MIN 40 |
| jojisdogakane | 0:2f6112d75752 | 33 | #define HR_MAX 150 |
| jojisdogakane | 0:2f6112d75752 | 34 | |
| jojisdogakane | 0:2f6112d75752 | 35 | #define RELAX_RATIO_MIN 0.6 |
| jojisdogakane | 0:2f6112d75752 | 36 | |
| jojisdogakane | 0:2f6112d75752 | 37 | // ラジアンを度に変換する． |
| jojisdogakane | 0:2f6112d75752 | 38 | #define RadianToDegree(radian) ((180 / 3.14159) * (radian)) |
| jojisdogakane | 0:2f6112d75752 | 39 | |
| jojisdogakane | 0:2f6112d75752 | 40 | // 度をラジアンに変換する． |
| jojisdogakane | 0:2f6112d75752 | 41 | #define DegreeToRadian(degree) ((3.14159 / 180) * (degree)) |
| jojisdogakane | 0:2f6112d75752 | 42 | |
| jojisdogakane | 0:2f6112d75752 | 43 | // 色の識別値設定（ピンではない） |
| jojisdogakane | 0:2f6112d75752 | 44 | #define RED 0 |
| jojisdogakane | 0:2f6112d75752 | 45 | #define BLUE 1 |
| jojisdogakane | 0:2f6112d75752 | 46 | #define GREEN 2 |
| jojisdogakane | 0:2f6112d75752 | 47 | |
| jojisdogakane | 0:2f6112d75752 | 48 | #define SHOW_EXCITE 1 |
| jojisdogakane | 0:2f6112d75752 | 49 | #define SHOW_HAPPY 2 |
| jojisdogakane | 0:2f6112d75752 | 50 | #define SHOW_CONCENTRATION 4 |
| jojisdogakane | 0:2f6112d75752 | 51 | |
| jojisdogakane | 0:2f6112d75752 | 52 | int displayFlag = 0; |
| jojisdogakane | 0:2f6112d75752 | 53 | |
| jojisdogakane | 0:2f6112d75752 | 54 | BLE ble; |
| ayaando | 2:221df6d014f5 | 55 | DFUService *dfuService; // ★OTA用 |
| jojisdogakane | 0:2f6112d75752 | 56 | |
| jojisdogakane | 0:2f6112d75752 | 57 | AnalogIn HRSigIn(P0_4); |
| jojisdogakane | 0:2f6112d75752 | 58 | |
| jojisdogakane | 0:2f6112d75752 | 59 | |
| ayaando | 1:f28bdfaf3005 | 60 | //const static char DEVICE_NAME[] = "InupLogo"; |
| ayaando | 1:f28bdfaf3005 | 61 | static char DEVICE_NAME[16] = "IND"; |
| jojisdogakane | 0:2f6112d75752 | 62 | static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE, |
| jojisdogakane | 0:2f6112d75752 | 63 | GattService::UUID_DEVICE_INFORMATION_SERVICE}; |
| jojisdogakane | 0:2f6112d75752 | 64 | static volatile bool triggerSensorPolling = true; |
| jojisdogakane | 0:2f6112d75752 | 65 | |
| jojisdogakane | 4:a0150e0f285b | 66 | short ColorArrayR[NEOPIXEL_COUNT]; |
| jojisdogakane | 4:a0150e0f285b | 67 | short ColorArrayG[NEOPIXEL_COUNT]; |
| jojisdogakane | 4:a0150e0f285b | 68 | short ColorArrayB[NEOPIXEL_COUNT]; |
| jojisdogakane | 0:2f6112d75752 | 69 | |
| jojisdogakane | 4:a0150e0f285b | 70 | short TargetColorArrayR[NEOPIXEL_COUNT]; |
| jojisdogakane | 4:a0150e0f285b | 71 | short TargetColorArrayG[NEOPIXEL_COUNT]; |
| jojisdogakane | 4:a0150e0f285b | 72 | short TargetColorArrayB[NEOPIXEL_COUNT]; |
| jojisdogakane | 0:2f6112d75752 | 73 | |
| jojisdogakane | 0:2f6112d75752 | 74 | |
| jojisdogakane | 4:a0150e0f285b | 75 | #define NEOPIXEL_COMMANDLEN ((NEOPIXEL_COUNT * 3) + 1 + 1) |
| jojisdogakane | 0:2f6112d75752 | 76 | |
| jojisdogakane | 0:2f6112d75752 | 77 | |
| jojisdogakane | 0:2f6112d75752 | 78 | unsigned long pixelStartCounter; |
| jojisdogakane | 0:2f6112d75752 | 79 | |
| jojisdogakane | 0:2f6112d75752 | 80 | int mainCurrentPixel = 0; |
| jojisdogakane | 0:2f6112d75752 | 81 | int mainPrevPixel = 0; |
| jojisdogakane | 0:2f6112d75752 | 82 | int mainPixelLightCount; |
| jojisdogakane | 0:2f6112d75752 | 83 | int mainPixelStep = 0; |
| jojisdogakane | 0:2f6112d75752 | 84 | int mainStepsRemain = 0; |
| jojisdogakane | 0:2f6112d75752 | 85 | |
| jojisdogakane | 0:2f6112d75752 | 86 | unsigned long currentmillis = 0; |
| jojisdogakane | 0:2f6112d75752 | 87 | unsigned long prevmillis = 0; |
| jojisdogakane | 0:2f6112d75752 | 88 | unsigned long intervalmillis = 0; |
| jojisdogakane | 0:2f6112d75752 | 89 | unsigned long prevIntervalmillis = 0; |
| jojisdogakane | 0:2f6112d75752 | 90 | unsigned long intervalmillisArray[MAXARRAY] = {1000}; |
| jojisdogakane | 4:a0150e0f285b | 91 | unsigned long intervalmillisHRVArray[MAXHRVARRAY] = {1000}; |
| jojisdogakane | 0:2f6112d75752 | 92 | float conArray[MAXCONARRAY] = {0.5}; |
| jojisdogakane | 0:2f6112d75752 | 93 | |
| jojisdogakane | 0:2f6112d75752 | 94 | //float stressArray[MAXSTRESSARRAY] = {0.5}; |
| jojisdogakane | 0:2f6112d75752 | 95 | |
| jojisdogakane | 0:2f6112d75752 | 96 | float prevFilteredAngle = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 97 | float happyRatio = 0.0; |
| jojisdogakane | 4:a0150e0f285b | 98 | float happyRatioNow = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 99 | float prevRelaxRatio = 1.0; |
| jojisdogakane | 0:2f6112d75752 | 100 | float prevFilteredDistRatio = 0.0; |
| jojisdogakane | 4:a0150e0f285b | 101 | |
| jojisdogakane | 4:a0150e0f285b | 102 | #define HR_THRESHOLD 2 |
| jojisdogakane | 4:a0150e0f285b | 103 | int HRThreshold[] = {120, 1000}; |
| jojisdogakane | 4:a0150e0f285b | 104 | |
| jojisdogakane | 0:2f6112d75752 | 105 | |
| jojisdogakane | 4:a0150e0f285b | 106 | // 気分判定フラグ |
| jojisdogakane | 4:a0150e0f285b | 107 | #define IS_RELAX 0 |
| jojisdogakane | 4:a0150e0f285b | 108 | #define IS_EXCITED 1 |
| jojisdogakane | 4:a0150e0f285b | 109 | #define IS_HAPPY 2 |
| jojisdogakane | 4:a0150e0f285b | 110 | #define IS_INTERESTED 3 |
| jojisdogakane | 4:a0150e0f285b | 111 | #define IS_STRESSED 4 // IS_STRESSEDはMOODの個数としても扱うので最後の番号にすること |
| jojisdogakane | 4:a0150e0f285b | 112 | #define MOOD_COUNT IS_STRESSED + 1 |
| jojisdogakane | 0:2f6112d75752 | 113 | |
| jojisdogakane | 4:a0150e0f285b | 114 | // 気分判定係数 |
| jojisdogakane | 4:a0150e0f285b | 115 | #define CONMARGIN 0.85 |
| jojisdogakane | 4:a0150e0f285b | 116 | #define RELAXMARGIN 0.2 |
| jojisdogakane | 4:a0150e0f285b | 117 | #define HAPPYMARGIN 0.225 |
| jojisdogakane | 0:2f6112d75752 | 118 | |
| jojisdogakane | 4:a0150e0f285b | 119 | // 気分判定フラグを索引にする |
| jojisdogakane | 4:a0150e0f285b | 120 | int MoodColorR[NEOPIXEL_COUNT][MOOD_COUNT]; |
| jojisdogakane | 4:a0150e0f285b | 121 | int MoodColorG[NEOPIXEL_COUNT][MOOD_COUNT]; |
| jojisdogakane | 4:a0150e0f285b | 122 | int MoodColorB[NEOPIXEL_COUNT][MOOD_COUNT]; |
| jojisdogakane | 4:a0150e0f285b | 123 | |
| jojisdogakane | 0:2f6112d75752 | 124 | |
| jojisdogakane | 4:a0150e0f285b | 125 | unsigned char RainbowR[] = { 50, 50, 50, 127, 200, 255}; |
| jojisdogakane | 4:a0150e0f285b | 126 | unsigned char RainbowG[] = { 50, 127, 255, 127, 100, 50}; |
| jojisdogakane | 4:a0150e0f285b | 127 | unsigned char RainbowB[] = {255, 127, 50, 50, 50, 50}; |
| jojisdogakane | 4:a0150e0f285b | 128 | /* |
| jojisdogakane | 4:a0150e0f285b | 129 | unsigned char HappyR[] = {255, 100, 100, 255, 100, 100}; |
| jojisdogakane | 4:a0150e0f285b | 130 | unsigned char HappyG[] = {100, 255, 100, 100, 255, 100}; |
| jojisdogakane | 4:a0150e0f285b | 131 | unsigned char HappyB[] = {100, 100, 255, 100, 100, 255}; |
| jojisdogakane | 0:2f6112d75752 | 132 | */ |
| jojisdogakane | 4:a0150e0f285b | 133 | unsigned char HappyR[] = {255, 200, 100, 255, 200, 100}; |
| jojisdogakane | 4:a0150e0f285b | 134 | unsigned char HappyG[] = {100, 255, 200, 100, 255, 200}; |
| jojisdogakane | 4:a0150e0f285b | 135 | unsigned char HappyB[] = {100, 100, 255, 100, 100, 255}; |
| jojisdogakane | 4:a0150e0f285b | 136 | |
| jojisdogakane | 4:a0150e0f285b | 137 | unsigned char RelaxR[] = { 0, 100, 255, 0, 100, 255}; |
| jojisdogakane | 4:a0150e0f285b | 138 | unsigned char RelaxG[] = {255, 255, 255, 255, 255, 255}; |
| jojisdogakane | 4:a0150e0f285b | 139 | unsigned char RelaxB[] = { 0, 100, 255, 0, 100, 255}; |
| jojisdogakane | 0:2f6112d75752 | 140 | |
| jojisdogakane | 4:a0150e0f285b | 141 | unsigned char ExciteR[] = {255, 255, 255, 255, 255, 255}; |
| jojisdogakane | 4:a0150e0f285b | 142 | unsigned char ExciteG[] = {100, 100, 100, 100, 100, 100}; |
| jojisdogakane | 4:a0150e0f285b | 143 | unsigned char ExciteB[] = { 75, 50, 25, 25, 50, 75}; |
| jojisdogakane | 4:a0150e0f285b | 144 | /* |
| jojisdogakane | 4:a0150e0f285b | 145 | unsigned char StressR[] = {255, 255, 255, 255, 100, 100}; |
| jojisdogakane | 4:a0150e0f285b | 146 | unsigned char StressG[] = { 0, 0, 0, 0, 0, 0}; |
| jojisdogakane | 4:a0150e0f285b | 147 | unsigned char StressB[] = { 0, 0, 100, 100, 255, 255}; |
| jojisdogakane | 4:a0150e0f285b | 148 | */ |
| jojisdogakane | 4:a0150e0f285b | 149 | unsigned char StressR[] = {100, 100, 255, 255, 100, 100}; |
| jojisdogakane | 4:a0150e0f285b | 150 | unsigned char StressG[] = { 0, 0, 0, 0, 0, 0}; |
| jojisdogakane | 4:a0150e0f285b | 151 | unsigned char StressB[] = {255, 255, 100, 100, 255, 255}; |
| jojisdogakane | 4:a0150e0f285b | 152 | |
| jojisdogakane | 4:a0150e0f285b | 153 | unsigned char InterestR[] = {255, 255, 255, 255, 255, 255}; |
| jojisdogakane | 4:a0150e0f285b | 154 | unsigned char InterestG[] = {255, 255, 255, 255, 255, 255}; |
| jojisdogakane | 4:a0150e0f285b | 155 | unsigned char InterestB[] = {255, 100, 0, 0, 100, 255}; |
| jojisdogakane | 0:2f6112d75752 | 156 | |
| jojisdogakane | 0:2f6112d75752 | 157 | float rainbowOffset = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 158 | |
| jojisdogakane | 0:2f6112d75752 | 159 | bool bConcentration; |
| jojisdogakane | 0:2f6112d75752 | 160 | unsigned long loopCounter = 0; |
| jojisdogakane | 0:2f6112d75752 | 161 | int lightPos = 1; |
| jojisdogakane | 0:2f6112d75752 | 162 | int prevLightPos = 1; |
| jojisdogakane | 0:2f6112d75752 | 163 | |
| jojisdogakane | 0:2f6112d75752 | 164 | double relaxRatio = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 165 | float conRatio = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 166 | float conRatioNow = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 167 | double HR_ratio = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 168 | |
| jojisdogakane | 0:2f6112d75752 | 169 | unsigned long IBI; |
| jojisdogakane | 0:2f6112d75752 | 170 | |
| jojisdogakane | 0:2f6112d75752 | 171 | int x3, y3; |
| jojisdogakane | 0:2f6112d75752 | 172 | |
| jojisdogakane | 0:2f6112d75752 | 173 | int mainPixelCounter; |
| jojisdogakane | 0:2f6112d75752 | 174 | |
| jojisdogakane | 0:2f6112d75752 | 175 | int prevInterval; |
| jojisdogakane | 0:2f6112d75752 | 176 | int prevprevInterval; |
| jojisdogakane | 0:2f6112d75752 | 177 | |
| jojisdogakane | 0:2f6112d75752 | 178 | void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) |
| jojisdogakane | 0:2f6112d75752 | 179 | { |
| jojisdogakane | 0:2f6112d75752 | 180 | ble.gap().startAdvertising(); // restart advertising |
| jojisdogakane | 0:2f6112d75752 | 181 | } |
| jojisdogakane | 0:2f6112d75752 | 182 | /* |
| jojisdogakane | 0:2f6112d75752 | 183 | void ConnectTimeoutCallback(Gap::EventCallback_t timeoutCallback) |
| jojisdogakane | 0:2f6112d75752 | 184 | { |
| jojisdogakane | 0:2f6112d75752 | 185 | ble.gap().startAdvertising(); // restart advertising |
| jojisdogakane | 0:2f6112d75752 | 186 | } |
| jojisdogakane | 0:2f6112d75752 | 187 | */ |
| jojisdogakane | 0:2f6112d75752 | 188 | |
| jojisdogakane | 0:2f6112d75752 | 189 | uint8_t BPM = 0; |
| jojisdogakane | 0:2f6112d75752 | 190 | float sensorValue = 0; |
| jojisdogakane | 0:2f6112d75752 | 191 | Timer t; |
| jojisdogakane | 0:2f6112d75752 | 192 | Timer tRand; |
| jojisdogakane | 0:2f6112d75752 | 193 | |
| jojisdogakane | 0:2f6112d75752 | 194 | float redVal = 0; |
| jojisdogakane | 0:2f6112d75752 | 195 | float blueVal = 255; |
| jojisdogakane | 0:2f6112d75752 | 196 | float greenVal = 0; |
| jojisdogakane | 0:2f6112d75752 | 197 | |
| jojisdogakane | 0:2f6112d75752 | 198 | Ticker neopixelTick; |
| jojisdogakane | 0:2f6112d75752 | 199 | |
| jojisdogakane | 0:2f6112d75752 | 200 | UARTService *uartServicePtr; |
| jojisdogakane | 0:2f6112d75752 | 201 | |
| jojisdogakane | 0:2f6112d75752 | 202 | void neopixelTick_Handler() { |
| jojisdogakane | 0:2f6112d75752 | 203 | return; |
| jojisdogakane | 0:2f6112d75752 | 204 | } |
| jojisdogakane | 0:2f6112d75752 | 205 | |
| jojisdogakane | 4:a0150e0f285b | 206 | void SetMoodColor() { |
| jojisdogakane | 4:a0150e0f285b | 207 | int i = 0; |
| jojisdogakane | 4:a0150e0f285b | 208 | |
| jojisdogakane | 4:a0150e0f285b | 209 | for(i = 0; i < NEOPIXEL_COUNT; i++) { |
| jojisdogakane | 4:a0150e0f285b | 210 | MoodColorR[i][IS_RELAX] = RelaxR[i]; |
| jojisdogakane | 4:a0150e0f285b | 211 | MoodColorG[i][IS_RELAX] = RelaxG[i]; |
| jojisdogakane | 4:a0150e0f285b | 212 | MoodColorB[i][IS_RELAX] = RelaxB[i]; |
| jojisdogakane | 4:a0150e0f285b | 213 | } |
| jojisdogakane | 4:a0150e0f285b | 214 | for(i = 0; i < NEOPIXEL_COUNT; i++) { |
| jojisdogakane | 4:a0150e0f285b | 215 | MoodColorR[i][IS_EXCITED] = ExciteR[i]; |
| jojisdogakane | 4:a0150e0f285b | 216 | MoodColorG[i][IS_EXCITED] = ExciteG[i]; |
| jojisdogakane | 4:a0150e0f285b | 217 | MoodColorB[i][IS_EXCITED] = ExciteB[i]; |
| jojisdogakane | 4:a0150e0f285b | 218 | } |
| jojisdogakane | 4:a0150e0f285b | 219 | |
| jojisdogakane | 4:a0150e0f285b | 220 | for(i = 0; i < NEOPIXEL_COUNT; i++) { |
| jojisdogakane | 4:a0150e0f285b | 221 | MoodColorR[i][IS_HAPPY] = HappyR[i]; |
| jojisdogakane | 4:a0150e0f285b | 222 | MoodColorG[i][IS_HAPPY] = HappyG[i]; |
| jojisdogakane | 4:a0150e0f285b | 223 | MoodColorB[i][IS_HAPPY] = HappyB[i]; |
| jojisdogakane | 4:a0150e0f285b | 224 | } |
| jojisdogakane | 4:a0150e0f285b | 225 | for(i = 0; i < NEOPIXEL_COUNT; i++) { |
| jojisdogakane | 4:a0150e0f285b | 226 | MoodColorR[i][IS_INTERESTED] = InterestR[i]; |
| jojisdogakane | 4:a0150e0f285b | 227 | MoodColorG[i][IS_INTERESTED] = InterestG[i]; |
| jojisdogakane | 4:a0150e0f285b | 228 | MoodColorB[i][IS_INTERESTED] = InterestB[i]; |
| jojisdogakane | 4:a0150e0f285b | 229 | } |
| jojisdogakane | 4:a0150e0f285b | 230 | for(i = 0; i < NEOPIXEL_COUNT; i++) { |
| jojisdogakane | 4:a0150e0f285b | 231 | MoodColorR[i][IS_STRESSED] = StressR[i]; |
| jojisdogakane | 4:a0150e0f285b | 232 | MoodColorG[i][IS_STRESSED] = StressG[i]; |
| jojisdogakane | 4:a0150e0f285b | 233 | MoodColorB[i][IS_STRESSED] = StressB[i]; |
| jojisdogakane | 4:a0150e0f285b | 234 | } |
| jojisdogakane | 0:2f6112d75752 | 235 | } |
| jojisdogakane | 0:2f6112d75752 | 236 | |
| jojisdogakane | 4:a0150e0f285b | 237 | // 現在の気分の判定 |
| jojisdogakane | 4:a0150e0f285b | 238 | int getMood(int bpm, double relaxRatio, double happyRatio, double conRatio) { |
| jojisdogakane | 4:a0150e0f285b | 239 | // 第一優先：集中 |
| jojisdogakane | 4:a0150e0f285b | 240 | // 集中係数が0.95以上なら集中と判定 |
| jojisdogakane | 4:a0150e0f285b | 241 | if(conRatio >= CONMARGIN) |
| jojisdogakane | 4:a0150e0f285b | 242 | { |
| jojisdogakane | 4:a0150e0f285b | 243 | return IS_INTERESTED; |
| jojisdogakane | 4:a0150e0f285b | 244 | } |
| jojisdogakane | 0:2f6112d75752 | 245 | |
| jojisdogakane | 4:a0150e0f285b | 246 | // 第二優先：Stress |
| jojisdogakane | 4:a0150e0f285b | 247 | // リラックス係数が0.2以下 |
| jojisdogakane | 4:a0150e0f285b | 248 | if(relaxRatio <= RELAXMARGIN) { |
| jojisdogakane | 4:a0150e0f285b | 249 | return IS_STRESSED; |
| jojisdogakane | 4:a0150e0f285b | 250 | } |
| jojisdogakane | 0:2f6112d75752 | 251 | |
| jojisdogakane | 4:a0150e0f285b | 252 | // 第三優先：Happy |
| jojisdogakane | 4:a0150e0f285b | 253 | // Happy係数が0.2以上 |
| jojisdogakane | 4:a0150e0f285b | 254 | if(happyRatio >= HAPPYMARGIN && relaxRatio > 0.5) { |
| jojisdogakane | 4:a0150e0f285b | 255 | return IS_HAPPY; |
| jojisdogakane | 4:a0150e0f285b | 256 | } |
| jojisdogakane | 0:2f6112d75752 | 257 | |
| jojisdogakane | 4:a0150e0f285b | 258 | // それ以外は興奮度合い |
| jojisdogakane | 4:a0150e0f285b | 259 | // 心拍数の閾値判定 |
| jojisdogakane | 4:a0150e0f285b | 260 | int i = 0; |
| jojisdogakane | 4:a0150e0f285b | 261 | for(i = 0; i < HR_THRESHOLD; i++) { |
| jojisdogakane | 4:a0150e0f285b | 262 | if (BPM <= HRThreshold[i]) { |
| jojisdogakane | 0:2f6112d75752 | 263 | break; |
| jojisdogakane | 0:2f6112d75752 | 264 | } |
| jojisdogakane | 0:2f6112d75752 | 265 | } |
| jojisdogakane | 4:a0150e0f285b | 266 | switch(i) { |
| jojisdogakane | 4:a0150e0f285b | 267 | // 第１領域：NORMAL |
| jojisdogakane | 4:a0150e0f285b | 268 | case 0: |
| jojisdogakane | 4:a0150e0f285b | 269 | return IS_RELAX; |
| jojisdogakane | 4:a0150e0f285b | 270 | // 第２領域：EXCITED1 |
| jojisdogakane | 4:a0150e0f285b | 271 | case 1: |
| jojisdogakane | 4:a0150e0f285b | 272 | return IS_EXCITED; |
| jojisdogakane | 0:2f6112d75752 | 273 | } |
| jojisdogakane | 0:2f6112d75752 | 274 | |
| jojisdogakane | 4:a0150e0f285b | 275 | return IS_RELAX; |
| jojisdogakane | 4:a0150e0f285b | 276 | } |
| jojisdogakane | 0:2f6112d75752 | 277 | |
| jojisdogakane | 4:a0150e0f285b | 278 | //Serial mySerial(USBTX, USBRX); |
| jojisdogakane | 4:a0150e0f285b | 279 | //Serial mySerial(P0_9, P0_11); |
| jojisdogakane | 0:2f6112d75752 | 280 | |
| ayaando | 1:f28bdfaf3005 | 281 | // MACアドレスを取得してBLEのデバイス名に組み込む |
| ayaando | 1:f28bdfaf3005 | 282 | void setBleDeviceName(char *deviceName) |
| ayaando | 1:f28bdfaf3005 | 283 | { |
| ayaando | 1:f28bdfaf3005 | 284 | int i; |
| ayaando | 1:f28bdfaf3005 | 285 | |
| ayaando | 1:f28bdfaf3005 | 286 | //BLEProtocol::AddressType_t type; //< The type of the BLE address. |
| ayaando | 1:f28bdfaf3005 | 287 | //BLEProtocol::AddressBytes_t mac_address; //< The BLE address. // typedef uint8_t AddressBytes_t[ADDR_LEN] |
| ayaando | 1:f28bdfaf3005 | 288 | |
| ayaando | 1:f28bdfaf3005 | 289 | ble_gap_addr_t mac_address; |
| ayaando | 1:f28bdfaf3005 | 290 | |
| ayaando | 1:f28bdfaf3005 | 291 | if(sd_ble_gap_address_get(&mac_address) != NRF_SUCCESS){ |
| ayaando | 1:f28bdfaf3005 | 292 | return; |
| ayaando | 1:f28bdfaf3005 | 293 | } |
| ayaando | 1:f28bdfaf3005 | 294 | |
| ayaando | 1:f28bdfaf3005 | 295 | // ADDR_LENは6 |
| ayaando | 1:f28bdfaf3005 | 296 | for(i=BLE_GAP_ADDR_LEN-1; i>=0; i--){ |
| ayaando | 1:f28bdfaf3005 | 297 | sprintf(deviceName, "%s%02x", deviceName, (mac_address.addr)[i]); |
| ayaando | 1:f28bdfaf3005 | 298 | } |
| ayaando | 1:f28bdfaf3005 | 299 | sprintf(deviceName, "%s%s", deviceName, "\0"); |
| ayaando | 1:f28bdfaf3005 | 300 | |
| ayaando | 1:f28bdfaf3005 | 301 | /* |
| ayaando | 1:f28bdfaf3005 | 302 | if(ble.getAddress(&type, mac_address) == BLE_ERROR_NONE){ |
| ayaando | 1:f28bdfaf3005 | 303 | // ADDR_LENは6 |
| ayaando | 1:f28bdfaf3005 | 304 | for(i=BLEProtocol::ADDR_LEN-1; i>=0; i--){ |
| ayaando | 1:f28bdfaf3005 | 305 | sprintf(deviceName, "%s%02x", deviceName, mac_address[i]); |
| ayaando | 1:f28bdfaf3005 | 306 | } |
| ayaando | 1:f28bdfaf3005 | 307 | sprintf(deviceName, "%s%s", deviceName, "\0"); |
| ayaando | 1:f28bdfaf3005 | 308 | } |
| ayaando | 1:f28bdfaf3005 | 309 | */ |
| ayaando | 1:f28bdfaf3005 | 310 | } |
| jojisdogakane | 0:2f6112d75752 | 311 | |
| jojisdogakane | 0:2f6112d75752 | 312 | int main(void) |
| jojisdogakane | 0:2f6112d75752 | 313 | { |
| jojisdogakane | 0:2f6112d75752 | 314 | char msgIBI[100]; |
| jojisdogakane | 0:2f6112d75752 | 315 | int nMsgIBILen = 0; |
| jojisdogakane | 0:2f6112d75752 | 316 | char msgHappy[10]; |
| jojisdogakane | 0:2f6112d75752 | 317 | int nMsgHappyLen = 0; |
| jojisdogakane | 0:2f6112d75752 | 318 | char msgConcentration[10]; |
| jojisdogakane | 0:2f6112d75752 | 319 | int nMsgConcentrationLen = 0; |
| jojisdogakane | 0:2f6112d75752 | 320 | char msgRelax[10]; |
| jojisdogakane | 0:2f6112d75752 | 321 | int nMsgRelaxLen = 0; |
| jojisdogakane | 0:2f6112d75752 | 322 | |
| jojisdogakane | 0:2f6112d75752 | 323 | char msg[256]; |
| jojisdogakane | 0:2f6112d75752 | 324 | int nMsgLen = 0; |
| jojisdogakane | 0:2f6112d75752 | 325 | |
| jojisdogakane | 0:2f6112d75752 | 326 | float Brightness = 1.0; |
| jojisdogakane | 0:2f6112d75752 | 327 | bool brighten = true; |
| jojisdogakane | 0:2f6112d75752 | 328 | float brightenSpeedMs = 1000.0; |
| jojisdogakane | 0:2f6112d75752 | 329 | float colorSpeedMs = 1000.0; |
| jojisdogakane | 0:2f6112d75752 | 330 | float conRatioShow = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 331 | |
| jojisdogakane | 0:2f6112d75752 | 332 | int i; |
| jojisdogakane | 0:2f6112d75752 | 333 | unsigned long lightPrevMillis = 0; |
| jojisdogakane | 0:2f6112d75752 | 334 | |
| jojisdogakane | 0:2f6112d75752 | 335 | int currentBPM; |
| jojisdogakane | 0:2f6112d75752 | 336 | |
| jojisdogakane | 0:2f6112d75752 | 337 | float fBeatIndex = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 338 | |
| jojisdogakane | 0:2f6112d75752 | 339 | // displayFlag = SHOW_EXCITE; |
| jojisdogakane | 0:2f6112d75752 | 340 | |
| jojisdogakane | 0:2f6112d75752 | 341 | displayFlag = SHOW_EXCITE | SHOW_HAPPY | SHOW_CONCENTRATION; |
| jojisdogakane | 0:2f6112d75752 | 342 | //displayFlag = SHOW_HAPPY ; |
| jojisdogakane | 0:2f6112d75752 | 343 | |
| jojisdogakane | 0:2f6112d75752 | 344 | // mySerial.baud(9600); |
| jojisdogakane | 0:2f6112d75752 | 345 | // mySerial.format(8, Serial::None, 1); |
| jojisdogakane | 0:2f6112d75752 | 346 | |
| jojisdogakane | 0:2f6112d75752 | 347 | // PwrPin = 1; |
| jojisdogakane | 0:2f6112d75752 | 348 | |
| jojisdogakane | 0:2f6112d75752 | 349 | static uint8_t rrIntervalH = 0; |
| jojisdogakane | 0:2f6112d75752 | 350 | static uint8_t rrIntervalL = 1; |
| jojisdogakane | 0:2f6112d75752 | 351 | // static uint8_t bpm[8] = {0x10, hrmCounter, 0, 0, 0, 0, rrIntervalH, rrIntervalL}; |
| jojisdogakane | 0:2f6112d75752 | 352 | |
| jojisdogakane | 0:2f6112d75752 | 353 | neopixel_strip_t neopixel1; |
| jojisdogakane | 0:2f6112d75752 | 354 | neopixel_init(&neopixel1, P0_7, 1); |
| jojisdogakane | 0:2f6112d75752 | 355 | neopixel_clear(&neopixel1); |
| jojisdogakane | 0:2f6112d75752 | 356 | |
| jojisdogakane | 0:2f6112d75752 | 357 | neopixel_strip_t neopixel2; |
| jojisdogakane | 0:2f6112d75752 | 358 | neopixel_init(&neopixel2, P0_8, 1); |
| jojisdogakane | 0:2f6112d75752 | 359 | neopixel_clear(&neopixel2); |
| jojisdogakane | 0:2f6112d75752 | 360 | |
| jojisdogakane | 0:2f6112d75752 | 361 | neopixel_strip_t neopixel3; |
| jojisdogakane | 0:2f6112d75752 | 362 | neopixel_init(&neopixel3, P0_10, 1); |
| jojisdogakane | 0:2f6112d75752 | 363 | neopixel_clear(&neopixel3); |
| jojisdogakane | 0:2f6112d75752 | 364 | |
| jojisdogakane | 0:2f6112d75752 | 365 | neopixel_strip_t neopixel4; |
| jojisdogakane | 0:2f6112d75752 | 366 | neopixel_init(&neopixel4, P0_9, 1); |
| jojisdogakane | 0:2f6112d75752 | 367 | neopixel_clear(&neopixel4); |
| jojisdogakane | 0:2f6112d75752 | 368 | |
| jojisdogakane | 0:2f6112d75752 | 369 | neopixel_strip_t neopixel5; |
| jojisdogakane | 0:2f6112d75752 | 370 | neopixel_init(&neopixel5, P0_15, 1); |
| jojisdogakane | 0:2f6112d75752 | 371 | neopixel_clear(&neopixel5); |
| jojisdogakane | 0:2f6112d75752 | 372 | |
| jojisdogakane | 0:2f6112d75752 | 373 | neopixel_strip_t neopixel6; |
| jojisdogakane | 0:2f6112d75752 | 374 | neopixel_init(&neopixel6, P0_11, 1); |
| jojisdogakane | 0:2f6112d75752 | 375 | neopixel_clear(&neopixel6); |
| jojisdogakane | 0:2f6112d75752 | 376 | |
| jojisdogakane | 0:2f6112d75752 | 377 | neopixel_strip_t *pixels[6]; |
| jojisdogakane | 0:2f6112d75752 | 378 | |
| jojisdogakane | 0:2f6112d75752 | 379 | pixels[0] = &neopixel1; |
| jojisdogakane | 0:2f6112d75752 | 380 | pixels[1] = &neopixel2; |
| jojisdogakane | 0:2f6112d75752 | 381 | pixels[2] = &neopixel3; |
| jojisdogakane | 0:2f6112d75752 | 382 | pixels[3] = &neopixel4; |
| jojisdogakane | 0:2f6112d75752 | 383 | pixels[4] = &neopixel5; |
| jojisdogakane | 0:2f6112d75752 | 384 | pixels[5] = &neopixel6; |
| jojisdogakane | 0:2f6112d75752 | 385 | |
| jojisdogakane | 4:a0150e0f285b | 386 | |
| jojisdogakane | 4:a0150e0f285b | 387 | int pixelIndex = 0; |
| jojisdogakane | 4:a0150e0f285b | 388 | SetMoodColor(); |
| jojisdogakane | 0:2f6112d75752 | 389 | // my_analogin_init(); |
| jojisdogakane | 4:a0150e0f285b | 390 | |
| jojisdogakane | 4:a0150e0f285b | 391 | |
| jojisdogakane | 0:2f6112d75752 | 392 | ble.init(); |
| jojisdogakane | 0:2f6112d75752 | 393 | ble.gap().onDisconnection(disconnectionCallback); |
| jojisdogakane | 0:2f6112d75752 | 394 | |
| jojisdogakane | 4:a0150e0f285b | 395 | |
| jojisdogakane | 0:2f6112d75752 | 396 | // ble.gap().onTimeout(ConnectTimeoutCallback); |
| jojisdogakane | 0:2f6112d75752 | 397 | |
| jojisdogakane | 0:2f6112d75752 | 398 | UARTService uartService(ble); |
| jojisdogakane | 0:2f6112d75752 | 399 | uartServicePtr = &uartService; |
| jojisdogakane | 0:2f6112d75752 | 400 | |
| ayaando | 2:221df6d014f5 | 401 | // ★OTA用 |
| ayaando | 2:221df6d014f5 | 402 | dfuService = new DFUService(ble, NULL); |
| jojisdogakane | 4:a0150e0f285b | 403 | |
| jojisdogakane | 0:2f6112d75752 | 404 | /* Setup primary service. */ |
| jojisdogakane | 0:2f6112d75752 | 405 | HeartRateService hrServiceBPM(ble, BPM, HeartRateService::LOCATION_CHEST); |
| jojisdogakane | 0:2f6112d75752 | 406 | // HeartRateService hrServiceIBI(ble, IBI, HeartRateService::LOCATION_FINGER); |
| jojisdogakane | 0:2f6112d75752 | 407 | |
| jojisdogakane | 0:2f6112d75752 | 408 | // BatteryService batteryService(ble, 100); |
| jojisdogakane | 0:2f6112d75752 | 409 | |
| jojisdogakane | 0:2f6112d75752 | 410 | /* Setup auxiliary service. */ |
| ayaando | 2:221df6d014f5 | 411 | //DeviceInformationService deviceInfo(ble, "ARM", "Model1", "SN1", "hw-rev1", "fw-rev1", "soft-rev1"); |
| ayaando | 2:221df6d014f5 | 412 | // ★ |
| ayaando | 2:221df6d014f5 | 413 | DeviceInformationService deviceInfo(ble, "INUPATHY01", "0.0.1", "SN1", "0.0.1", "0.0.1", "0.0.1"); |
| jojisdogakane | 0:2f6112d75752 | 414 | |
| ayaando | 1:f28bdfaf3005 | 415 | // MACアドレスをDEVICE_NAMEに組み込む |
| ayaando | 1:f28bdfaf3005 | 416 | setBleDeviceName(DEVICE_NAME); |
| ayaando | 1:f28bdfaf3005 | 417 | |
| jojisdogakane | 0:2f6112d75752 | 418 | /* Setup advertising. */ |
| jojisdogakane | 0:2f6112d75752 | 419 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); |
| jojisdogakane | 0:2f6112d75752 | 420 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list)); |
| jojisdogakane | 0:2f6112d75752 | 421 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR); |
| jojisdogakane | 0:2f6112d75752 | 422 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); |
| jojisdogakane | 0:2f6112d75752 | 423 | ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
| jojisdogakane | 0:2f6112d75752 | 424 | ble.gap().setAdvertisingInterval(800); /* 1000ms */ |
| jojisdogakane | 0:2f6112d75752 | 425 | ble.gap().startAdvertising(); |
| jojisdogakane | 4:a0150e0f285b | 426 | |
| jojisdogakane | 0:2f6112d75752 | 427 | |
| jojisdogakane | 4:a0150e0f285b | 428 | for( i = 0; i < NEOPIXEL_COUNT; i++) |
| jojisdogakane | 0:2f6112d75752 | 429 | { |
| jojisdogakane | 4:a0150e0f285b | 430 | // ColorArrayR[i] = RainbowR[i]; |
| jojisdogakane | 4:a0150e0f285b | 431 | // ColorArrayG[i] = RainbowG[i]; |
| jojisdogakane | 4:a0150e0f285b | 432 | // ColorArrayB[i] = RainbowB[i]; |
| jojisdogakane | 4:a0150e0f285b | 433 | ColorArrayR[i] = StressR[i]; |
| jojisdogakane | 4:a0150e0f285b | 434 | ColorArrayG[i] = StressG[i]; |
| jojisdogakane | 4:a0150e0f285b | 435 | ColorArrayB[i] = StressB[i]; |
| jojisdogakane | 4:a0150e0f285b | 436 | TargetColorArrayR[i] = ColorArrayR[i]; |
| jojisdogakane | 4:a0150e0f285b | 437 | TargetColorArrayG[i] = ColorArrayG[i]; |
| jojisdogakane | 4:a0150e0f285b | 438 | TargetColorArrayB[i] = ColorArrayB[i]; |
| jojisdogakane | 4:a0150e0f285b | 439 | |
| jojisdogakane | 0:2f6112d75752 | 440 | neopixel_set_color( |
| jojisdogakane | 0:2f6112d75752 | 441 | pixels[i], |
| jojisdogakane | 0:2f6112d75752 | 442 | 0, |
| jojisdogakane | 4:a0150e0f285b | 443 | short(float(ColorArrayR[i] * DIM_LED)), |
| jojisdogakane | 4:a0150e0f285b | 444 | short(float(ColorArrayG[i] * DIM_LED)), |
| jojisdogakane | 4:a0150e0f285b | 445 | short(float(ColorArrayB[i] * DIM_LED))); |
| jojisdogakane | 0:2f6112d75752 | 446 | neopixel_show(pixels[i]); |
| jojisdogakane | 0:2f6112d75752 | 447 | } |
| jojisdogakane | 4:a0150e0f285b | 448 | |
| jojisdogakane | 0:2f6112d75752 | 449 | t.start(); |
| jojisdogakane | 0:2f6112d75752 | 450 | |
| jojisdogakane | 0:2f6112d75752 | 451 | prevmillis = t.read_ms(); |
| jojisdogakane | 0:2f6112d75752 | 452 | |
| jojisdogakane | 0:2f6112d75752 | 453 | bool bPulseEvent = false; |
| jojisdogakane | 0:2f6112d75752 | 454 | |
| jojisdogakane | 0:2f6112d75752 | 455 | float degPoint = 0; |
| jojisdogakane | 0:2f6112d75752 | 456 | float degRatio = 0; |
| jojisdogakane | 0:2f6112d75752 | 457 | |
| jojisdogakane | 0:2f6112d75752 | 458 | bool bSwingR = false; |
| jojisdogakane | 0:2f6112d75752 | 459 | |
| jojisdogakane | 0:2f6112d75752 | 460 | float swingIndex = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 461 | |
| jojisdogakane | 0:2f6112d75752 | 462 | float midPoint = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 463 | |
| jojisdogakane | 0:2f6112d75752 | 464 | // neopixelTick.attach(neopixelTick_Handler, 0.005); |
| jojisdogakane | 0:2f6112d75752 | 465 | |
| jojisdogakane | 0:2f6112d75752 | 466 | while (1) { |
| jojisdogakane | 0:2f6112d75752 | 467 | |
| jojisdogakane | 0:2f6112d75752 | 468 | // batteryService.updateBatteryLevel( (uint8_t)read_battery_level()); |
| jojisdogakane | 0:2f6112d75752 | 469 | |
| jojisdogakane | 0:2f6112d75752 | 470 | float currentValue = HRSigIn; |
| jojisdogakane | 0:2f6112d75752 | 471 | |
| jojisdogakane | 0:2f6112d75752 | 472 | bPulseEvent = false; |
| jojisdogakane | 0:2f6112d75752 | 473 | |
| jojisdogakane | 4:a0150e0f285b | 474 | if ((currentValue <= 0.3) && (sensorValue > 0.3)) |
| jojisdogakane | 0:2f6112d75752 | 475 | { |
| jojisdogakane | 0:2f6112d75752 | 476 | bPulseEvent = true; |
| jojisdogakane | 0:2f6112d75752 | 477 | } |
| jojisdogakane | 0:2f6112d75752 | 478 | |
| jojisdogakane | 0:2f6112d75752 | 479 | int sens = 1023 * currentValue; |
| jojisdogakane | 0:2f6112d75752 | 480 | |
| jojisdogakane | 0:2f6112d75752 | 481 | // printf("S%d", sens); |
| jojisdogakane | 0:2f6112d75752 | 482 | // printf("S%d\n", sens); |
| jojisdogakane | 0:2f6112d75752 | 483 | |
| jojisdogakane | 0:2f6112d75752 | 484 | sensorValue = currentValue; |
| jojisdogakane | 0:2f6112d75752 | 485 | |
| jojisdogakane | 0:2f6112d75752 | 486 | if(bPulseEvent) { |
| jojisdogakane | 0:2f6112d75752 | 487 | fBeatIndex = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 488 | // batteryService.updateBatteryLevel( (uint8_t)read_battery_level()); |
| jojisdogakane | 0:2f6112d75752 | 489 | |
| jojisdogakane | 0:2f6112d75752 | 490 | // 現在の時刻を取得 |
| jojisdogakane | 0:2f6112d75752 | 491 | // t.stop(); // stop, startは時間計測の結果を有意に短くしてしまうので使わない |
| jojisdogakane | 0:2f6112d75752 | 492 | currentmillis = t.read_ms(); |
| jojisdogakane | 0:2f6112d75752 | 493 | // t.start(); |
| jojisdogakane | 0:2f6112d75752 | 494 | // 現在時刻から過去時刻を引き、間隔を求める |
| jojisdogakane | 0:2f6112d75752 | 495 | intervalmillis = currentmillis - prevmillis; |
| jojisdogakane | 0:2f6112d75752 | 496 | |
| jojisdogakane | 0:2f6112d75752 | 497 | // 心拍の間の間隔 |
| jojisdogakane | 0:2f6112d75752 | 498 | IBI = intervalmillis; |
| jojisdogakane | 0:2f6112d75752 | 499 | |
| jojisdogakane | 0:2f6112d75752 | 500 | // 現在の時刻を過去時刻として記録 |
| jojisdogakane | 0:2f6112d75752 | 501 | prevmillis = currentmillis; |
| jojisdogakane | 0:2f6112d75752 | 502 | |
| jojisdogakane | 0:2f6112d75752 | 503 | // 平均値算出用配列に値を詰める |
| jojisdogakane | 0:2f6112d75752 | 504 | double averageIntervalMillis; |
| jojisdogakane | 0:2f6112d75752 | 505 | |
| jojisdogakane | 0:2f6112d75752 | 506 | for(i = 0; i < MAXARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 507 | { |
| jojisdogakane | 0:2f6112d75752 | 508 | // 最後尾以外の場合 |
| jojisdogakane | 0:2f6112d75752 | 509 | if(i != MAXARRAY - 1) |
| jojisdogakane | 0:2f6112d75752 | 510 | { |
| jojisdogakane | 0:2f6112d75752 | 511 | // 次の要素の値をコピーする |
| jojisdogakane | 0:2f6112d75752 | 512 | intervalmillisArray[i] = intervalmillisArray[i + 1]; |
| jojisdogakane | 0:2f6112d75752 | 513 | } |
| jojisdogakane | 0:2f6112d75752 | 514 | // 最後尾の場合 |
| jojisdogakane | 0:2f6112d75752 | 515 | else { |
| jojisdogakane | 0:2f6112d75752 | 516 | // 最新値を詰める |
| jojisdogakane | 0:2f6112d75752 | 517 | intervalmillisArray[i] = intervalmillis; |
| jojisdogakane | 0:2f6112d75752 | 518 | } |
| jojisdogakane | 0:2f6112d75752 | 519 | } |
| jojisdogakane | 0:2f6112d75752 | 520 | // 先頭が0の間は何もしない |
| jojisdogakane | 0:2f6112d75752 | 521 | unsigned long sumIntervalMillis = 0; |
| jojisdogakane | 0:2f6112d75752 | 522 | unsigned long sumIntervalMillisV = 0; |
| jojisdogakane | 0:2f6112d75752 | 523 | if (intervalmillisArray[0] != 0) |
| jojisdogakane | 0:2f6112d75752 | 524 | { |
| jojisdogakane | 0:2f6112d75752 | 525 | // 平均値算出 |
| jojisdogakane | 0:2f6112d75752 | 526 | averageIntervalMillis = 0; |
| jojisdogakane | 0:2f6112d75752 | 527 | for(i = 0; i < MAXARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 528 | { |
| jojisdogakane | 0:2f6112d75752 | 529 | sumIntervalMillis += intervalmillisArray[i]; |
| jojisdogakane | 0:2f6112d75752 | 530 | sumIntervalMillisV += intervalmillisArray[i] * intervalmillisArray[i]; |
| jojisdogakane | 0:2f6112d75752 | 531 | } |
| jojisdogakane | 0:2f6112d75752 | 532 | double squ; |
| jojisdogakane | 0:2f6112d75752 | 533 | double var; |
| jojisdogakane | 0:2f6112d75752 | 534 | double stdDev; |
| jojisdogakane | 0:2f6112d75752 | 535 | |
| jojisdogakane | 0:2f6112d75752 | 536 | averageIntervalMillis = (double)sumIntervalMillis / (double)MAXARRAY; |
| jojisdogakane | 0:2f6112d75752 | 537 | |
| jojisdogakane | 0:2f6112d75752 | 538 | squ = sumIntervalMillisV - (double)(sumIntervalMillis * sumIntervalMillis) / (double)MAXARRAY; |
| jojisdogakane | 0:2f6112d75752 | 539 | var = squ / (double)(MAXARRAY - 1); |
| jojisdogakane | 0:2f6112d75752 | 540 | stdDev = sqrt(var); |
| jojisdogakane | 0:2f6112d75752 | 541 | |
| jojisdogakane | 0:2f6112d75752 | 542 | BPM = min32_of(60000 / averageIntervalMillis, 255); |
| jojisdogakane | 0:2f6112d75752 | 543 | // if(intervalmillis < averageIntervalMillis - stdDev) { // || intervalmillis > averageIntervalMillis + stdDev) { |
| jojisdogakane | 0:2f6112d75752 | 544 | |
| jojisdogakane | 0:2f6112d75752 | 545 | currentBPM = min32_of(60000 / intervalmillis, 255); |
| jojisdogakane | 0:2f6112d75752 | 546 | /* } |
| jojisdogakane | 0:2f6112d75752 | 547 | else { |
| jojisdogakane | 0:2f6112d75752 | 548 | currentBPM = BPM; |
| jojisdogakane | 0:2f6112d75752 | 549 | } */ |
| jojisdogakane | 0:2f6112d75752 | 550 | } |
| jojisdogakane | 0:2f6112d75752 | 551 | /* |
| jojisdogakane | 0:2f6112d75752 | 552 | int IBI1024 = IBI * (1024/1000); |
| jojisdogakane | 0:2f6112d75752 | 553 | |
| jojisdogakane | 0:2f6112d75752 | 554 | bpm[1] = BPM; |
| jojisdogakane | 0:2f6112d75752 | 555 | bpm[6] = (IBI1024 & 0xFF00) >> 8; |
| jojisdogakane | 0:2f6112d75752 | 556 | bpm[7] = (IBI1024 & 0xFF); |
| jojisdogakane | 0:2f6112d75752 | 557 | */ |
| jojisdogakane | 0:2f6112d75752 | 558 | // update bpm |
| jojisdogakane | 0:2f6112d75752 | 559 | if(ble.gap().getState().connected == 1) |
| jojisdogakane | 0:2f6112d75752 | 560 | { |
| jojisdogakane | 0:2f6112d75752 | 561 | hrServiceBPM.updateHeartRate(BPM); |
| jojisdogakane | 0:2f6112d75752 | 562 | // hrServiceIBI.updateHeartRate(IBI); |
| jojisdogakane | 0:2f6112d75752 | 563 | } |
| jojisdogakane | 0:2f6112d75752 | 564 | |
| jojisdogakane | 0:2f6112d75752 | 565 | // printf("B%d\n", BPM); |
| jojisdogakane | 0:2f6112d75752 | 566 | // printf("Q%d\n", IBI); |
| jojisdogakane | 0:2f6112d75752 | 567 | |
| jojisdogakane | 0:2f6112d75752 | 568 | // nMsgLen = sprintf(msg,"B%d\0", BPM); |
| jojisdogakane | 0:2f6112d75752 | 569 | // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); |
| jojisdogakane | 0:2f6112d75752 | 570 | |
| jojisdogakane | 0:2f6112d75752 | 571 | nMsgIBILen = sprintf(msgIBI,"Q%d\0", IBI); |
| jojisdogakane | 0:2f6112d75752 | 572 | // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); |
| jojisdogakane | 0:2f6112d75752 | 573 | |
| jojisdogakane | 0:2f6112d75752 | 574 | // 平均値算出用配列に値を詰める |
| jojisdogakane | 0:2f6112d75752 | 575 | for(i = 0; i < MAXHRVARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 576 | { |
| jojisdogakane | 0:2f6112d75752 | 577 | // 最後尾以外の場合 |
| jojisdogakane | 0:2f6112d75752 | 578 | if(i != MAXHRVARRAY - 1) |
| jojisdogakane | 0:2f6112d75752 | 579 | { |
| jojisdogakane | 0:2f6112d75752 | 580 | // 次の要素の値をコピーする |
| jojisdogakane | 0:2f6112d75752 | 581 | intervalmillisHRVArray[i] = intervalmillisHRVArray[i + 1]; |
| jojisdogakane | 0:2f6112d75752 | 582 | } |
| jojisdogakane | 0:2f6112d75752 | 583 | // 最後尾の場合 |
| jojisdogakane | 0:2f6112d75752 | 584 | else { |
| jojisdogakane | 0:2f6112d75752 | 585 | // 最新値を詰める |
| jojisdogakane | 0:2f6112d75752 | 586 | intervalmillisHRVArray[i] = intervalmillis; |
| jojisdogakane | 0:2f6112d75752 | 587 | } |
| jojisdogakane | 0:2f6112d75752 | 588 | } |
| jojisdogakane | 0:2f6112d75752 | 589 | |
| jojisdogakane | 0:2f6112d75752 | 590 | // 平均値算出 |
| jojisdogakane | 0:2f6112d75752 | 591 | int nNNInterval; |
| jojisdogakane | 0:2f6112d75752 | 592 | unsigned long ulIntervalSum = 0; |
| jojisdogakane | 0:2f6112d75752 | 593 | int nSampleCount = 0; |
| jojisdogakane | 0:2f6112d75752 | 594 | for(i = 0; i < MAXHRVARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 595 | { |
| jojisdogakane | 0:2f6112d75752 | 596 | if(intervalmillisHRVArray[i] > 0) |
| jojisdogakane | 0:2f6112d75752 | 597 | { |
| jojisdogakane | 0:2f6112d75752 | 598 | ulIntervalSum += intervalmillisHRVArray[i]; |
| jojisdogakane | 0:2f6112d75752 | 599 | nSampleCount++; |
| jojisdogakane | 0:2f6112d75752 | 600 | } |
| jojisdogakane | 0:2f6112d75752 | 601 | } |
| jojisdogakane | 0:2f6112d75752 | 602 | |
| jojisdogakane | 0:2f6112d75752 | 603 | float IntervalAverage = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 604 | if(nSampleCount > 0){ |
| jojisdogakane | 0:2f6112d75752 | 605 | IntervalAverage = (float)ulIntervalSum / (float)nSampleCount; |
| jojisdogakane | 0:2f6112d75752 | 606 | } |
| jojisdogakane | 0:2f6112d75752 | 607 | |
| jojisdogakane | 0:2f6112d75752 | 608 | nNNInterval = (int)((float)IntervalAverage * PNNX_FACTOR); |
| jojisdogakane | 0:2f6112d75752 | 609 | |
| jojisdogakane | 0:2f6112d75752 | 610 | |
| jojisdogakane | 0:2f6112d75752 | 611 | bool bContinue = false; |
| jojisdogakane | 0:2f6112d75752 | 612 | int nNNxCount = 0; |
| jojisdogakane | 0:2f6112d75752 | 613 | for(i = 0; i < MAXHRVARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 614 | { |
| jojisdogakane | 0:2f6112d75752 | 615 | // if not end |
| jojisdogakane | 0:2f6112d75752 | 616 | if(i < MAXHRVARRAY - 1) { |
| jojisdogakane | 0:2f6112d75752 | 617 | if(abs((int)(intervalmillisHRVArray[i] - intervalmillisHRVArray[i + 1])) >= nNNInterval) |
| jojisdogakane | 0:2f6112d75752 | 618 | { |
| jojisdogakane | 0:2f6112d75752 | 619 | if(bContinue == false) |
| jojisdogakane | 0:2f6112d75752 | 620 | { |
| jojisdogakane | 0:2f6112d75752 | 621 | nNNxCount++; |
| jojisdogakane | 0:2f6112d75752 | 622 | } |
| jojisdogakane | 0:2f6112d75752 | 623 | nNNxCount++; |
| jojisdogakane | 0:2f6112d75752 | 624 | bContinue = true; |
| jojisdogakane | 0:2f6112d75752 | 625 | } |
| jojisdogakane | 0:2f6112d75752 | 626 | else { |
| jojisdogakane | 0:2f6112d75752 | 627 | bContinue = false; |
| jojisdogakane | 0:2f6112d75752 | 628 | } |
| jojisdogakane | 0:2f6112d75752 | 629 | } |
| jojisdogakane | 0:2f6112d75752 | 630 | } |
| jojisdogakane | 0:2f6112d75752 | 631 | |
| jojisdogakane | 0:2f6112d75752 | 632 | float diffRatio = (float)nNNxCount / (float)nSampleCount; |
| jojisdogakane | 0:2f6112d75752 | 633 | |
| jojisdogakane | 0:2f6112d75752 | 634 | if(diffRatio > 1) |
| jojisdogakane | 0:2f6112d75752 | 635 | { |
| jojisdogakane | 0:2f6112d75752 | 636 | diffRatio = 1; |
| jojisdogakane | 0:2f6112d75752 | 637 | } |
| jojisdogakane | 0:2f6112d75752 | 638 | |
| jojisdogakane | 0:2f6112d75752 | 639 | // リラックスの係数を算出する |
| jojisdogakane | 0:2f6112d75752 | 640 | relaxRatio = diffRatio; |
| jojisdogakane | 0:2f6112d75752 | 641 | |
| jojisdogakane | 0:2f6112d75752 | 642 | nMsgRelaxLen = sprintf(msgRelax,"R%0.2f\0", relaxRatio); |
| jojisdogakane | 0:2f6112d75752 | 643 | |
| jojisdogakane | 0:2f6112d75752 | 644 | // nMsgLen = sprintf(msg,"R%0.2f\0", relaxRatio); |
| jojisdogakane | 0:2f6112d75752 | 645 | // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); |
| jojisdogakane | 0:2f6112d75752 | 646 | |
| jojisdogakane | 0:2f6112d75752 | 647 | // 現在値から最低値を引く |
| jojisdogakane | 0:2f6112d75752 | 648 | // relaxRatio = relaxRatio - RELAX_RATIO_MIN; |
| jojisdogakane | 0:2f6112d75752 | 649 | |
| jojisdogakane | 0:2f6112d75752 | 650 | // 1.0 / 最低値 × 現在値 |
| jojisdogakane | 0:2f6112d75752 | 651 | // relaxRatio = relaxRatio * (1.0 / (1.0 - RELAX_RATIO_MIN)); |
| jojisdogakane | 0:2f6112d75752 | 652 | |
| jojisdogakane | 0:2f6112d75752 | 653 | int x1, y1, x2, y2; |
| jojisdogakane | 0:2f6112d75752 | 654 | int tempx1, tempy1, tempx2, tempy2, tempx3, tempy3; |
| jojisdogakane | 0:2f6112d75752 | 655 | int longx, shortx, longy, shorty; |
| jojisdogakane | 0:2f6112d75752 | 656 | long xx1, xx2, yy1, yy2; |
| jojisdogakane | 0:2f6112d75752 | 657 | |
| jojisdogakane | 0:2f6112d75752 | 658 | int movex, movey; |
| jojisdogakane | 0:2f6112d75752 | 659 | |
| jojisdogakane | 0:2f6112d75752 | 660 | double dist1, dist2; |
| jojisdogakane | 0:2f6112d75752 | 661 | double conRatioRatio; |
| jojisdogakane | 0:2f6112d75752 | 662 | |
| jojisdogakane | 0:2f6112d75752 | 663 | /* |
| jojisdogakane | 0:2f6112d75752 | 664 | // 最後尾の3データから二つのx, y地点を作る。 |
| jojisdogakane | 0:2f6112d75752 | 665 | x1 = intervalmillisArray[nSampleCount - 1]; |
| jojisdogakane | 0:2f6112d75752 | 666 | y1 = intervalmillisArray[nSampleCount - 2]; |
| jojisdogakane | 0:2f6112d75752 | 667 | x2 = intervalmillisArray[nSampleCount - 2]; |
| jojisdogakane | 0:2f6112d75752 | 668 | y2 = intervalmillisArray[nSampleCount - 3]; |
| jojisdogakane | 0:2f6112d75752 | 669 | */ |
| jojisdogakane | 0:2f6112d75752 | 670 | |
| jojisdogakane | 0:2f6112d75752 | 671 | x1 = intervalmillis; |
| jojisdogakane | 0:2f6112d75752 | 672 | y1 = prevInterval; |
| jojisdogakane | 0:2f6112d75752 | 673 | x2 = prevInterval; |
| jojisdogakane | 0:2f6112d75752 | 674 | y2 = prevprevInterval; |
| jojisdogakane | 0:2f6112d75752 | 675 | |
| jojisdogakane | 0:2f6112d75752 | 676 | // ２地点を結ぶ直線の長さを算出する。 |
| jojisdogakane | 0:2f6112d75752 | 677 | if(x1 > x2) |
| jojisdogakane | 0:2f6112d75752 | 678 | { |
| jojisdogakane | 0:2f6112d75752 | 679 | longx = x1; |
| jojisdogakane | 0:2f6112d75752 | 680 | shortx = x2; |
| jojisdogakane | 0:2f6112d75752 | 681 | } |
| jojisdogakane | 0:2f6112d75752 | 682 | else { |
| jojisdogakane | 0:2f6112d75752 | 683 | longx = x2; |
| jojisdogakane | 0:2f6112d75752 | 684 | shortx = x1; |
| jojisdogakane | 0:2f6112d75752 | 685 | } |
| jojisdogakane | 0:2f6112d75752 | 686 | |
| jojisdogakane | 0:2f6112d75752 | 687 | if(y1 > y2) |
| jojisdogakane | 0:2f6112d75752 | 688 | { |
| jojisdogakane | 0:2f6112d75752 | 689 | longy = y1; |
| jojisdogakane | 0:2f6112d75752 | 690 | shorty = y2; |
| jojisdogakane | 0:2f6112d75752 | 691 | } |
| jojisdogakane | 0:2f6112d75752 | 692 | else { |
| jojisdogakane | 0:2f6112d75752 | 693 | longy = y2; |
| jojisdogakane | 0:2f6112d75752 | 694 | shorty = y1; |
| jojisdogakane | 0:2f6112d75752 | 695 | } |
| jojisdogakane | 0:2f6112d75752 | 696 | |
| jojisdogakane | 0:2f6112d75752 | 697 | xx1 = longx - shortx; |
| jojisdogakane | 0:2f6112d75752 | 698 | yy1 = longy - shorty; |
| jojisdogakane | 0:2f6112d75752 | 699 | |
| jojisdogakane | 0:2f6112d75752 | 700 | dist1 = sqrt((double)((xx1 * xx1) + (yy1 * yy1))); |
| jojisdogakane | 0:2f6112d75752 | 701 | |
| jojisdogakane | 0:2f6112d75752 | 702 | dist2 = longx + longy; |
| jojisdogakane | 0:2f6112d75752 | 703 | float distRatio = dist1 / dist2; |
| jojisdogakane | 0:2f6112d75752 | 704 | |
| jojisdogakane | 0:2f6112d75752 | 705 | conRatioNow = 1 - distRatio; |
| jojisdogakane | 0:2f6112d75752 | 706 | |
| jojisdogakane | 0:2f6112d75752 | 707 | // 平均値算出用配列に値を詰める |
| jojisdogakane | 0:2f6112d75752 | 708 | float averageConRatio = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 709 | for(i = 0; i < MAXCONARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 710 | { |
| jojisdogakane | 0:2f6112d75752 | 711 | // 最後尾以外の場合 |
| jojisdogakane | 0:2f6112d75752 | 712 | if(i != MAXCONARRAY - 1) |
| jojisdogakane | 0:2f6112d75752 | 713 | { |
| jojisdogakane | 0:2f6112d75752 | 714 | // 次の要素の値をコピーする |
| jojisdogakane | 0:2f6112d75752 | 715 | conArray[i] = conArray[i + 1]; |
| jojisdogakane | 0:2f6112d75752 | 716 | } |
| jojisdogakane | 0:2f6112d75752 | 717 | // 最後尾の場合 |
| jojisdogakane | 0:2f6112d75752 | 718 | else { |
| jojisdogakane | 0:2f6112d75752 | 719 | // 最新値を詰める |
| jojisdogakane | 0:2f6112d75752 | 720 | conArray[i] = distRatio; |
| jojisdogakane | 0:2f6112d75752 | 721 | } |
| jojisdogakane | 0:2f6112d75752 | 722 | } |
| jojisdogakane | 0:2f6112d75752 | 723 | // 先頭が0の間は何もしない |
| jojisdogakane | 0:2f6112d75752 | 724 | if (conArray[0] != 0) |
| jojisdogakane | 0:2f6112d75752 | 725 | { |
| jojisdogakane | 0:2f6112d75752 | 726 | // 平均値算出 |
| jojisdogakane | 0:2f6112d75752 | 727 | averageConRatio = 0; |
| jojisdogakane | 0:2f6112d75752 | 728 | for(i = 0; i < MAXCONARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 729 | { |
| jojisdogakane | 0:2f6112d75752 | 730 | averageConRatio += conArray[i]; |
| jojisdogakane | 0:2f6112d75752 | 731 | } |
| jojisdogakane | 0:2f6112d75752 | 732 | averageConRatio = averageConRatio / (double)MAXCONARRAY; |
| jojisdogakane | 0:2f6112d75752 | 733 | } |
| jojisdogakane | 0:2f6112d75752 | 734 | /* |
| jojisdogakane | 0:2f6112d75752 | 735 | float averageStressRatio = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 736 | for(i = 0; i < MAXSTRESSARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 737 | { |
| jojisdogakane | 0:2f6112d75752 | 738 | // 最後尾以外の場合 |
| jojisdogakane | 0:2f6112d75752 | 739 | if(i != MAXSTRESSARRAY - 1) |
| jojisdogakane | 0:2f6112d75752 | 740 | { |
| jojisdogakane | 0:2f6112d75752 | 741 | // 次の要素の値をコピーする |
| jojisdogakane | 0:2f6112d75752 | 742 | stressArray[i] = stressArray[i + 1]; |
| jojisdogakane | 0:2f6112d75752 | 743 | } |
| jojisdogakane | 0:2f6112d75752 | 744 | // 最後尾の場合 |
| jojisdogakane | 0:2f6112d75752 | 745 | else { |
| jojisdogakane | 0:2f6112d75752 | 746 | // 最新値を詰める |
| jojisdogakane | 0:2f6112d75752 | 747 | if(x1 > y1) { |
| jojisdogakane | 0:2f6112d75752 | 748 | stressArray[i] = y1 / x1; |
| jojisdogakane | 0:2f6112d75752 | 749 | } |
| jojisdogakane | 0:2f6112d75752 | 750 | else { |
| jojisdogakane | 0:2f6112d75752 | 751 | stressArray[i] = x1 / y1; |
| jojisdogakane | 0:2f6112d75752 | 752 | } |
| jojisdogakane | 0:2f6112d75752 | 753 | } |
| jojisdogakane | 0:2f6112d75752 | 754 | } |
| jojisdogakane | 0:2f6112d75752 | 755 | // 先頭が0の間は何もしない |
| jojisdogakane | 0:2f6112d75752 | 756 | if (stressArray[0] != 0) |
| jojisdogakane | 0:2f6112d75752 | 757 | { |
| jojisdogakane | 0:2f6112d75752 | 758 | // 平均値算出 |
| jojisdogakane | 0:2f6112d75752 | 759 | averageStressRatio = 0; |
| jojisdogakane | 0:2f6112d75752 | 760 | for(i = 0; i < MAXSTRESSARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 761 | { |
| jojisdogakane | 0:2f6112d75752 | 762 | averageStressRatio += stressArray[i]; |
| jojisdogakane | 0:2f6112d75752 | 763 | } |
| jojisdogakane | 0:2f6112d75752 | 764 | averageStressRatio = averageStressRatio / (double)MAXSTRESSARRAY; |
| jojisdogakane | 0:2f6112d75752 | 765 | } |
| jojisdogakane | 0:2f6112d75752 | 766 | */ |
| jojisdogakane | 0:2f6112d75752 | 767 | |
| jojisdogakane | 0:2f6112d75752 | 768 | /* float rcFilter = RCFILTER2; |
| jojisdogakane | 0:2f6112d75752 | 769 | float filteredDistRatio = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 770 | float a; |
| jojisdogakane | 0:2f6112d75752 | 771 | if(prevFilteredDistRatio != 0) { |
| jojisdogakane | 0:2f6112d75752 | 772 | a = rcFilter * prevFilteredDistRatio; |
| jojisdogakane | 0:2f6112d75752 | 773 | filteredDistRatio = a + (1 - rcFilter) * distRatio; |
| jojisdogakane | 0:2f6112d75752 | 774 | } |
| jojisdogakane | 0:2f6112d75752 | 775 | else { |
| jojisdogakane | 0:2f6112d75752 | 776 | filteredDistRatio = (1 - rcFilter) * distRatio; |
| jojisdogakane | 0:2f6112d75752 | 777 | } |
| jojisdogakane | 0:2f6112d75752 | 778 | prevFilteredDistRatio = filteredDistRatio ; |
| jojisdogakane | 0:2f6112d75752 | 779 | |
| jojisdogakane | 0:2f6112d75752 | 780 | conRatio = filteredDistRatio ; |
| jojisdogakane | 0:2f6112d75752 | 781 | */ |
| jojisdogakane | 0:2f6112d75752 | 782 | conRatio = averageConRatio; |
| jojisdogakane | 0:2f6112d75752 | 783 | conRatio = 1 - conRatio; |
| jojisdogakane | 0:2f6112d75752 | 784 | |
| jojisdogakane | 0:2f6112d75752 | 785 | nMsgConcentrationLen = sprintf(msgConcentration,"C%0.2f\0", conRatio); |
| jojisdogakane | 0:2f6112d75752 | 786 | |
| jojisdogakane | 0:2f6112d75752 | 787 | // nMsgLen = sprintf(msg,"C%0.2f\0", conRatio); |
| jojisdogakane | 0:2f6112d75752 | 788 | // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); |
| jojisdogakane | 0:2f6112d75752 | 789 | |
| jojisdogakane | 0:2f6112d75752 | 790 | |
| jojisdogakane | 0:2f6112d75752 | 791 | prevprevInterval = prevInterval; |
| jojisdogakane | 0:2f6112d75752 | 792 | prevInterval = intervalmillis; |
| jojisdogakane | 0:2f6112d75752 | 793 | |
| jojisdogakane | 0:2f6112d75752 | 794 | // (x2, y2)が原点となるように座標を変換する |
| jojisdogakane | 0:2f6112d75752 | 795 | movex = x2 * -1; |
| jojisdogakane | 0:2f6112d75752 | 796 | movey = y2 * -1; |
| jojisdogakane | 0:2f6112d75752 | 797 | |
| jojisdogakane | 0:2f6112d75752 | 798 | tempx1 = x1 - movex; |
| jojisdogakane | 0:2f6112d75752 | 799 | tempy1 = y1 - movey; |
| jojisdogakane | 0:2f6112d75752 | 800 | tempx2 = x2 - movex; |
| jojisdogakane | 0:2f6112d75752 | 801 | tempy2 = y2 - movey; |
| jojisdogakane | 0:2f6112d75752 | 802 | tempx3 = x3 - movex; |
| jojisdogakane | 0:2f6112d75752 | 803 | tempy3 = y3 - movey; |
| jojisdogakane | 0:2f6112d75752 | 804 | |
| jojisdogakane | 0:2f6112d75752 | 805 | // (x2, y2)を中心とした円を考える。 |
| jojisdogakane | 0:2f6112d75752 | 806 | |
| jojisdogakane | 0:2f6112d75752 | 807 | // (x3, y3)を(x2, y2)を中心に180°回転する。→(rotx3, roty3) |
| jojisdogakane | 0:2f6112d75752 | 808 | double theta = DegreeToRadian(180); |
| jojisdogakane | 0:2f6112d75752 | 809 | |
| jojisdogakane | 0:2f6112d75752 | 810 | double rotx3 = (tempx3) * cos(theta) - (tempy3) * sin(theta); |
| jojisdogakane | 0:2f6112d75752 | 811 | double roty3 = (tempx3) * sin(theta) + (tempy3) * cos(theta); |
| jojisdogakane | 0:2f6112d75752 | 812 | |
| jojisdogakane | 0:2f6112d75752 | 813 | // ベクトル1とベクトル3の内積を求める |
| jojisdogakane | 0:2f6112d75752 | 814 | double vecInt = tempx1 * rotx3 + tempy1 * roty3; |
| jojisdogakane | 0:2f6112d75752 | 815 | |
| jojisdogakane | 0:2f6112d75752 | 816 | // ベクトル1とベクトル3の外積を求める |
| jojisdogakane | 0:2f6112d75752 | 817 | double vecExt = tempx1 * roty3 - tempy1 * rotx3; |
| jojisdogakane | 0:2f6112d75752 | 818 | |
| jojisdogakane | 0:2f6112d75752 | 819 | // ベクトル1とベクトル3の角度を求める |
| jojisdogakane | 0:2f6112d75752 | 820 | theta = atan2(vecExt, vecInt); |
| jojisdogakane | 0:2f6112d75752 | 821 | |
| jojisdogakane | 0:2f6112d75752 | 822 | double deg = RadianToDegree(theta); |
| jojisdogakane | 0:2f6112d75752 | 823 | |
| jojisdogakane | 0:2f6112d75752 | 824 | midPoint = degRatio; |
| jojisdogakane | 0:2f6112d75752 | 825 | |
| jojisdogakane | 0:2f6112d75752 | 826 | if( deg <= 11.25 && deg >= -11.25) { |
| jojisdogakane | 0:2f6112d75752 | 827 | degPoint = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 828 | } |
| jojisdogakane | 0:2f6112d75752 | 829 | else if(deg >= 11.25 && deg <= 33.75) { |
| jojisdogakane | 0:2f6112d75752 | 830 | degPoint = 0.375; |
| jojisdogakane | 0:2f6112d75752 | 831 | } |
| jojisdogakane | 0:2f6112d75752 | 832 | else if(deg >= 33.75 && deg <= 56.25) { |
| jojisdogakane | 0:2f6112d75752 | 833 | degPoint = 0.25; |
| jojisdogakane | 0:2f6112d75752 | 834 | } |
| jojisdogakane | 0:2f6112d75752 | 835 | else if(deg >= 56.25 && deg <= 78.75) { |
| jojisdogakane | 0:2f6112d75752 | 836 | degPoint = 0.125; |
| jojisdogakane | 0:2f6112d75752 | 837 | } |
| jojisdogakane | 0:2f6112d75752 | 838 | else if(deg >= 78.75 && deg <= 101.25) { |
| jojisdogakane | 0:2f6112d75752 | 839 | degPoint = 0; |
| jojisdogakane | 0:2f6112d75752 | 840 | } |
| jojisdogakane | 0:2f6112d75752 | 841 | else if(deg >= 101.25 && deg <= 123.75) { |
| jojisdogakane | 0:2f6112d75752 | 842 | degPoint = 0.125; |
| jojisdogakane | 0:2f6112d75752 | 843 | } |
| jojisdogakane | 0:2f6112d75752 | 844 | else if(deg >= 123.75 && deg <= 146.25) { |
| jojisdogakane | 0:2f6112d75752 | 845 | degPoint = 0.25; |
| jojisdogakane | 0:2f6112d75752 | 846 | } |
| jojisdogakane | 0:2f6112d75752 | 847 | else if(deg >= 146.25 && deg <= 168.75) { |
| jojisdogakane | 0:2f6112d75752 | 848 | degPoint = 0.375; |
| jojisdogakane | 0:2f6112d75752 | 849 | } |
| jojisdogakane | 0:2f6112d75752 | 850 | else if(deg >= 168.75 && deg <= 180 || deg <= 0 && deg >= -11.25) { |
| jojisdogakane | 0:2f6112d75752 | 851 | degPoint = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 852 | } |
| jojisdogakane | 0:2f6112d75752 | 853 | else if(deg <= -11.25 && deg >= -33.75) { |
| jojisdogakane | 0:2f6112d75752 | 854 | degPoint = 0.625; |
| jojisdogakane | 0:2f6112d75752 | 855 | } |
| jojisdogakane | 0:2f6112d75752 | 856 | else if(deg <= -33.75 && deg >= -56.25) { |
| jojisdogakane | 0:2f6112d75752 | 857 | degPoint = 0.75; |
| jojisdogakane | 0:2f6112d75752 | 858 | } |
| jojisdogakane | 0:2f6112d75752 | 859 | else if(deg <= -56.25 && deg >= -78.75) { |
| jojisdogakane | 0:2f6112d75752 | 860 | degPoint = 0.875; |
| jojisdogakane | 0:2f6112d75752 | 861 | } |
| jojisdogakane | 0:2f6112d75752 | 862 | else if(deg <= -78.75 && deg >= -101.25) { |
| jojisdogakane | 0:2f6112d75752 | 863 | degPoint = 1; |
| jojisdogakane | 0:2f6112d75752 | 864 | } |
| jojisdogakane | 0:2f6112d75752 | 865 | else if(deg <= -101.25 && deg >= -123.75) { |
| jojisdogakane | 0:2f6112d75752 | 866 | degPoint = 0.875; |
| jojisdogakane | 0:2f6112d75752 | 867 | } |
| jojisdogakane | 0:2f6112d75752 | 868 | else if(deg <= -123.75 && deg >= -146.25) { |
| jojisdogakane | 0:2f6112d75752 | 869 | degPoint = 0.75; |
| jojisdogakane | 0:2f6112d75752 | 870 | } |
| jojisdogakane | 0:2f6112d75752 | 871 | else if(deg <= -146.25 && deg >= -168.75) { |
| jojisdogakane | 0:2f6112d75752 | 872 | degPoint = 0.625; |
| jojisdogakane | 0:2f6112d75752 | 873 | } |
| jojisdogakane | 0:2f6112d75752 | 874 | else { |
| jojisdogakane | 0:2f6112d75752 | 875 | degPoint = 0; |
| jojisdogakane | 0:2f6112d75752 | 876 | } |
| jojisdogakane | 0:2f6112d75752 | 877 | |
| jojisdogakane | 4:a0150e0f285b | 878 | happyRatioNow = degPoint; |
| jojisdogakane | 4:a0150e0f285b | 879 | |
| jojisdogakane | 0:2f6112d75752 | 880 | degRatio = (deg + 180.0) / 360.0; |
| jojisdogakane | 0:2f6112d75752 | 881 | |
| jojisdogakane | 0:2f6112d75752 | 882 | float rcFilter = RCFILTER; |
| jojisdogakane | 0:2f6112d75752 | 883 | float filteredAngle = rcFilter * prevFilteredAngle + (1 - rcFilter) * degPoint; |
| jojisdogakane | 0:2f6112d75752 | 884 | prevFilteredAngle = filteredAngle ; |
| jojisdogakane | 0:2f6112d75752 | 885 | |
| jojisdogakane | 0:2f6112d75752 | 886 | happyRatio = filteredAngle - 0.5; |
| jojisdogakane | 0:2f6112d75752 | 887 | |
| jojisdogakane | 0:2f6112d75752 | 888 | if(happyRatio <= 0){ |
| jojisdogakane | 0:2f6112d75752 | 889 | happyRatio = abs(happyRatio); |
| jojisdogakane | 0:2f6112d75752 | 890 | } |
| jojisdogakane | 0:2f6112d75752 | 891 | else { |
| jojisdogakane | 0:2f6112d75752 | 892 | happyRatio = 0; |
| jojisdogakane | 0:2f6112d75752 | 893 | } |
| jojisdogakane | 0:2f6112d75752 | 894 | |
| jojisdogakane | 0:2f6112d75752 | 895 | // happyRatio = averageStressRatio ;// @@@ |
| jojisdogakane | 0:2f6112d75752 | 896 | |
| jojisdogakane | 0:2f6112d75752 | 897 | nMsgHappyLen = sprintf(msgHappy,"H%0.2f\0", happyRatio); |
| jojisdogakane | 0:2f6112d75752 | 898 | |
| jojisdogakane | 0:2f6112d75752 | 899 | // nMsgLen = sprintf(msg,"H%0.2f\0", happyRatio); |
| jojisdogakane | 0:2f6112d75752 | 900 | // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); |
| jojisdogakane | 0:2f6112d75752 | 901 | |
| jojisdogakane | 4:a0150e0f285b | 902 | // happyRatio = happyRatio * happyRatio * happyRatio; |
| jojisdogakane | 0:2f6112d75752 | 903 | |
| jojisdogakane | 4:a0150e0f285b | 904 | // happyRatio *= 10; |
| jojisdogakane | 0:2f6112d75752 | 905 | |
| jojisdogakane | 0:2f6112d75752 | 906 | x3 = x2; |
| jojisdogakane | 0:2f6112d75752 | 907 | y3 = y2; |
| jojisdogakane | 0:2f6112d75752 | 908 | |
| jojisdogakane | 0:2f6112d75752 | 909 | int HR_range = 0; |
| jojisdogakane | 0:2f6112d75752 | 910 | |
| jojisdogakane | 0:2f6112d75752 | 911 | int HR_tempval = currentBPM; |
| jojisdogakane | 0:2f6112d75752 | 912 | |
| jojisdogakane | 0:2f6112d75752 | 913 | |
| jojisdogakane | 0:2f6112d75752 | 914 | // 心拍の係数を算出する |
| jojisdogakane | 0:2f6112d75752 | 915 | |
| jojisdogakane | 0:2f6112d75752 | 916 | // 最大心拍数以上→最大心拍数に |
| jojisdogakane | 0:2f6112d75752 | 917 | if( HR_MAX < HR_tempval) |
| jojisdogakane | 0:2f6112d75752 | 918 | { |
| jojisdogakane | 0:2f6112d75752 | 919 | HR_tempval = HR_MAX; |
| jojisdogakane | 0:2f6112d75752 | 920 | } |
| jojisdogakane | 0:2f6112d75752 | 921 | |
| jojisdogakane | 0:2f6112d75752 | 922 | // 最少心拍数以下→最少心拍数に |
| jojisdogakane | 0:2f6112d75752 | 923 | if( HR_MIN > HR_tempval) |
| jojisdogakane | 0:2f6112d75752 | 924 | { |
| jojisdogakane | 0:2f6112d75752 | 925 | HR_tempval = HR_MIN; |
| jojisdogakane | 0:2f6112d75752 | 926 | } |
| jojisdogakane | 0:2f6112d75752 | 927 | |
| jojisdogakane | 0:2f6112d75752 | 928 | // 心拍の幅を計算 |
| jojisdogakane | 0:2f6112d75752 | 929 | HR_range = HR_MAX - HR_MIN; |
| jojisdogakane | 0:2f6112d75752 | 930 | |
| jojisdogakane | 0:2f6112d75752 | 931 | // 現在値－最少心拍数 |
| jojisdogakane | 0:2f6112d75752 | 932 | HR_tempval = HR_tempval - HR_MIN; |
| jojisdogakane | 0:2f6112d75752 | 933 | |
| jojisdogakane | 0:2f6112d75752 | 934 | // 心拍の係数を算出。0 ～ 1.0 |
| jojisdogakane | 0:2f6112d75752 | 935 | HR_ratio = (double)HR_tempval / (double)HR_range; |
| jojisdogakane | 0:2f6112d75752 | 936 | |
| jojisdogakane | 0:2f6112d75752 | 937 | if(conRatioNow >= 0.95 /* && averageAngle >= 0.7*/) |
| jojisdogakane | 0:2f6112d75752 | 938 | { |
| jojisdogakane | 0:2f6112d75752 | 939 | bConcentration = true; |
| jojisdogakane | 0:2f6112d75752 | 940 | } |
| jojisdogakane | 0:2f6112d75752 | 941 | else { |
| jojisdogakane | 0:2f6112d75752 | 942 | bConcentration = false; |
| jojisdogakane | 0:2f6112d75752 | 943 | } |
| jojisdogakane | 0:2f6112d75752 | 944 | prevRelaxRatio = relaxRatio; |
| jojisdogakane | 0:2f6112d75752 | 945 | |
| jojisdogakane | 0:2f6112d75752 | 946 | int hval = 1023 * happyRatio; |
| jojisdogakane | 0:2f6112d75752 | 947 | int rval = 1023 * relaxRatio; |
| jojisdogakane | 0:2f6112d75752 | 948 | int cval = 1023 * conRatio; |
| jojisdogakane | 0:2f6112d75752 | 949 | |
| jojisdogakane | 0:2f6112d75752 | 950 | // printf("H%d\n", hval); |
| jojisdogakane | 0:2f6112d75752 | 951 | // printf("R%d\n", rval); |
| jojisdogakane | 0:2f6112d75752 | 952 | // printf("C%d\n", cval); |
| jojisdogakane | 0:2f6112d75752 | 953 | |
| jojisdogakane | 0:2f6112d75752 | 954 | sprintf(msg,"%s%s%s%s\0", msgIBI, msgHappy, msgRelax, msgConcentration); |
| jojisdogakane | 0:2f6112d75752 | 955 | ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), |
| jojisdogakane | 0:2f6112d75752 | 956 | (uint8_t*)msg, nMsgIBILen + nMsgHappyLen + nMsgRelaxLen + nMsgConcentrationLen); |
| jojisdogakane | 0:2f6112d75752 | 957 | // (uint8_t*)msg, nMsgIBILen + 1 + nMsgHappyLen + 1 + nMsgRelaxLen + 1 + nMsgConcentrationLen); |
| jojisdogakane | 0:2f6112d75752 | 958 | |
| jojisdogakane | 0:2f6112d75752 | 959 | // determine color range |
| jojisdogakane | 0:2f6112d75752 | 960 | |
| jojisdogakane | 0:2f6112d75752 | 961 | conRatio = conRatio - 0.5; |
| jojisdogakane | 0:2f6112d75752 | 962 | |
| jojisdogakane | 0:2f6112d75752 | 963 | if(conRatio < 0) { |
| jojisdogakane | 0:2f6112d75752 | 964 | conRatio = 0; |
| jojisdogakane | 0:2f6112d75752 | 965 | } |
| jojisdogakane | 0:2f6112d75752 | 966 | |
| jojisdogakane | 0:2f6112d75752 | 967 | conRatio = conRatio * 2; |
| jojisdogakane | 0:2f6112d75752 | 968 | |
| jojisdogakane | 0:2f6112d75752 | 969 | // happyRatio = happyRatio * 2; |
| jojisdogakane | 0:2f6112d75752 | 970 | // happyRatio = happyRatio * 2; |
| jojisdogakane | 0:2f6112d75752 | 971 | if(happyRatio > 1) { |
| jojisdogakane | 0:2f6112d75752 | 972 | happyRatio = 1; |
| jojisdogakane | 0:2f6112d75752 | 973 | } |
| jojisdogakane | 4:a0150e0f285b | 974 | /* |
| jojisdogakane | 0:2f6112d75752 | 975 | if(BPM >= HR_MIN) { |
| jojisdogakane | 0:2f6112d75752 | 976 | happyRatio /= ((float)(BPM * 2) / 255.0) ; |
| jojisdogakane | 0:2f6112d75752 | 977 | } |
| jojisdogakane | 4:a0150e0f285b | 978 | */ |
| jojisdogakane | 0:2f6112d75752 | 979 | } |
| jojisdogakane | 0:2f6112d75752 | 980 | |
| jojisdogakane | 4:a0150e0f285b | 981 | if(BPM > 0 && bPulseEvent) { |
| jojisdogakane | 4:a0150e0f285b | 982 | int NewMood = getMood(BPM, relaxRatio, happyRatio, conRatio) ; |
| jojisdogakane | 4:a0150e0f285b | 983 | |
| jojisdogakane | 4:a0150e0f285b | 984 | for(i = 0 ; i < NEOPIXEL_COUNT; i++) { |
| jojisdogakane | 4:a0150e0f285b | 985 | TargetColorArrayR[i] = MoodColorR[i][NewMood]; |
| jojisdogakane | 4:a0150e0f285b | 986 | TargetColorArrayG[i] = MoodColorG[i][NewMood]; |
| jojisdogakane | 4:a0150e0f285b | 987 | TargetColorArrayB[i] = MoodColorB[i][NewMood]; |
| jojisdogakane | 0:2f6112d75752 | 988 | } |
| jojisdogakane | 4:a0150e0f285b | 989 | } |
| jojisdogakane | 0:2f6112d75752 | 990 | |
| jojisdogakane | 4:a0150e0f285b | 991 | // for(i = NEOPIXEL_COUNT - 1; i >= 0; i--) { |
| jojisdogakane | 4:a0150e0f285b | 992 | if(ColorArrayR[pixelIndex] < TargetColorArrayR[pixelIndex]) { |
| jojisdogakane | 4:a0150e0f285b | 993 | ColorArrayR[pixelIndex]++; |
| jojisdogakane | 4:a0150e0f285b | 994 | } |
| jojisdogakane | 4:a0150e0f285b | 995 | else if(ColorArrayR[pixelIndex] > TargetColorArrayR[pixelIndex]) { |
| jojisdogakane | 4:a0150e0f285b | 996 | ColorArrayR[pixelIndex]--; |
| jojisdogakane | 4:a0150e0f285b | 997 | } |
| jojisdogakane | 4:a0150e0f285b | 998 | /* if(ColorArrayR[pixelIndex] < 200) { |
| jojisdogakane | 4:a0150e0f285b | 999 | ColorArrayR[pixelIndex]++; |
| jojisdogakane | 4:a0150e0f285b | 1000 | } |
| jojisdogakane | 4:a0150e0f285b | 1001 | else if(ColorArrayR[pixelIndex] > 200) { |
| jojisdogakane | 4:a0150e0f285b | 1002 | ColorArrayR[pixelIndex]--; |
| jojisdogakane | 4:a0150e0f285b | 1003 | }*/ |
| jojisdogakane | 4:a0150e0f285b | 1004 | if(ColorArrayG[pixelIndex] < TargetColorArrayG[pixelIndex]) { |
| jojisdogakane | 4:a0150e0f285b | 1005 | ColorArrayG[pixelIndex]++; |
| jojisdogakane | 4:a0150e0f285b | 1006 | } |
| jojisdogakane | 4:a0150e0f285b | 1007 | else if(ColorArrayG[pixelIndex] > TargetColorArrayG[pixelIndex]) { |
| jojisdogakane | 4:a0150e0f285b | 1008 | ColorArrayG[pixelIndex]--; |
| jojisdogakane | 4:a0150e0f285b | 1009 | } |
| jojisdogakane | 4:a0150e0f285b | 1010 | if(ColorArrayB[pixelIndex] < TargetColorArrayB[pixelIndex]) { |
| jojisdogakane | 4:a0150e0f285b | 1011 | ColorArrayB[pixelIndex]++; |
| jojisdogakane | 4:a0150e0f285b | 1012 | } |
| jojisdogakane | 4:a0150e0f285b | 1013 | else if(ColorArrayB[pixelIndex] > TargetColorArrayB[pixelIndex]) { |
| jojisdogakane | 4:a0150e0f285b | 1014 | ColorArrayB[pixelIndex]--; |
| jojisdogakane | 0:2f6112d75752 | 1015 | } |
| jojisdogakane | 0:2f6112d75752 | 1016 | |
| jojisdogakane | 4:a0150e0f285b | 1017 | neopixel_set_color( |
| jojisdogakane | 4:a0150e0f285b | 1018 | pixels[pixelIndex], |
| jojisdogakane | 4:a0150e0f285b | 1019 | 0, |
| jojisdogakane | 4:a0150e0f285b | 1020 | short(float(ColorArrayR[pixelIndex]) * DIM_LED), |
| jojisdogakane | 4:a0150e0f285b | 1021 | short(float(ColorArrayG[pixelIndex]) * DIM_LED), |
| jojisdogakane | 4:a0150e0f285b | 1022 | short(float(ColorArrayB[pixelIndex]) * DIM_LED)); |
| jojisdogakane | 4:a0150e0f285b | 1023 | neopixel_show(pixels[pixelIndex]); |
| jojisdogakane | 4:a0150e0f285b | 1024 | // } |
| jojisdogakane | 4:a0150e0f285b | 1025 | pixelIndex++; |
| jojisdogakane | 4:a0150e0f285b | 1026 | if(pixelIndex >= NEOPIXEL_COUNT) |
| jojisdogakane | 4:a0150e0f285b | 1027 | { |
| jojisdogakane | 4:a0150e0f285b | 1028 | pixelIndex = 0; |
| jojisdogakane | 0:2f6112d75752 | 1029 | } |
| jojisdogakane | 4:a0150e0f285b | 1030 | //wait_ms(1); |
| jojisdogakane | 4:a0150e0f285b | 1031 | wait_us(500); |
| jojisdogakane | 0:2f6112d75752 | 1032 | } |
| jojisdogakane | 0:2f6112d75752 | 1033 | } |