Toshiyuki Saito / Mbed 2 deprecated 4WD_Watcher

ミニ四駆魔改造ファームウェア

Dependencies:   ADXL345_I2C BLE_API mbed nRF51822

main.cpp@0:49972c65a3f8, 2016-01-07 (annotated)

Committer:
toshiyukisaito
Date:
Thu Jan 07 05:55:52 2016 +0000
Revision:
0:49972c65a3f8
version 1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
toshiyukisaito 0:49972c65a3f8 1 /*
toshiyukisaito 0:49972c65a3f8 2
toshiyukisaito 0:49972c65a3f8 3 Copyright (c) 2012-2014 RedBearLab
toshiyukisaito 0:49972c65a3f8 4
toshiyukisaito 0:49972c65a3f8 5 Permission is hereby granted, free of charge, to any person obtaining a copy of this software
toshiyukisaito 0:49972c65a3f8 6 and associated documentation files (the "Software"), to deal in the Software without restriction,
toshiyukisaito 0:49972c65a3f8 7 including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
toshiyukisaito 0:49972c65a3f8 8 and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
toshiyukisaito 0:49972c65a3f8 9 subject to the following conditions:
toshiyukisaito 0:49972c65a3f8 10 The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
toshiyukisaito 0:49972c65a3f8 11
toshiyukisaito 0:49972c65a3f8 12 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
toshiyukisaito 0:49972c65a3f8 13 INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
toshiyukisaito 0:49972c65a3f8 14 PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
toshiyukisaito 0:49972c65a3f8 15 FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
toshiyukisaito 0:49972c65a3f8 16 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
toshiyukisaito 0:49972c65a3f8 17
toshiyukisaito 0:49972c65a3f8 18 */
toshiyukisaito 0:49972c65a3f8 19
toshiyukisaito 0:49972c65a3f8 20 #include "mbed.h"
toshiyukisaito 0:49972c65a3f8 21 #include "ble/BLE.h"
toshiyukisaito 0:49972c65a3f8 22 #include "ADXL345_I2C/ADXL345_I2C.h"
toshiyukisaito 0:49972c65a3f8 23
toshiyukisaito 0:49972c65a3f8 24 #define TXRX_BUF_LEN 20
toshiyukisaito 0:49972c65a3f8 25 #define LOCAL_NAME "4WD"
toshiyukisaito 0:49972c65a3f8 26 #define NOTIFY_INTERVAL 200
toshiyukisaito 0:49972c65a3f8 27 #define MEASURE_INTERVAL 10
toshiyukisaito 0:49972c65a3f8 28
toshiyukisaito 0:49972c65a3f8 29 ADXL345_I2C accelerometer(P0_10, P0_8);
toshiyukisaito 0:49972c65a3f8 30 BLE ble;
toshiyukisaito 0:49972c65a3f8 31 DigitalOut led(P0_19);
toshiyukisaito 0:49972c65a3f8 32
toshiyukisaito 0:49972c65a3f8 33 static const uint8_t uart_base_uuid[] = {0xc8, 0xd6, 0xea, 0x62, 0x40, 0x76, 0x45, 0x61, 0xbf, 0xc8, 0x90, 0xaf, 0x4e, 0xd7, 0x8f, 0x4a};
toshiyukisaito 0:49972c65a3f8 34 static const uint8_t uart_tx_uuid[] = {0xd5, 0x31, 0x16, 0xfc, 0x4b, 0x96, 0x44, 0x46, 0xb8, 0x9f, 0xd9, 0x60, 0x00, 0x1c, 0xea, 0x91};
toshiyukisaito 0:49972c65a3f8 35 static const uint8_t uart_rx_uuid[] = {0xd9, 0x46, 0x28, 0x6e, 0xb6, 0xae, 0x4e, 0x75, 0x8c, 0x8d, 0xcb, 0x18, 0x43, 0x4a, 0x13, 0x98};
toshiyukisaito 0:49972c65a3f8 36 static uint8_t uart_base_uuid_rev[] = {0x4a, 0x8f, 0xd7, 0x4e, 0xaf, 0x90, 0xc8, 0xbf, 0x61, 0x45, 0x76, 0x40, 0x62, 0xea, 0xd6, 0xc8};
toshiyukisaito 0:49972c65a3f8 37
toshiyukisaito 0:49972c65a3f8 38 uint8_t txPayload[TXRX_BUF_LEN] = {0,};
toshiyukisaito 0:49972c65a3f8 39 uint8_t rxPayload[TXRX_BUF_LEN] = {0,};
toshiyukisaito 0:49972c65a3f8 40
toshiyukisaito 0:49972c65a3f8 41 GattCharacteristic txCharacteristic (uart_tx_uuid, txPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
toshiyukisaito 0:49972c65a3f8 42 GattCharacteristic rxCharacteristic (uart_rx_uuid, rxPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
toshiyukisaito 0:49972c65a3f8 43 GattCharacteristic *uartChars[] = {&txCharacteristic, &rxCharacteristic};
toshiyukisaito 0:49972c65a3f8 44 GattService uartService(uart_base_uuid, uartChars, sizeof(uartChars) / sizeof(GattCharacteristic *));
toshiyukisaito 0:49972c65a3f8 45
toshiyukisaito 0:49972c65a3f8 46 struct Vector3 {
toshiyukisaito 0:49972c65a3f8 47 float x;
toshiyukisaito 0:49972c65a3f8 48 float y;
toshiyukisaito 0:49972c65a3f8 49 float z;
toshiyukisaito 0:49972c65a3f8 50 Vector3():x(0),y(0),z(0){}
toshiyukisaito 0:49972c65a3f8 51 void clear(){x=y=z=0;}
toshiyukisaito 0:49972c65a3f8 52 };
toshiyukisaito 0:49972c65a3f8 53
toshiyukisaito 0:49972c65a3f8 54 struct NotifyData {
toshiyukisaito 0:49972c65a3f8 55 float x;
toshiyukisaito 0:49972c65a3f8 56 float y;
toshiyukisaito 0:49972c65a3f8 57 float z;
toshiyukisaito 0:49972c65a3f8 58 float speed;
toshiyukisaito 0:49972c65a3f8 59 };
toshiyukisaito 0:49972c65a3f8 60
toshiyukisaito 0:49972c65a3f8 61 NotifyData notifyData;
toshiyukisaito 0:49972c65a3f8 62
toshiyukisaito 0:49972c65a3f8 63 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t* disconnectionCallback) {
toshiyukisaito 0:49972c65a3f8 64 ble.startAdvertising();
toshiyukisaito 0:49972c65a3f8 65 }
toshiyukisaito 0:49972c65a3f8 66
toshiyukisaito 0:49972c65a3f8 67 void WrittenHandler(const GattWriteCallbackParams *Handler) {
toshiyukisaito 0:49972c65a3f8 68 }
toshiyukisaito 0:49972c65a3f8 69
toshiyukisaito 0:49972c65a3f8 70 void notifyValues() {
toshiyukisaito 0:49972c65a3f8 71 uint8_t buf[20];
toshiyukisaito 0:49972c65a3f8 72 memcpy(buf, &notifyData, sizeof(NotifyData));
toshiyukisaito 0:49972c65a3f8 73 ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, sizeof(NotifyData));
toshiyukisaito 0:49972c65a3f8 74 }
toshiyukisaito 0:49972c65a3f8 75
toshiyukisaito 0:49972c65a3f8 76 Vector3 rawValue;
toshiyukisaito 0:49972c65a3f8 77 int rawCount = 0;
toshiyukisaito 0:49972c65a3f8 78
toshiyukisaito 0:49972c65a3f8 79 void measure() {
toshiyukisaito 0:49972c65a3f8 80 int readings[3] = {0, 0, 0};
toshiyukisaito 0:49972c65a3f8 81 accelerometer.getOutput(readings);
toshiyukisaito 0:49972c65a3f8 82 // accelerometerの値は16bitの2の補数表現なので、自力で負数に変換する
toshiyukisaito 0:49972c65a3f8 83 for (int i = 0; i < 3; i++) {
toshiyukisaito 0:49972c65a3f8 84 if ((readings[i] & 0x8000) != 0) {
toshiyukisaito 0:49972c65a3f8 85 readings[i] = -((~(readings[i] & 0x7FFF)) & 0x7FFF);
toshiyukisaito 0:49972c65a3f8 86 }
toshiyukisaito 0:49972c65a3f8 87 }
toshiyukisaito 0:49972c65a3f8 88
toshiyukisaito 0:49972c65a3f8 89 rawValue.x += readings[0];
toshiyukisaito 0:49972c65a3f8 90 rawValue.y += readings[1];
toshiyukisaito 0:49972c65a3f8 91 rawValue.z += readings[2];
toshiyukisaito 0:49972c65a3f8 92 rawCount++;
toshiyukisaito 0:49972c65a3f8 93
toshiyukisaito 0:49972c65a3f8 94 if (rawCount >= NOTIFY_INTERVAL/MEASURE_INTERVAL) {
toshiyukisaito 0:49972c65a3f8 95 notifyData.x = rawValue.x / 255 / rawCount; // G
toshiyukisaito 0:49972c65a3f8 96 notifyData.y = rawValue.y / 255 / rawCount; // G
toshiyukisaito 0:49972c65a3f8 97 notifyData.z = rawValue.z / 255 / rawCount; // G
toshiyukisaito 0:49972c65a3f8 98 notifyData.speed += (rawValue.x / 255.0 * 9.8 * 3.6 / 2 / MEASURE_INTERVAL) - notifyData.speed; // km/h
toshiyukisaito 0:49972c65a3f8 99
toshiyukisaito 0:49972c65a3f8 100 rawValue.clear();
toshiyukisaito 0:49972c65a3f8 101 rawCount = 0;
toshiyukisaito 0:49972c65a3f8 102 }
toshiyukisaito 0:49972c65a3f8 103 }
toshiyukisaito 0:49972c65a3f8 104
toshiyukisaito 0:49972c65a3f8 105 int main(void) {
toshiyukisaito 0:49972c65a3f8 106
toshiyukisaito 0:49972c65a3f8 107 memset(&notifyData, 0, sizeof(NotifyData));
toshiyukisaito 0:49972c65a3f8 108
toshiyukisaito 0:49972c65a3f8 109 Ticker ticker;
toshiyukisaito 0:49972c65a3f8 110 ticker.attach_us(notifyValues, NOTIFY_INTERVAL * 1000);
toshiyukisaito 0:49972c65a3f8 111
toshiyukisaito 0:49972c65a3f8 112 Ticker ticker2;
toshiyukisaito 0:49972c65a3f8 113 ticker2.attach_us(measure, MEASURE_INTERVAL * 1000);
toshiyukisaito 0:49972c65a3f8 114
toshiyukisaito 0:49972c65a3f8 115 // Go into standby mode to configure the device.
toshiyukisaito 0:49972c65a3f8 116 accelerometer.setPowerControl(0x00);
toshiyukisaito 0:49972c65a3f8 117 // Full resolution, +/-16g, 4mg/LSB.
toshiyukisaito 0:49972c65a3f8 118 accelerometer.setDataFormatControl(0x0B);
toshiyukisaito 0:49972c65a3f8 119 // 100Hz data rate.(10msec)
toshiyukisaito 0:49972c65a3f8 120 accelerometer.setDataRate(ADXL345_100HZ);
toshiyukisaito 0:49972c65a3f8 121 // Measurement mode.
toshiyukisaito 0:49972c65a3f8 122 accelerometer.setPowerControl(0x08);
toshiyukisaito 0:49972c65a3f8 123
toshiyukisaito 0:49972c65a3f8 124 ble.init();
toshiyukisaito 0:49972c65a3f8 125 ble.onDisconnection(disconnectionCallback);
toshiyukisaito 0:49972c65a3f8 126 ble.onDataWritten(WrittenHandler);
toshiyukisaito 0:49972c65a3f8 127 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
toshiyukisaito 0:49972c65a3f8 128 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
toshiyukisaito 0:49972c65a3f8 129 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
toshiyukisaito 0:49972c65a3f8 130 (const uint8_t *)LOCAL_NAME, sizeof(LOCAL_NAME) - 1);
toshiyukisaito 0:49972c65a3f8 131 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
toshiyukisaito 0:49972c65a3f8 132 (const uint8_t *)uart_base_uuid_rev, sizeof(uart_base_uuid));
toshiyukisaito 0:49972c65a3f8 133 ble.setDeviceName(LOCAL_NAME);
toshiyukisaito 0:49972c65a3f8 134 ble.setAdvertisingInterval(1000/0.625);
toshiyukisaito 0:49972c65a3f8 135 ble.addService(uartService);
toshiyukisaito 0:49972c65a3f8 136 ble.startAdvertising();
toshiyukisaito 0:49972c65a3f8 137 led = 0;
toshiyukisaito 0:49972c65a3f8 138
toshiyukisaito 0:49972c65a3f8 139 while(1) {
toshiyukisaito 0:49972c65a3f8 140 ble.waitForEvent();
toshiyukisaito 0:49972c65a3f8 141 }
toshiyukisaito 0:49972c65a3f8 142 }