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
main.cpp
- Committer:
- lipoyang
- Date:
- 2015-01-30
- Revision:
- 1:3f38c4bad274
- Parent:
- 0:c5082e68ff72
- Child:
- 2:9189fa810ef8
File content as of revision 1:3f38c4bad274:
/* * 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" 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 *)); // 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) AnalogIn sensor_l (P0_4); // SENS-L: P0_4 (A3) AnalogIn sensor_r (P0_5); // SENS-R: P0_5 (A4) // flag for safety bool StopFlag = false; // 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) { if (Handler->charHandle == charStick.getValueAttribute().getHandle()) { uint16_t bytesRead; ble.readCharacteristicValue(charStick.getValueAttribute().getHandle(), payload, &bytesRead); memcpy( &stickData.bytes[0], payload, sizeof(stickData)); //pc.printf("DATA:%02X %02X\n\r",stickData.bytes[0],stickData.bytes[1]); float m = (float)stickData.value.fb / 128.0; motor(m); // if(!StopFlag) motor(m); float s = (float)stickData.value.lr / 128.0; servo(s); } } // Distance sensor averaging number #define AVE_SIZE 8 // Distance constant // Vout[V] = 2.369V - 0.0369*d[cm] // sample = Vout[V]/3.3V * 0xFFFF // sample = 47190 - 733.66*d[cm] // 10cm : 39853 // 20cm : 32516 // 30cm : 25180 // 40cm : 17843 // 50cm : 10507 // 60cm : 3170 // 64cm : 0 #define DISTANCE_STOP 17843 void TimerHandler(void) { // Averaging of dinstance sensors' value static int cnt = 0; static uint16_t r_buff[AVE_SIZE] ={0,}; static uint16_t l_buff[AVE_SIZE] ={0,}; static uint32_t r_acc = 0; static uint32_t l_acc = 0; uint16_t r = sensor_r; r_acc -= (uint32_t)r_buff[cnt]; r_acc += (uint32_t)r; r_buff[cnt] = r; uint16_t l = sensor_l; l_acc -= (uint32_t)l_buff[cnt]; l_acc += (uint32_t)l; l_buff[cnt] = l; cnt++; if(cnt >= AVE_SIZE){ cnt = 0; uint16_t r_ave = (uint16_t)(r_acc / AVE_SIZE); uint16_t l_ave = (uint16_t)(l_acc / AVE_SIZE); // Stop! Obstacle Ahead if( (r_ave >= DISTANCE_STOP) && (l_ave >= DISTANCE_STOP) ) { motor(0); StopFlag = true; }else{ StopFlag = false; } } } int main(void) { // initialize servo & motor servo_pwm.period_ms(20); servo(0.5); motor(0); Ticker ticker; ticker.attach_us(TimerHandler, 20000); // 20msec interval // initialize BLE ble.init(); ble.onDisconnection(disconnectionCallback); ble.onDataWritten(WrittenHandler); //pc.baud(9600); //pc.printf("BLE initialized\r\n"); // 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, 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(); } }