AndroidのBLEラジコンプロポアプリ「BLEPropo」と接続し、RCサーボとDCモータを制御するプログラムです。 BLE Nanoで動作を確認しています。 BLEPropo → https://github.com/lipoyang/BLEPropo
BLEを使ったAndroid用ラジコンプロポアプリ「BLEPropo」に対応するBLE Nano用ファームウェアです。
BLEPropoは、GitHubにて公開中。
https://github.com/lipoyang/BLEPropo
ラジコンは、mbed HRM1017とRCサーボやDCモータを組み合わせて作ります。
回路図
/media/uploads/lipoyang/ministeer3.pdf
Diff: main.cpp
- Revision:
- 0:c5082e68ff72
- Child:
- 1:3f38c4bad274
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Jan 29 23:30:56 2015 +0000 @@ -0,0 +1,315 @@ +/* + * Copyright (C) 2015 Bizan Nishimura (@lipoyang) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "mbed.h" +#include "BLEDevice.h" +//#include "Servo.h" + + +//#define BLE_UUID_TXRX_SERVICE 0x0000 /**< The UUID of the Nordic UART Service. */ +//#define BLE_UUID_TX_CHARACTERISTIC 0x0002 /**< The UUID of the TX Characteristic. */ +//#define BLE_UUIDS_RX_CHARACTERISTIC 0x0003 /**< The UUID of the RX Characteristic. */ + +//#define TXRX_BUF_LEN 20 + +//#define DIGITAL_OUT_PIN P0_9 //TXD +//#define DIGITAL_IN_PIN P0_10 //CTS +//#define PWM_PIN P0_11 //RXD +//#define SERVO_PIN P0_8 //RTS +//#define ANALOG_IN_PIN P0_4 //P04 + +BLEDevice ble; + +// BluePropo service UUID +//static const uint16_t UUID_BLUEPROPO = 0xFFF0; +static const uint8_t UUID_BLUEPROPO[] = +{ 0xc4, 0x9d, 0xfd, 0x1b, 0x86, 0x04, 0x41, 0xd2, 0x89, 0x43, 0x13, 0x6f, 0x21, 0x4d, 0xd0, 0xbf }; + +// BluePropo::Stick characteristic UUID +//static const uint16_t UUID_BLUEPROPO_STICK = 0xFFF1; +static const uint8_t UUID_BLUEPROPO_STICK[] = +{ 0x74, 0x25, 0xfb, 0xa0, 0x72, 0x15, 0x41, 0x36, 0xaa, 0x3f, 0x07, 0x2a, 0xa0, 0x7d, 0x93, 0x54 }; + +// Device Name (for display) +#define DEVICE_NAME "MiniSteer BLE Nano" + +// BluePropo::Stick data structure +union StickData +{ + struct { + // F(-128)<- 0 ->B(+127) + signed char fb; + // L(-128)<- 0 ->R(+127) + signed char lr; + }value; + unsigned char bytes[2]; +}; +StickData stickData; + +// buffer for BluePropo payload +uint8_t payload[10] = {0,}; + +// BluePropo::Stick characteristic +GattCharacteristic charStick (UUID_BLUEPROPO_STICK, payload, 2, 2, + GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | + GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE); +// BluePropo characteristics set +GattCharacteristic *chars[] = {&charStick}; +// BluePropo service +GattService serviceBluePropo(UUID_BLUEPROPO, chars, sizeof(chars) / sizeof(GattCharacteristic *)); + + +//DigitalOut LED_SET(DIGITAL_OUT_PIN); +//DigitalIn BUTTON(DIGITAL_IN_PIN); +//PwmOut PWM(PWM_PIN); +//AnalogIn ANALOG(ANALOG_IN_PIN); +//Servo MYSERVO(SERVO_PIN); + +//Serial pc(USBTX, USBRX); + +//static uint8_t analog_enabled = 0; +//static uint8_t old_state = 0; + +// The Nordic UART Service +//static const uint8_t uart_base_uuid[] = {0x71, 0x3D, 0, 0, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E}; +//static const uint8_t uart_tx_uuid[] = {0x71, 0x3D, 0, 3, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E}; +//static const uint8_t uart_rx_uuid[] = {0x71, 0x3D, 0, 2, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E}; +//static const uint8_t uart_base_uuid_rev[] = {0x1E, 0x94, 0x8D, 0xF1, 0x48, 0x31, 0x94, 0xBA, 0x75, 0x4C, 0x3E, 0x50, 0, 0, 0x3D, 0x71}; + + + +//uint8_t txPayload[TXRX_BUF_LEN] = {0,}; +//uint8_t rxPayload[TXRX_BUF_LEN] = {0,}; + +//static uint8_t rx_buf[TXRX_BUF_LEN]; +//static uint8_t rx_len=0; + +/* +GattCharacteristic txCharacteristic (uart_tx_uuid, txPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE); + +GattCharacteristic rxCharacteristic (uart_rx_uuid, rxPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY); + +GattCharacteristic *uartChars[] = {&txCharacteristic, &rxCharacteristic}; + +GattService uartService(uart_base_uuid, uartChars, sizeof(uartChars) / sizeof(GattCharacteristic *)); +*/ + +// pin asign +DigitalOut tb6612_ain1(P0_10); // AIN1: P0_10 (D2) +DigitalOut tb6612_ain2(P0_9); // AIN2: P0_9 (D1) +PwmOut tb6612_pwma(P0_11); // PWMA: P0_11 (D0) +PwmOut servo_pwm (P0_8); // SERVO: P0_8 (D3) + +// SENS-L: P0_4 (A3) +// SENS-R: P0_5 (A4) + +// DC motor driver (TB6612) +void motor (float speed) +{ + if (speed > 0) { + // CW + tb6612_pwma = speed; + tb6612_ain1 = 1; + tb6612_ain2 = 0; + } else + if (speed < 0) { + // CCW + tb6612_pwma = - speed; + tb6612_ain1 = 0; + tb6612_ain2 = 1; + } else { + // stop + tb6612_pwma = 1; + tb6612_ain1 = 0; + tb6612_ain2 = 0; +// // break +// tb6612_pwma = 1; +// tb6612_ain1 = 1; +// tb6612_ain2 = 1; + } +} + +// RC servo +void servo (float deg) +{ + servo_pwm.pulsewidth_us(1500 + (int)(500.0 * deg)); +} + +void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) +{ + //pc.printf("Disconnected \r\n"); + //pc.printf("Restart advertising \r\n"); + ble.startAdvertising(); +} + +void WrittenHandler(const GattCharacteristicWriteCBParams *Handler) +{ +// uint8_t buf[TXRX_BUF_LEN]; +// uint16_t bytesRead; + + if (Handler->charHandle == charStick.getValueAttribute().getHandle()) + { + uint16_t bytesRead; + ble.readCharacteristicValue(charStick.getValueAttribute().getHandle(), payload, &bytesRead); + memcpy( &stickData.bytes[0], payload, sizeof(stickData)); +#if DBG + + pc.printf("DATA:%02X %02X\n\r",stickData.bytes[0],stickData.bytes[1]); +#endif + float m = (float)stickData.value.fb / 128.0; + motor(m); + float s = (float)stickData.value.lr / 128.0; + servo(s); + +/* + ble.readCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(), buf, &bytesRead); + memset(txPayload, 0, TXRX_BUF_LEN); + memcpy(txPayload, buf, TXRX_BUF_LEN); + + //for(index=0; index<bytesRead; index++) + //pc.putc(buf[index]); + + if(buf[0] == 0x01) + { + if(buf[1] == 0x01) + LED_SET = 1; + else + LED_SET = 0; + } + else if(buf[0] == 0xA0) + { + if(buf[1] == 0x01) + analog_enabled = 1; + else + analog_enabled = 0; + } + else if(buf[0] == 0x02) + { + float value = (float)buf[1]/255; + PWM = value; + } + else if(buf[0] == 0x03) + { + MYSERVO.write(buf[1]); + } + else if(buf[0] == 0x04) + { + analog_enabled = 0; + PWM = 0; + MYSERVO.write(0); + LED_SET = 0; + old_state = 0; + } +*/ + } +} +/* +void uartCB(void) +{ + while(pc.readable()) + { + rx_buf[rx_len++] = pc.getc(); + if(rx_len>=20 || rx_buf[rx_len-1]=='\0' || rx_buf[rx_len-1]=='\n') + { + ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), rx_buf, rx_len); + pc.printf("RecHandler \r\n"); + pc.printf("Length: "); + pc.putc(rx_len); + pc.printf("\r\n"); + rx_len = 0; + break; + } + } +} +*/ +void m_status_check_handle(void) +{ +/* + uint8_t buf[3]; + if (analog_enabled) // if analog reading enabled + { + // Read and send out + float s = ANALOG; + uint16_t value = s*1024; + buf[0] = (0x0B); + buf[1] = (value >> 8); + buf[2] = (value); + ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, 3); + } + + // If digital in changes, report the state + if (BUTTON != old_state) + { + old_state = BUTTON; + + if (BUTTON == 1) + { + buf[0] = (0x0A); + buf[1] = (0x01); + buf[2] = (0x00); + ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, 3); + } + else + { + buf[0] = (0x0A); + buf[1] = (0x00); + buf[2] = (0x00); + ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, 3); + } + } +*/ +} + + +int main(void) +{ + // initialize servo & motor + servo_pwm.period_ms(20); + servo(0.5); + motor(0); + + Ticker ticker; + ticker.attach_us(m_status_check_handle, 200000); + + // initialize BLE + ble.init(); + ble.onDisconnection(disconnectionCallback); + ble.onDataWritten(WrittenHandler); + + //pc.baud(9600); + //pc.printf("SimpleChat Init \r\n"); + //pc.attach( uartCB , pc.RxIrq); + + // setup advertising + ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED); + ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); + ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME, + (const uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME) - 1); +// ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS, +// (const uint8_t *)uart_base_uuid_rev, sizeof(uart_base_uuid)); + ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, + (const uint8_t *)UUID_BLUEPROPO, sizeof(UUID_BLUEPROPO)); + ble.setAdvertisingInterval(160); // 100ms; in multiples of 0.625ms. + ble.addService(serviceBluePropo); + ble.startAdvertising(); + //pc.printf("Advertising Start \r\n"); + + // main loop (wait for BLE event) + while(true) + { + ble.waitForEvent(); + } +}