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-03-14
- Revision:
- 5:7f89fca19a9e
- Parent:
- 3:7c90c4811843
File content as of revision 5:7f89fca19a9e:
/* * 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; } } void motor_break() { // break tb6612_pwma = 1; tb6612_ain1 = 1; tb6612_ain2 = 1; } // you must adjust center position #define RL_ADJ (-120) #define RL_CENTER (1500) #define RL_RANGE (200.0) // RC servo // rl : -1.0 ? 1.0 void servo (float rl) { servo_pwm.pulsewidth_us(RL_CENTER + RL_ADJ - (int)(RL_RANGE * rl)); } void connectionCallback(Gap::Handle_t handle, Gap::addr_type_t peerAddrType, const Gap::address_t peerAddr, const Gap::ConnectionParams_t *params) { servo(0); } void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) { //pc.printf("Disconnected \r\n"); //pc.printf("Restart advertising \r\n"); servo_pwm.pulsewidth_us(0); 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; if(!StopFlag) motor(m); if( StopFlag && (m == 0)) StopFlag = false; // motor(m); float s = (float)stickData.value.lr / 128.0; servo(s); } } // Distance sensor averaging number #define AVE_SIZE 8 // Distance constant (for Sharp GP2Y0E03) // Vout[V] = 2.369V - 0.0369*d[cm] // sample = Vout[V]/3.3V * 0xFFFF // sample = 47190 - 733.66*d[cm] // 10cm : 39853 / 64 = 623 // 20cm : 32516 / 64 = 508 // 30cm : 25180 / 64 = 393 // 40cm : 17843 / 64 = 278 // 50cm : 10507 / 64 = 164 // 60cm : 3170 / 64 = 50 // 64cm : 0 #define DISTANCE_STOP 278 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.read_u16(); r_acc -= (uint32_t)r_buff[cnt]; r_acc += (uint32_t)r; r_buff[cnt] = r; uint16_t l = sensor_l.read_u16(); 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); motor_break(); StopFlag = true; }else{ //StopFlag = false; } } } int main(void) { // initialize servo & motor servo_pwm.period_ms(20); servo_pwm.pulsewidth_us(0); // servo(0); motor(0); Ticker ticker; ticker.attach_us(TimerHandler, 20000); // 20msec interval // initialize BLE ble.init(); ble.onConnection(connectionCallback); 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(); } }