Working code with BLE and accelerometer.(DFU pending)
Dependencies: BLE_API mbed nRF51822 strike_detect
Fork of TenCount_BLE by
main.cpp@5:7c5630442028, 2016-08-25 (annotated)
- Committer:
- AamirNiaz
- Date:
- Thu Aug 25 09:02:27 2016 +0000
- Revision:
- 5:7c5630442028
- Parent:
- 4:2b0947ed0e0d
nrf51822 code with working accelerometer.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
RedBearLab | 1:1c058e553423 | 1 | /* |
RedBearLab | 0:cffe8ac1bdf0 | 2 | |
RedBearLab | 1:1c058e553423 | 3 | Copyright (c) 2012-2014 RedBearLab |
RedBearLab | 1:1c058e553423 | 4 | |
RedBearLab | 1:1c058e553423 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software |
RedBearLab | 1:1c058e553423 | 6 | and associated documentation files (the "Software"), to deal in the Software without restriction, |
RedBearLab | 1:1c058e553423 | 7 | including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, |
RedBearLab | 1:1c058e553423 | 8 | and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, |
RedBearLab | 1:1c058e553423 | 9 | subject to the following conditions: |
RedBearLab | 1:1c058e553423 | 10 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. |
RedBearLab | 1:1c058e553423 | 11 | |
RedBearLab | 1:1c058e553423 | 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, |
RedBearLab | 1:1c058e553423 | 13 | INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR |
RedBearLab | 1:1c058e553423 | 14 | PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE |
RedBearLab | 1:1c058e553423 | 15 | FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, |
RedBearLab | 1:1c058e553423 | 16 | ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. |
RedBearLab | 1:1c058e553423 | 17 | |
RedBearLab | 1:1c058e553423 | 18 | */ |
RedBearLab | 1:1c058e553423 | 19 | |
RedBearLab | 1:1c058e553423 | 20 | /* |
RedBearLab | 1:1c058e553423 | 21 | * The application works with the BLEController iOS/Android App. |
RedBearLab | 1:1c058e553423 | 22 | * Type something from the Terminal to send |
RedBearLab | 1:1c058e553423 | 23 | * to the BLEController App or vice verse. |
RedBearLab | 1:1c058e553423 | 24 | * Characteristics received from App will print on Terminal. |
RedBearLab | 1:1c058e553423 | 25 | */ |
RedBearLab | 1:1c058e553423 | 26 | |
RedBearLab | 0:cffe8ac1bdf0 | 27 | #include "mbed.h" |
RedBearLab | 2:4b66b69c7ecb | 28 | #include "ble/BLE.h" |
elmbed | 4:2b0947ed0e0d | 29 | #include "LIS3DH.h" |
elmbed | 4:2b0947ed0e0d | 30 | #include "strike.h" |
elmbed | 4:2b0947ed0e0d | 31 | #include "Sensor.h" |
RedBearLab | 0:cffe8ac1bdf0 | 32 | |
RedBearLab | 0:cffe8ac1bdf0 | 33 | #define BLE_UUID_TXRX_SERVICE 0x0000 /**< The UUID of the Nordic UART Service. */ |
RedBearLab | 0:cffe8ac1bdf0 | 34 | #define BLE_UUID_TX_CHARACTERISTIC 0x0002 /**< The UUID of the TX Characteristic. */ |
RedBearLab | 0:cffe8ac1bdf0 | 35 | #define BLE_UUIDS_RX_CHARACTERISTIC 0x0003 /**< The UUID of the RX Characteristic. */ |
RedBearLab | 0:cffe8ac1bdf0 | 36 | |
RedBearLab | 0:cffe8ac1bdf0 | 37 | #define TXRX_BUF_LEN 20 |
RedBearLab | 0:cffe8ac1bdf0 | 38 | |
elmbed | 4:2b0947ed0e0d | 39 | |
elmbed | 4:2b0947ed0e0d | 40 | //you can receive less then 93 bytes at 921600 Bd |
elmbed | 4:2b0947ed0e0d | 41 | #define UART_SENSOR_COMMAND_BUFFER_SIZE (100) |
elmbed | 4:2b0947ed0e0d | 42 | #define SYSTEM_COM_BUFFER (1024) |
elmbed | 4:2b0947ed0e0d | 43 | #define RADIO_MESSAGE_LEN (64) |
elmbed | 4:2b0947ed0e0d | 44 | #define VERSION "alpha_20150409-01" |
elmbed | 4:2b0947ed0e0d | 45 | #define SPEW (0) |
elmbed | 4:2b0947ed0e0d | 46 | #define STRIKE_DETECT_1 (1) |
elmbed | 4:2b0947ed0e0d | 47 | #define STRIKE_DETECT_2 (2) |
elmbed | 4:2b0947ed0e0d | 48 | |
elmbed | 4:2b0947ed0e0d | 49 | #define DEFAULT_TIMESTEP (10) // in milliseconds |
elmbed | 4:2b0947ed0e0d | 50 | //#define DEFAULT_FAST_FILTER (650) |
elmbed | 4:2b0947ed0e0d | 51 | //#define DEFAULT_SLOW_FILTER (975) |
elmbed | 4:2b0947ed0e0d | 52 | #define DEFAULT_BIG_HIT_THRESHOLD (3000) |
elmbed | 4:2b0947ed0e0d | 53 | #define DEFAULT_SMALL_HIT_THRESHOLD (200) |
elmbed | 4:2b0947ed0e0d | 54 | #define DEFAULT_JERK_THRESHOLD (500) |
elmbed | 4:2b0947ed0e0d | 55 | |
elmbed | 4:2b0947ed0e0d | 56 | #define DEFAULT_C1_1 (0.35) |
elmbed | 4:2b0947ed0e0d | 57 | #define DEFAULT_C2_1 (0.65) |
elmbed | 4:2b0947ed0e0d | 58 | #define DEFAULT_C1_2 (0.025) |
elmbed | 4:2b0947ed0e0d | 59 | #define DEFAULT_C2_2 (0.975) |
elmbed | 4:2b0947ed0e0d | 60 | |
elmbed | 4:2b0947ed0e0d | 61 | #define DEFAULT_SHADOW_DECAY (0.8) |
elmbed | 4:2b0947ed0e0d | 62 | |
RedBearLab | 2:4b66b69c7ecb | 63 | BLE ble; |
RedBearLab | 0:cffe8ac1bdf0 | 64 | |
AamirNiaz | 5:7c5630442028 | 65 | Serial pc(P0_4, USBRX); |
RedBearLab | 0:cffe8ac1bdf0 | 66 | |
RedBearLab | 0:cffe8ac1bdf0 | 67 | |
RedBearLab | 0:cffe8ac1bdf0 | 68 | // The Nordic UART Service |
RedBearLab | 0:cffe8ac1bdf0 | 69 | static const uint8_t uart_base_uuid[] = {0x71, 0x3D, 0, 0, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E}; |
RedBearLab | 0:cffe8ac1bdf0 | 70 | static const uint8_t uart_tx_uuid[] = {0x71, 0x3D, 0, 3, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E}; |
RedBearLab | 0:cffe8ac1bdf0 | 71 | static const uint8_t uart_rx_uuid[] = {0x71, 0x3D, 0, 2, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E}; |
RedBearLab | 0:cffe8ac1bdf0 | 72 | static const uint8_t uart_base_uuid_rev[] = {0x1E, 0x94, 0x8D, 0xF1, 0x48, 0x31, 0x94, 0xBA, 0x75, 0x4C, 0x3E, 0x50, 0, 0, 0x3D, 0x71}; |
RedBearLab | 0:cffe8ac1bdf0 | 73 | |
RedBearLab | 0:cffe8ac1bdf0 | 74 | |
RedBearLab | 0:cffe8ac1bdf0 | 75 | uint8_t txPayload[TXRX_BUF_LEN] = {0,}; |
RedBearLab | 0:cffe8ac1bdf0 | 76 | uint8_t rxPayload[TXRX_BUF_LEN] = {0,}; |
RedBearLab | 0:cffe8ac1bdf0 | 77 | |
RedBearLab | 0:cffe8ac1bdf0 | 78 | static uint8_t rx_buf[TXRX_BUF_LEN]; |
RedBearLab | 0:cffe8ac1bdf0 | 79 | static uint8_t rx_len=0; |
RedBearLab | 0:cffe8ac1bdf0 | 80 | |
RedBearLab | 0:cffe8ac1bdf0 | 81 | |
RedBearLab | 0:cffe8ac1bdf0 | 82 | GattCharacteristic txCharacteristic (uart_tx_uuid, txPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE); |
RedBearLab | 0:cffe8ac1bdf0 | 83 | |
RedBearLab | 0:cffe8ac1bdf0 | 84 | GattCharacteristic rxCharacteristic (uart_rx_uuid, rxPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY); |
RedBearLab | 0:cffe8ac1bdf0 | 85 | |
RedBearLab | 0:cffe8ac1bdf0 | 86 | GattCharacteristic *uartChars[] = {&txCharacteristic, &rxCharacteristic}; |
RedBearLab | 0:cffe8ac1bdf0 | 87 | |
RedBearLab | 0:cffe8ac1bdf0 | 88 | GattService uartService(uart_base_uuid, uartChars, sizeof(uartChars) / sizeof(GattCharacteristic *)); |
RedBearLab | 0:cffe8ac1bdf0 | 89 | |
RedBearLab | 0:cffe8ac1bdf0 | 90 | |
RedBearLab | 0:cffe8ac1bdf0 | 91 | |
elmbed | 4:2b0947ed0e0d | 92 | void interpret(char parameter, int value); |
elmbed | 4:2b0947ed0e0d | 93 | void getRadioInput(char *data, uint16_t size); |
elmbed | 4:2b0947ed0e0d | 94 | void read_accel(); |
elmbed | 4:2b0947ed0e0d | 95 | |
elmbed | 4:2b0947ed0e0d | 96 | Ticker measure; |
elmbed | 4:2b0947ed0e0d | 97 | Ticker spew; |
elmbed | 4:2b0947ed0e0d | 98 | uint16_t strike_value = 0; |
elmbed | 4:2b0947ed0e0d | 99 | int last_result = 0; |
elmbed | 4:2b0947ed0e0d | 100 | bool new_strike = false; |
elmbed | 4:2b0947ed0e0d | 101 | bool active = false; |
elmbed | 4:2b0947ed0e0d | 102 | static volatile int config_parameter = 0; |
elmbed | 4:2b0947ed0e0d | 103 | |
elmbed | 4:2b0947ed0e0d | 104 | static volatile int mode = STRIKE_DETECT_2; |
elmbed | 4:2b0947ed0e0d | 105 | static volatile int timestep = DEFAULT_TIMESTEP; |
elmbed | 4:2b0947ed0e0d | 106 | //static volatile int fast_filter = DEFAULT_FAST_FILTER; |
elmbed | 4:2b0947ed0e0d | 107 | //static volatile int slow_filter = DEFAULT_SLOW_FILTER; |
elmbed | 4:2b0947ed0e0d | 108 | static volatile int big_hit_threshold = DEFAULT_BIG_HIT_THRESHOLD; |
elmbed | 4:2b0947ed0e0d | 109 | static volatile int small_hit_threshold = DEFAULT_SMALL_HIT_THRESHOLD; |
elmbed | 4:2b0947ed0e0d | 110 | static volatile int jerk_threshold = DEFAULT_JERK_THRESHOLD; |
elmbed | 4:2b0947ed0e0d | 111 | |
elmbed | 4:2b0947ed0e0d | 112 | static volatile float c1_1 = DEFAULT_C1_1; |
elmbed | 4:2b0947ed0e0d | 113 | static volatile float c2_1 = DEFAULT_C2_1; |
elmbed | 4:2b0947ed0e0d | 114 | static volatile float c1_2 = DEFAULT_C1_2; |
elmbed | 4:2b0947ed0e0d | 115 | static volatile float c2_2 = DEFAULT_C2_2; |
elmbed | 4:2b0947ed0e0d | 116 | |
elmbed | 4:2b0947ed0e0d | 117 | static volatile float shadow_decay = DEFAULT_SHADOW_DECAY; |
elmbed | 4:2b0947ed0e0d | 118 | |
elmbed | 4:2b0947ed0e0d | 119 | static int16_t xzy[3]; |
elmbed | 4:2b0947ed0e0d | 120 | |
elmbed | 4:2b0947ed0e0d | 121 | |
RedBearLab | 3:b3f6c612b603 | 122 | void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params) |
RedBearLab | 0:cffe8ac1bdf0 | 123 | { |
RedBearLab | 0:cffe8ac1bdf0 | 124 | pc.printf("Disconnected \r\n"); |
RedBearLab | 0:cffe8ac1bdf0 | 125 | pc.printf("Restart advertising \r\n"); |
RedBearLab | 0:cffe8ac1bdf0 | 126 | ble.startAdvertising(); |
RedBearLab | 0:cffe8ac1bdf0 | 127 | } |
RedBearLab | 0:cffe8ac1bdf0 | 128 | |
RedBearLab | 2:4b66b69c7ecb | 129 | void WrittenHandler(const GattWriteCallbackParams *Handler) |
RedBearLab | 0:cffe8ac1bdf0 | 130 | { |
RedBearLab | 0:cffe8ac1bdf0 | 131 | uint8_t buf[TXRX_BUF_LEN]; |
RedBearLab | 0:cffe8ac1bdf0 | 132 | uint16_t bytesRead, index; |
RedBearLab | 0:cffe8ac1bdf0 | 133 | |
RedBearLab | 2:4b66b69c7ecb | 134 | if (Handler->handle == txCharacteristic.getValueAttribute().getHandle()) |
RedBearLab | 0:cffe8ac1bdf0 | 135 | { |
RedBearLab | 0:cffe8ac1bdf0 | 136 | ble.readCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(), buf, &bytesRead); |
RedBearLab | 0:cffe8ac1bdf0 | 137 | memset(txPayload, 0, TXRX_BUF_LEN); |
RedBearLab | 0:cffe8ac1bdf0 | 138 | memcpy(txPayload, buf, TXRX_BUF_LEN); |
RedBearLab | 0:cffe8ac1bdf0 | 139 | pc.printf("WriteHandler \r\n"); |
elmbed | 4:2b0947ed0e0d | 140 | pc.printf("Length: %d\r\n", bytesRead); |
RedBearLab | 0:cffe8ac1bdf0 | 141 | pc.printf("Data: "); |
RedBearLab | 0:cffe8ac1bdf0 | 142 | for(index=0; index<bytesRead; index++) |
RedBearLab | 0:cffe8ac1bdf0 | 143 | { |
RedBearLab | 0:cffe8ac1bdf0 | 144 | pc.putc(txPayload[index]); |
RedBearLab | 0:cffe8ac1bdf0 | 145 | } |
RedBearLab | 0:cffe8ac1bdf0 | 146 | pc.printf("\r\n"); |
elmbed | 4:2b0947ed0e0d | 147 | |
elmbed | 4:2b0947ed0e0d | 148 | getRadioInput((char*)txPayload, bytesRead); |
RedBearLab | 0:cffe8ac1bdf0 | 149 | } |
RedBearLab | 0:cffe8ac1bdf0 | 150 | } |
RedBearLab | 0:cffe8ac1bdf0 | 151 | |
RedBearLab | 0:cffe8ac1bdf0 | 152 | void uartCB(void) |
RedBearLab | 0:cffe8ac1bdf0 | 153 | { |
RedBearLab | 0:cffe8ac1bdf0 | 154 | while(pc.readable()) |
RedBearLab | 0:cffe8ac1bdf0 | 155 | { |
RedBearLab | 0:cffe8ac1bdf0 | 156 | rx_buf[rx_len++] = pc.getc(); |
RedBearLab | 0:cffe8ac1bdf0 | 157 | if(rx_len>=20 || rx_buf[rx_len-1]=='\0' || rx_buf[rx_len-1]=='\n') |
RedBearLab | 0:cffe8ac1bdf0 | 158 | { |
RedBearLab | 0:cffe8ac1bdf0 | 159 | ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), rx_buf, rx_len); |
RedBearLab | 0:cffe8ac1bdf0 | 160 | pc.printf("RecHandler \r\n"); |
RedBearLab | 0:cffe8ac1bdf0 | 161 | pc.printf("Length: "); |
RedBearLab | 0:cffe8ac1bdf0 | 162 | pc.putc(rx_len); |
RedBearLab | 0:cffe8ac1bdf0 | 163 | pc.printf("\r\n"); |
RedBearLab | 0:cffe8ac1bdf0 | 164 | rx_len = 0; |
RedBearLab | 0:cffe8ac1bdf0 | 165 | break; |
RedBearLab | 0:cffe8ac1bdf0 | 166 | } |
RedBearLab | 0:cffe8ac1bdf0 | 167 | } |
RedBearLab | 0:cffe8ac1bdf0 | 168 | } |
RedBearLab | 0:cffe8ac1bdf0 | 169 | |
RedBearLab | 0:cffe8ac1bdf0 | 170 | int main(void) |
RedBearLab | 0:cffe8ac1bdf0 | 171 | { |
RedBearLab | 0:cffe8ac1bdf0 | 172 | ble.init(); |
RedBearLab | 0:cffe8ac1bdf0 | 173 | ble.onDisconnection(disconnectionCallback); |
RedBearLab | 0:cffe8ac1bdf0 | 174 | ble.onDataWritten(WrittenHandler); |
RedBearLab | 0:cffe8ac1bdf0 | 175 | |
RedBearLab | 0:cffe8ac1bdf0 | 176 | pc.baud(9600); |
elmbed | 4:2b0947ed0e0d | 177 | pc.printf("TenCount \r\n"); |
elmbed | 4:2b0947ed0e0d | 178 | |
elmbed | 4:2b0947ed0e0d | 179 | LIS3DH_init(); |
RedBearLab | 0:cffe8ac1bdf0 | 180 | |
AamirNiaz | 5:7c5630442028 | 181 | //pc.attach( uartCB , pc.RxIrq); |
RedBearLab | 0:cffe8ac1bdf0 | 182 | // setup advertising |
RedBearLab | 0:cffe8ac1bdf0 | 183 | ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED); |
RedBearLab | 0:cffe8ac1bdf0 | 184 | ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
RedBearLab | 0:cffe8ac1bdf0 | 185 | ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME, |
elmbed | 4:2b0947ed0e0d | 186 | (const uint8_t *)"TenCount", sizeof("TenCount") - 1); |
RedBearLab | 0:cffe8ac1bdf0 | 187 | ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS, |
RedBearLab | 0:cffe8ac1bdf0 | 188 | (const uint8_t *)uart_base_uuid_rev, sizeof(uart_base_uuid)); |
RedBearLab | 0:cffe8ac1bdf0 | 189 | // 100ms; in multiples of 0.625ms. |
RedBearLab | 0:cffe8ac1bdf0 | 190 | ble.setAdvertisingInterval(160); |
RedBearLab | 0:cffe8ac1bdf0 | 191 | |
RedBearLab | 0:cffe8ac1bdf0 | 192 | ble.addService(uartService); |
RedBearLab | 0:cffe8ac1bdf0 | 193 | |
RedBearLab | 0:cffe8ac1bdf0 | 194 | ble.startAdvertising(); |
RedBearLab | 0:cffe8ac1bdf0 | 195 | pc.printf("Advertising Start \r\n"); |
RedBearLab | 0:cffe8ac1bdf0 | 196 | |
elmbed | 4:2b0947ed0e0d | 197 | char txbuff[40] = {0}; |
elmbed | 4:2b0947ed0e0d | 198 | |
RedBearLab | 0:cffe8ac1bdf0 | 199 | while(1) |
RedBearLab | 0:cffe8ac1bdf0 | 200 | { |
elmbed | 4:2b0947ed0e0d | 201 | //ble.waitForEvent(); |
elmbed | 4:2b0947ed0e0d | 202 | if(new_strike){ |
elmbed | 4:2b0947ed0e0d | 203 | if(mode == SPEW){ |
elmbed | 4:2b0947ed0e0d | 204 | pc.printf("s%u\r\n", strike_value); |
elmbed | 4:2b0947ed0e0d | 205 | } |
elmbed | 4:2b0947ed0e0d | 206 | else{ |
elmbed | 4:2b0947ed0e0d | 207 | snprintf(txbuff, sizeof(txbuff), "s%u\r\n", strike_value); |
elmbed | 4:2b0947ed0e0d | 208 | pc.printf("%s", txbuff); |
elmbed | 4:2b0947ed0e0d | 209 | ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), (uint8_t*)txbuff, strlen(txbuff)); |
elmbed | 4:2b0947ed0e0d | 210 | new_strike = false; |
elmbed | 4:2b0947ed0e0d | 211 | } |
elmbed | 4:2b0947ed0e0d | 212 | } |
elmbed | 4:2b0947ed0e0d | 213 | } |
elmbed | 4:2b0947ed0e0d | 214 | } |
elmbed | 4:2b0947ed0e0d | 215 | |
elmbed | 4:2b0947ed0e0d | 216 | void strike_detect1(){ |
elmbed | 4:2b0947ed0e0d | 217 | //static int16_t xzy[3]; |
elmbed | 4:2b0947ed0e0d | 218 | static Sensor* s = new Sensor(); |
elmbed | 4:2b0947ed0e0d | 219 | |
elmbed | 4:2b0947ed0e0d | 220 | LIS3DH_readAll(xzy); |
elmbed | 4:2b0947ed0e0d | 221 | //algorithm expects -/+8g, 0-4095, accelerometer reports ??? (+/-16g, 0-65535?) |
elmbed | 4:2b0947ed0e0d | 222 | int result = parseXY(xzy[0]/16 + 2048, xzy[2]/16 + 2048, s); |
elmbed | 4:2b0947ed0e0d | 223 | if(result){ |
elmbed | 4:2b0947ed0e0d | 224 | //strike_timer.reset(); |
elmbed | 4:2b0947ed0e0d | 225 | //strike_timer.start(); |
elmbed | 4:2b0947ed0e0d | 226 | strike_detect_reset(s); |
elmbed | 4:2b0947ed0e0d | 227 | result = int((float)result*(10000.0/65535.0)); |
elmbed | 4:2b0947ed0e0d | 228 | new_strike = true; |
elmbed | 4:2b0947ed0e0d | 229 | last_result = result; |
elmbed | 4:2b0947ed0e0d | 230 | strike_value = result; |
elmbed | 4:2b0947ed0e0d | 231 | } |
elmbed | 4:2b0947ed0e0d | 232 | } |
elmbed | 4:2b0947ed0e0d | 233 | |
elmbed | 4:2b0947ed0e0d | 234 | void strike_detect2(){ |
elmbed | 4:2b0947ed0e0d | 235 | static float low_pass1 = 0.0; |
elmbed | 4:2b0947ed0e0d | 236 | static float low_pass2 = 0.0; |
elmbed | 4:2b0947ed0e0d | 237 | float RMS = 0.0; |
elmbed | 4:2b0947ed0e0d | 238 | static float last_decay = 0.0; |
elmbed | 4:2b0947ed0e0d | 239 | float decay = 0.0; |
elmbed | 4:2b0947ed0e0d | 240 | static float accumulator = 0.0; |
elmbed | 4:2b0947ed0e0d | 241 | bool accumulating = false; |
elmbed | 4:2b0947ed0e0d | 242 | float jerk = 0.0; |
elmbed | 4:2b0947ed0e0d | 243 | static float shadow = 0.0; |
elmbed | 4:2b0947ed0e0d | 244 | |
elmbed | 4:2b0947ed0e0d | 245 | LIS3DH_readAll(xzy); |
elmbed | 4:2b0947ed0e0d | 246 | float t1 = xzy[0]; |
elmbed | 4:2b0947ed0e0d | 247 | float t2 = xzy[2]; |
elmbed | 4:2b0947ed0e0d | 248 | RMS = sqrt(t1*t1 + t2*t2); |
elmbed | 4:2b0947ed0e0d | 249 | |
elmbed | 4:2b0947ed0e0d | 250 | jerk = low_pass1; |
elmbed | 4:2b0947ed0e0d | 251 | low_pass1 = c1_1*RMS + c2_1*low_pass1; |
elmbed | 4:2b0947ed0e0d | 252 | low_pass2 = c1_2*RMS + c2_2*low_pass2; |
elmbed | 4:2b0947ed0e0d | 253 | |
elmbed | 4:2b0947ed0e0d | 254 | jerk = low_pass1 - jerk; |
elmbed | 4:2b0947ed0e0d | 255 | if(jerk > jerk_threshold)accumulating = true; |
elmbed | 4:2b0947ed0e0d | 256 | |
elmbed | 4:2b0947ed0e0d | 257 | decay = low_pass1 - low_pass2; |
elmbed | 4:2b0947ed0e0d | 258 | if(decay < 0) decay = 0; |
elmbed | 4:2b0947ed0e0d | 259 | |
elmbed | 4:2b0947ed0e0d | 260 | shadow *= shadow_decay; |
elmbed | 4:2b0947ed0e0d | 261 | |
elmbed | 4:2b0947ed0e0d | 262 | if(decay < 400){ |
elmbed | 4:2b0947ed0e0d | 263 | strike_value = ((long)accumulator/30); |
elmbed | 4:2b0947ed0e0d | 264 | if(strike_value > small_hit_threshold && strike_value > shadow){ |
elmbed | 4:2b0947ed0e0d | 265 | new_strike = true; |
elmbed | 4:2b0947ed0e0d | 266 | pc.printf("s%ld\r", (long)accumulator/30); |
elmbed | 4:2b0947ed0e0d | 267 | if(strike_value > big_hit_threshold) shadow = (float) strike_value; |
elmbed | 4:2b0947ed0e0d | 268 | } |
elmbed | 4:2b0947ed0e0d | 269 | accumulator = 0; |
elmbed | 4:2b0947ed0e0d | 270 | accumulating = false; |
elmbed | 4:2b0947ed0e0d | 271 | } |
elmbed | 4:2b0947ed0e0d | 272 | else if(accumulating)accumulator += decay; |
elmbed | 4:2b0947ed0e0d | 273 | last_decay = decay; |
elmbed | 4:2b0947ed0e0d | 274 | } |
elmbed | 4:2b0947ed0e0d | 275 | |
elmbed | 4:2b0947ed0e0d | 276 | void spew_accel_old(){ |
elmbed | 4:2b0947ed0e0d | 277 | static int counter = 0; |
elmbed | 4:2b0947ed0e0d | 278 | static float low_pass = 0.0; |
elmbed | 4:2b0947ed0e0d | 279 | float RMS = 0.0; |
elmbed | 4:2b0947ed0e0d | 280 | float delta = 0.2; |
elmbed | 4:2b0947ed0e0d | 281 | float decay = 0.0; |
elmbed | 4:2b0947ed0e0d | 282 | |
elmbed | 4:2b0947ed0e0d | 283 | //static int16_t xzy[3]; |
elmbed | 4:2b0947ed0e0d | 284 | LIS3DH_readAll(xzy); |
elmbed | 4:2b0947ed0e0d | 285 | RMS = sqrt((float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2]); |
elmbed | 4:2b0947ed0e0d | 286 | low_pass = delta*RMS + (1-delta)*low_pass; |
elmbed | 4:2b0947ed0e0d | 287 | |
elmbed | 4:2b0947ed0e0d | 288 | if(RMS > decay)decay = low_pass; |
elmbed | 4:2b0947ed0e0d | 289 | decay = decay * 0.95; |
elmbed | 4:2b0947ed0e0d | 290 | |
elmbed | 4:2b0947ed0e0d | 291 | //UART.printf("%d %d %d\r\n", last_result, xzy[0], xzy[2]); // should not be printing in interrupt, am going to hell |
elmbed | 4:2b0947ed0e0d | 292 | pc.printf("%f, %f, %f\r\n", RMS, low_pass, decay); // should not be printing in interrupt, am going to hell |
elmbed | 4:2b0947ed0e0d | 293 | if(counter > 15){ |
elmbed | 4:2b0947ed0e0d | 294 | last_result = 0; |
elmbed | 4:2b0947ed0e0d | 295 | counter = 0; |
elmbed | 4:2b0947ed0e0d | 296 | } |
elmbed | 4:2b0947ed0e0d | 297 | if(last_result)counter++; |
elmbed | 4:2b0947ed0e0d | 298 | } |
elmbed | 4:2b0947ed0e0d | 299 | /* |
elmbed | 4:2b0947ed0e0d | 300 | void spew_accel(){ |
elmbed | 4:2b0947ed0e0d | 301 | static float low_pass1 = 0.0; |
elmbed | 4:2b0947ed0e0d | 302 | static float low_pass2 = 0.0; |
elmbed | 4:2b0947ed0e0d | 303 | float RMS = 0.0; |
elmbed | 4:2b0947ed0e0d | 304 | static float last_decay = 0.0; |
elmbed | 4:2b0947ed0e0d | 305 | float decay = 0.0; |
elmbed | 4:2b0947ed0e0d | 306 | static float accumulator = 0.0; |
elmbed | 4:2b0947ed0e0d | 307 | bool accumulating = false; |
elmbed | 4:2b0947ed0e0d | 308 | float jerk = 0.0; |
elmbed | 4:2b0947ed0e0d | 309 | //static float biggest_jerk = 0.0; |
elmbed | 4:2b0947ed0e0d | 310 | |
elmbed | 4:2b0947ed0e0d | 311 | //static int16_t xzy[3]; |
elmbed | 4:2b0947ed0e0d | 312 | LIS3DH_readAll(xzy); |
elmbed | 4:2b0947ed0e0d | 313 | //RMS = (float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2]; |
elmbed | 4:2b0947ed0e0d | 314 | float t1 = xzy[0]; |
elmbed | 4:2b0947ed0e0d | 315 | float t2 = xzy[2]; |
elmbed | 4:2b0947ed0e0d | 316 | RMS = sqrt(t1*t1 + t2*t2); |
elmbed | 4:2b0947ed0e0d | 317 | //RMS = sqrt((float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2]); |
elmbed | 4:2b0947ed0e0d | 318 | jerk = low_pass1; |
elmbed | 4:2b0947ed0e0d | 319 | low_pass1 = 0.35*RMS + 0.65*low_pass1; |
elmbed | 4:2b0947ed0e0d | 320 | low_pass2 = 0.025*RMS + 0.975*low_pass2; |
elmbed | 4:2b0947ed0e0d | 321 | |
elmbed | 4:2b0947ed0e0d | 322 | jerk = low_pass1 - jerk; |
elmbed | 4:2b0947ed0e0d | 323 | if(jerk > 500)accumulating = true; |
elmbed | 4:2b0947ed0e0d | 324 | //if(jerk > biggest_jerk)biggest_jerk = jerk; |
elmbed | 4:2b0947ed0e0d | 325 | |
elmbed | 4:2b0947ed0e0d | 326 | //if(RMS > decay)decay = RMS; |
elmbed | 4:2b0947ed0e0d | 327 | //decay = decay * 0.9999; |
elmbed | 4:2b0947ed0e0d | 328 | decay = low_pass1 - low_pass2; |
elmbed | 4:2b0947ed0e0d | 329 | if(decay < 0) decay = 0; |
elmbed | 4:2b0947ed0e0d | 330 | |
elmbed | 4:2b0947ed0e0d | 331 | if(decay < 400){ |
elmbed | 4:2b0947ed0e0d | 332 | if(accumulator > 10000)UART.printf("s%ld\r", (long)accumulator/30); |
elmbed | 4:2b0947ed0e0d | 333 | accumulator = 0; |
elmbed | 4:2b0947ed0e0d | 334 | accumulating = false; |
elmbed | 4:2b0947ed0e0d | 335 | //biggest_jerk = 0; |
elmbed | 4:2b0947ed0e0d | 336 | } |
elmbed | 4:2b0947ed0e0d | 337 | else if(accumulating)accumulator += decay; |
elmbed | 4:2b0947ed0e0d | 338 | last_decay = decay; |
elmbed | 4:2b0947ed0e0d | 339 | |
elmbed | 4:2b0947ed0e0d | 340 | }*/ |
elmbed | 4:2b0947ed0e0d | 341 | |
elmbed | 4:2b0947ed0e0d | 342 | void getRadioInput(char *data, uint16_t length) |
elmbed | 4:2b0947ed0e0d | 343 | { |
elmbed | 4:2b0947ed0e0d | 344 | static int i = 0; |
elmbed | 4:2b0947ed0e0d | 345 | static char parameter = '_'; |
elmbed | 4:2b0947ed0e0d | 346 | static char buffer[RADIO_MESSAGE_LEN + 1]; |
elmbed | 4:2b0947ed0e0d | 347 | int value = 0; |
elmbed | 4:2b0947ed0e0d | 348 | |
elmbed | 4:2b0947ed0e0d | 349 | // listen for commands coming over bluetooth |
elmbed | 4:2b0947ed0e0d | 350 | for (int j = 0; j < length; ++j){ |
elmbed | 4:2b0947ed0e0d | 351 | char ch = data[j]; |
elmbed | 4:2b0947ed0e0d | 352 | |
elmbed | 4:2b0947ed0e0d | 353 | pc.printf("looping: %c. %c\r\n", ch, parameter); |
elmbed | 4:2b0947ed0e0d | 354 | |
elmbed | 4:2b0947ed0e0d | 355 | // if ch is a message terminator, if parameter is anything other than '_', process the message |
elmbed | 4:2b0947ed0e0d | 356 | if((ch == '\r' || ch == ';' || ch == '\n')) |
elmbed | 4:2b0947ed0e0d | 357 | { |
elmbed | 4:2b0947ed0e0d | 358 | if(i > 0 && parameter != '_') |
elmbed | 4:2b0947ed0e0d | 359 | { |
elmbed | 4:2b0947ed0e0d | 360 | buffer[i-1] = 0; |
elmbed | 4:2b0947ed0e0d | 361 | value = atoi(buffer); |
elmbed | 4:2b0947ed0e0d | 362 | |
elmbed | 4:2b0947ed0e0d | 363 | interpret(parameter, value); |
elmbed | 4:2b0947ed0e0d | 364 | } |
elmbed | 4:2b0947ed0e0d | 365 | |
elmbed | 4:2b0947ed0e0d | 366 | // done sending, reset parameters |
elmbed | 4:2b0947ed0e0d | 367 | parameter = '_'; |
elmbed | 4:2b0947ed0e0d | 368 | buffer[0] = 0; |
elmbed | 4:2b0947ed0e0d | 369 | i=0; |
elmbed | 4:2b0947ed0e0d | 370 | } |
elmbed | 4:2b0947ed0e0d | 371 | else |
elmbed | 4:2b0947ed0e0d | 372 | { // got anything but a message terminator |
elmbed | 4:2b0947ed0e0d | 373 | if(i==0) |
elmbed | 4:2b0947ed0e0d | 374 | parameter = ch; // nothing accumulated yet |
elmbed | 4:2b0947ed0e0d | 375 | else |
elmbed | 4:2b0947ed0e0d | 376 | buffer[i-1] = ch; |
elmbed | 4:2b0947ed0e0d | 377 | |
elmbed | 4:2b0947ed0e0d | 378 | i++; |
elmbed | 4:2b0947ed0e0d | 379 | } |
elmbed | 4:2b0947ed0e0d | 380 | //UART.printf("%c: %s, %d\r\n",parameter, buffer, i); |
elmbed | 4:2b0947ed0e0d | 381 | } |
elmbed | 4:2b0947ed0e0d | 382 | } |
elmbed | 4:2b0947ed0e0d | 383 | |
elmbed | 4:2b0947ed0e0d | 384 | void interpret(char parameter, int value){ |
elmbed | 4:2b0947ed0e0d | 385 | |
elmbed | 4:2b0947ed0e0d | 386 | switch(parameter){ |
elmbed | 4:2b0947ed0e0d | 387 | case 'g': |
elmbed | 4:2b0947ed0e0d | 388 | if(value == 1){ |
elmbed | 4:2b0947ed0e0d | 389 | new_strike = false; |
elmbed | 4:2b0947ed0e0d | 390 | //active = true; |
elmbed | 4:2b0947ed0e0d | 391 | if(mode == STRIKE_DETECT_1)measure.attach_us(&strike_detect1, timestep*1000); |
elmbed | 4:2b0947ed0e0d | 392 | if(mode == STRIKE_DETECT_2)measure.attach_us(&strike_detect2, timestep*1000); |
elmbed | 4:2b0947ed0e0d | 393 | } |
elmbed | 4:2b0947ed0e0d | 394 | if(value == 0)measure.detach(); //active = false; |
elmbed | 4:2b0947ed0e0d | 395 | break; |
elmbed | 4:2b0947ed0e0d | 396 | case 'm': |
elmbed | 4:2b0947ed0e0d | 397 | mode = value; |
elmbed | 4:2b0947ed0e0d | 398 | break; |
elmbed | 4:2b0947ed0e0d | 399 | case 'p': |
elmbed | 4:2b0947ed0e0d | 400 | config_parameter = value; |
elmbed | 4:2b0947ed0e0d | 401 | //UART.printf("parameter is: %d\r\n", value); |
elmbed | 4:2b0947ed0e0d | 402 | break; |
elmbed | 4:2b0947ed0e0d | 403 | case 'v': |
elmbed | 4:2b0947ed0e0d | 404 | if(config_parameter < 8)set_setting(config_parameter, value); |
elmbed | 4:2b0947ed0e0d | 405 | else{ |
elmbed | 4:2b0947ed0e0d | 406 | //UART.printf("value is: %d\r\n", value); |
elmbed | 4:2b0947ed0e0d | 407 | if(config_parameter == 101){ |
elmbed | 4:2b0947ed0e0d | 408 | //fast_filter = value; |
elmbed | 4:2b0947ed0e0d | 409 | c1_1 = (float)value/1000.0; |
elmbed | 4:2b0947ed0e0d | 410 | c2_1 = 1.0 - c1_1; |
elmbed | 4:2b0947ed0e0d | 411 | } |
elmbed | 4:2b0947ed0e0d | 412 | if(config_parameter == 102){ |
elmbed | 4:2b0947ed0e0d | 413 | //slow_filter = value; |
elmbed | 4:2b0947ed0e0d | 414 | c1_2 = (float)value/1000.0; |
elmbed | 4:2b0947ed0e0d | 415 | c2_2 = 1.0 - c1_2; |
elmbed | 4:2b0947ed0e0d | 416 | } |
elmbed | 4:2b0947ed0e0d | 417 | if(config_parameter == 103)big_hit_threshold = value; |
elmbed | 4:2b0947ed0e0d | 418 | if(config_parameter == 104)small_hit_threshold = value; |
elmbed | 4:2b0947ed0e0d | 419 | if(config_parameter == 105)jerk_threshold = value; |
elmbed | 4:2b0947ed0e0d | 420 | if(config_parameter == 106){ |
elmbed | 4:2b0947ed0e0d | 421 | float x = value; |
elmbed | 4:2b0947ed0e0d | 422 | shadow_decay = 0.9 + x*0.0001; |
elmbed | 4:2b0947ed0e0d | 423 | //UART.printf("shadow_decay is: %f\r\n", shadow_decay); |
elmbed | 4:2b0947ed0e0d | 424 | } |
elmbed | 4:2b0947ed0e0d | 425 | } |
elmbed | 4:2b0947ed0e0d | 426 | break; |
elmbed | 4:2b0947ed0e0d | 427 | case 's': |
elmbed | 4:2b0947ed0e0d | 428 | if(value > 0){ |
elmbed | 4:2b0947ed0e0d | 429 | //measure.attach_us(&spew_accel, 1000); |
elmbed | 4:2b0947ed0e0d | 430 | //spew.attach(&spew_accel, float(value)/1000); |
elmbed | 4:2b0947ed0e0d | 431 | //measure.attach_us(&read_accel, 1000); |
elmbed | 4:2b0947ed0e0d | 432 | } |
elmbed | 4:2b0947ed0e0d | 433 | if(value == 0)spew.detach(); //active = false; |
elmbed | 4:2b0947ed0e0d | 434 | break; |
elmbed | 4:2b0947ed0e0d | 435 | case 't': |
elmbed | 4:2b0947ed0e0d | 436 | //y(k) = a * y(k-1) + (1-a) * x(k) |
elmbed | 4:2b0947ed0e0d | 437 | //a = exp (-T/tau) |
elmbed | 4:2b0947ed0e0d | 438 | break; |
elmbed | 4:2b0947ed0e0d | 439 | case 'z': |
elmbed | 4:2b0947ed0e0d | 440 | pc.printf(VERSION); |
elmbed | 4:2b0947ed0e0d | 441 | break; |
elmbed | 4:2b0947ed0e0d | 442 | default: |
elmbed | 4:2b0947ed0e0d | 443 | // print stuff |
elmbed | 4:2b0947ed0e0d | 444 | break; |
RedBearLab | 0:cffe8ac1bdf0 | 445 | } |
RedBearLab | 0:cffe8ac1bdf0 | 446 | } |
RedBearLab | 0:cffe8ac1bdf0 | 447 | |
RedBearLab | 0:cffe8ac1bdf0 | 448 | |
RedBearLab | 0:cffe8ac1bdf0 | 449 | |
RedBearLab | 0:cffe8ac1bdf0 | 450 | |
RedBearLab | 0:cffe8ac1bdf0 | 451 | |
RedBearLab | 0:cffe8ac1bdf0 | 452 | |
RedBearLab | 0:cffe8ac1bdf0 | 453 | |
RedBearLab | 0:cffe8ac1bdf0 | 454 | |
RedBearLab | 0:cffe8ac1bdf0 | 455 | |
RedBearLab | 0:cffe8ac1bdf0 | 456 | |
RedBearLab | 0:cffe8ac1bdf0 | 457 | |
RedBearLab | 0:cffe8ac1bdf0 | 458 | |
RedBearLab | 0:cffe8ac1bdf0 | 459 | |
RedBearLab | 0:cffe8ac1bdf0 | 460 | |
RedBearLab | 0:cffe8ac1bdf0 | 461 | |
RedBearLab | 0:cffe8ac1bdf0 | 462 |