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@3:7c90c4811843, 2015-03-14 (annotated)
- Committer:
- lipoyang
- Date:
- Sat Mar 14 11:02:38 2015 +0000
- Revision:
- 3:7c90c4811843
- Parent:
- 2:9189fa810ef8
??????????????????????; ?BLE?????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
lipoyang | 0:c5082e68ff72 | 1 | /* |
lipoyang | 0:c5082e68ff72 | 2 | * Copyright (C) 2015 Bizan Nishimura (@lipoyang) |
lipoyang | 0:c5082e68ff72 | 3 | * |
lipoyang | 0:c5082e68ff72 | 4 | * Licensed under the Apache License, Version 2.0 (the "License"); |
lipoyang | 0:c5082e68ff72 | 5 | * you may not use this file except in compliance with the License. |
lipoyang | 0:c5082e68ff72 | 6 | * You may obtain a copy of the License at |
lipoyang | 0:c5082e68ff72 | 7 | * |
lipoyang | 0:c5082e68ff72 | 8 | * http://www.apache.org/licenses/LICENSE-2.0 |
lipoyang | 0:c5082e68ff72 | 9 | * |
lipoyang | 0:c5082e68ff72 | 10 | * Unless required by applicable law or agreed to in writing, software |
lipoyang | 0:c5082e68ff72 | 11 | * distributed under the License is distributed on an "AS IS" BASIS, |
lipoyang | 0:c5082e68ff72 | 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
lipoyang | 0:c5082e68ff72 | 13 | * See the License for the specific language governing permissions and |
lipoyang | 0:c5082e68ff72 | 14 | * limitations under the License. |
lipoyang | 0:c5082e68ff72 | 15 | */ |
lipoyang | 0:c5082e68ff72 | 16 | |
lipoyang | 0:c5082e68ff72 | 17 | #include "mbed.h" |
lipoyang | 0:c5082e68ff72 | 18 | #include "BLEDevice.h" |
lipoyang | 0:c5082e68ff72 | 19 | |
lipoyang | 0:c5082e68ff72 | 20 | BLEDevice ble; |
lipoyang | 0:c5082e68ff72 | 21 | |
lipoyang | 0:c5082e68ff72 | 22 | // BluePropo service UUID |
lipoyang | 0:c5082e68ff72 | 23 | //static const uint16_t UUID_BLUEPROPO = 0xFFF0; |
lipoyang | 0:c5082e68ff72 | 24 | static const uint8_t UUID_BLUEPROPO[] = |
lipoyang | 0:c5082e68ff72 | 25 | { 0xc4, 0x9d, 0xfd, 0x1b, 0x86, 0x04, 0x41, 0xd2, 0x89, 0x43, 0x13, 0x6f, 0x21, 0x4d, 0xd0, 0xbf }; |
lipoyang | 0:c5082e68ff72 | 26 | |
lipoyang | 0:c5082e68ff72 | 27 | // BluePropo::Stick characteristic UUID |
lipoyang | 0:c5082e68ff72 | 28 | //static const uint16_t UUID_BLUEPROPO_STICK = 0xFFF1; |
lipoyang | 0:c5082e68ff72 | 29 | static const uint8_t UUID_BLUEPROPO_STICK[] = |
lipoyang | 0:c5082e68ff72 | 30 | { 0x74, 0x25, 0xfb, 0xa0, 0x72, 0x15, 0x41, 0x36, 0xaa, 0x3f, 0x07, 0x2a, 0xa0, 0x7d, 0x93, 0x54 }; |
lipoyang | 0:c5082e68ff72 | 31 | |
lipoyang | 0:c5082e68ff72 | 32 | // Device Name (for display) |
lipoyang | 0:c5082e68ff72 | 33 | #define DEVICE_NAME "MiniSteer BLE Nano" |
lipoyang | 0:c5082e68ff72 | 34 | |
lipoyang | 0:c5082e68ff72 | 35 | // BluePropo::Stick data structure |
lipoyang | 0:c5082e68ff72 | 36 | union StickData |
lipoyang | 0:c5082e68ff72 | 37 | { |
lipoyang | 0:c5082e68ff72 | 38 | struct { |
lipoyang | 0:c5082e68ff72 | 39 | // F(-128)<- 0 ->B(+127) |
lipoyang | 0:c5082e68ff72 | 40 | signed char fb; |
lipoyang | 0:c5082e68ff72 | 41 | // L(-128)<- 0 ->R(+127) |
lipoyang | 0:c5082e68ff72 | 42 | signed char lr; |
lipoyang | 0:c5082e68ff72 | 43 | }value; |
lipoyang | 0:c5082e68ff72 | 44 | unsigned char bytes[2]; |
lipoyang | 0:c5082e68ff72 | 45 | }; |
lipoyang | 0:c5082e68ff72 | 46 | StickData stickData; |
lipoyang | 0:c5082e68ff72 | 47 | |
lipoyang | 0:c5082e68ff72 | 48 | // buffer for BluePropo payload |
lipoyang | 0:c5082e68ff72 | 49 | uint8_t payload[10] = {0,}; |
lipoyang | 0:c5082e68ff72 | 50 | |
lipoyang | 0:c5082e68ff72 | 51 | // BluePropo::Stick characteristic |
lipoyang | 0:c5082e68ff72 | 52 | GattCharacteristic charStick (UUID_BLUEPROPO_STICK, payload, 2, 2, |
lipoyang | 0:c5082e68ff72 | 53 | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | |
lipoyang | 0:c5082e68ff72 | 54 | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE); |
lipoyang | 0:c5082e68ff72 | 55 | // BluePropo characteristics set |
lipoyang | 0:c5082e68ff72 | 56 | GattCharacteristic *chars[] = {&charStick}; |
lipoyang | 0:c5082e68ff72 | 57 | // BluePropo service |
lipoyang | 0:c5082e68ff72 | 58 | GattService serviceBluePropo(UUID_BLUEPROPO, chars, sizeof(chars) / sizeof(GattCharacteristic *)); |
lipoyang | 0:c5082e68ff72 | 59 | |
lipoyang | 1:3f38c4bad274 | 60 | // pin asign |
lipoyang | 1:3f38c4bad274 | 61 | DigitalOut tb6612_ain1(P0_10); // AIN1: P0_10 (D2) |
lipoyang | 1:3f38c4bad274 | 62 | DigitalOut tb6612_ain2(P0_9); // AIN2: P0_9 (D1) |
lipoyang | 1:3f38c4bad274 | 63 | PwmOut tb6612_pwma(P0_11); // PWMA: P0_11 (D0) |
lipoyang | 1:3f38c4bad274 | 64 | PwmOut servo_pwm (P0_8); // SERVO: P0_8 (D3) |
lipoyang | 1:3f38c4bad274 | 65 | AnalogIn sensor_l (P0_4); // SENS-L: P0_4 (A3) |
lipoyang | 1:3f38c4bad274 | 66 | AnalogIn sensor_r (P0_5); // SENS-R: P0_5 (A4) |
lipoyang | 0:c5082e68ff72 | 67 | |
lipoyang | 1:3f38c4bad274 | 68 | // flag for safety |
lipoyang | 1:3f38c4bad274 | 69 | bool StopFlag = false; |
lipoyang | 0:c5082e68ff72 | 70 | |
lipoyang | 0:c5082e68ff72 | 71 | // DC motor driver (TB6612) |
lipoyang | 0:c5082e68ff72 | 72 | void motor (float speed) |
lipoyang | 0:c5082e68ff72 | 73 | { |
lipoyang | 0:c5082e68ff72 | 74 | if (speed > 0) { |
lipoyang | 0:c5082e68ff72 | 75 | // CW |
lipoyang | 0:c5082e68ff72 | 76 | tb6612_pwma = speed; |
lipoyang | 0:c5082e68ff72 | 77 | tb6612_ain1 = 1; |
lipoyang | 0:c5082e68ff72 | 78 | tb6612_ain2 = 0; |
lipoyang | 0:c5082e68ff72 | 79 | } else |
lipoyang | 0:c5082e68ff72 | 80 | if (speed < 0) { |
lipoyang | 0:c5082e68ff72 | 81 | // CCW |
lipoyang | 0:c5082e68ff72 | 82 | tb6612_pwma = - speed; |
lipoyang | 0:c5082e68ff72 | 83 | tb6612_ain1 = 0; |
lipoyang | 0:c5082e68ff72 | 84 | tb6612_ain2 = 1; |
lipoyang | 0:c5082e68ff72 | 85 | } else { |
lipoyang | 0:c5082e68ff72 | 86 | // stop |
lipoyang | 0:c5082e68ff72 | 87 | tb6612_pwma = 1; |
lipoyang | 0:c5082e68ff72 | 88 | tb6612_ain1 = 0; |
lipoyang | 0:c5082e68ff72 | 89 | tb6612_ain2 = 0; |
lipoyang | 0:c5082e68ff72 | 90 | } |
lipoyang | 0:c5082e68ff72 | 91 | } |
lipoyang | 0:c5082e68ff72 | 92 | |
lipoyang | 3:7c90c4811843 | 93 | void motor_break() |
lipoyang | 3:7c90c4811843 | 94 | { |
lipoyang | 3:7c90c4811843 | 95 | // break |
lipoyang | 3:7c90c4811843 | 96 | tb6612_pwma = 1; |
lipoyang | 3:7c90c4811843 | 97 | tb6612_ain1 = 1; |
lipoyang | 3:7c90c4811843 | 98 | tb6612_ain2 = 1; |
lipoyang | 3:7c90c4811843 | 99 | } |
lipoyang | 3:7c90c4811843 | 100 | |
lipoyang | 2:9189fa810ef8 | 101 | // you must adjust center position |
lipoyang | 3:7c90c4811843 | 102 | #define RL_ADJ (-120) |
lipoyang | 3:7c90c4811843 | 103 | #define RL_CENTER (1500) |
lipoyang | 3:7c90c4811843 | 104 | #define RL_RANGE (200.0) |
lipoyang | 2:9189fa810ef8 | 105 | |
lipoyang | 0:c5082e68ff72 | 106 | // RC servo |
lipoyang | 2:9189fa810ef8 | 107 | // rl : -1.0 ? 1.0 |
lipoyang | 2:9189fa810ef8 | 108 | void servo (float rl) |
lipoyang | 0:c5082e68ff72 | 109 | { |
lipoyang | 3:7c90c4811843 | 110 | servo_pwm.pulsewidth_us(RL_CENTER + RL_ADJ - (int)(RL_RANGE * rl)); |
lipoyang | 3:7c90c4811843 | 111 | } |
lipoyang | 3:7c90c4811843 | 112 | |
lipoyang | 3:7c90c4811843 | 113 | void connectionCallback(Gap::Handle_t handle, Gap::addr_type_t peerAddrType, const Gap::address_t peerAddr, const Gap::ConnectionParams_t *params) |
lipoyang | 3:7c90c4811843 | 114 | { |
lipoyang | 3:7c90c4811843 | 115 | servo(0); |
lipoyang | 0:c5082e68ff72 | 116 | } |
lipoyang | 0:c5082e68ff72 | 117 | |
lipoyang | 0:c5082e68ff72 | 118 | void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) |
lipoyang | 0:c5082e68ff72 | 119 | { |
lipoyang | 0:c5082e68ff72 | 120 | //pc.printf("Disconnected \r\n"); |
lipoyang | 0:c5082e68ff72 | 121 | //pc.printf("Restart advertising \r\n"); |
lipoyang | 1:3f38c4bad274 | 122 | |
lipoyang | 3:7c90c4811843 | 123 | servo_pwm.pulsewidth_us(0); |
lipoyang | 3:7c90c4811843 | 124 | |
lipoyang | 0:c5082e68ff72 | 125 | ble.startAdvertising(); |
lipoyang | 0:c5082e68ff72 | 126 | } |
lipoyang | 0:c5082e68ff72 | 127 | |
lipoyang | 0:c5082e68ff72 | 128 | void WrittenHandler(const GattCharacteristicWriteCBParams *Handler) |
lipoyang | 1:3f38c4bad274 | 129 | { |
lipoyang | 0:c5082e68ff72 | 130 | if (Handler->charHandle == charStick.getValueAttribute().getHandle()) |
lipoyang | 0:c5082e68ff72 | 131 | { |
lipoyang | 0:c5082e68ff72 | 132 | uint16_t bytesRead; |
lipoyang | 0:c5082e68ff72 | 133 | ble.readCharacteristicValue(charStick.getValueAttribute().getHandle(), payload, &bytesRead); |
lipoyang | 0:c5082e68ff72 | 134 | memcpy( &stickData.bytes[0], payload, sizeof(stickData)); |
lipoyang | 1:3f38c4bad274 | 135 | |
lipoyang | 1:3f38c4bad274 | 136 | //pc.printf("DATA:%02X %02X\n\r",stickData.bytes[0],stickData.bytes[1]); |
lipoyang | 1:3f38c4bad274 | 137 | |
lipoyang | 0:c5082e68ff72 | 138 | float m = (float)stickData.value.fb / 128.0; |
lipoyang | 3:7c90c4811843 | 139 | if(!StopFlag) motor(m); |
lipoyang | 3:7c90c4811843 | 140 | if( StopFlag && (m == 0)) StopFlag = false; |
lipoyang | 3:7c90c4811843 | 141 | // motor(m); |
lipoyang | 0:c5082e68ff72 | 142 | float s = (float)stickData.value.lr / 128.0; |
lipoyang | 0:c5082e68ff72 | 143 | servo(s); |
lipoyang | 0:c5082e68ff72 | 144 | } |
lipoyang | 0:c5082e68ff72 | 145 | } |
lipoyang | 1:3f38c4bad274 | 146 | |
lipoyang | 1:3f38c4bad274 | 147 | // Distance sensor averaging number |
lipoyang | 1:3f38c4bad274 | 148 | #define AVE_SIZE 8 |
lipoyang | 1:3f38c4bad274 | 149 | |
lipoyang | 2:9189fa810ef8 | 150 | // Distance constant (for Sharp GP2Y0E03) |
lipoyang | 1:3f38c4bad274 | 151 | // Vout[V] = 2.369V - 0.0369*d[cm] |
lipoyang | 1:3f38c4bad274 | 152 | // sample = Vout[V]/3.3V * 0xFFFF |
lipoyang | 1:3f38c4bad274 | 153 | // sample = 47190 - 733.66*d[cm] |
lipoyang | 3:7c90c4811843 | 154 | // 10cm : 39853 / 64 = 623 |
lipoyang | 3:7c90c4811843 | 155 | // 20cm : 32516 / 64 = 508 |
lipoyang | 3:7c90c4811843 | 156 | // 30cm : 25180 / 64 = 393 |
lipoyang | 3:7c90c4811843 | 157 | // 40cm : 17843 / 64 = 278 |
lipoyang | 3:7c90c4811843 | 158 | // 50cm : 10507 / 64 = 164 |
lipoyang | 3:7c90c4811843 | 159 | // 60cm : 3170 / 64 = 50 |
lipoyang | 1:3f38c4bad274 | 160 | // 64cm : 0 |
lipoyang | 3:7c90c4811843 | 161 | #define DISTANCE_STOP 278 |
lipoyang | 1:3f38c4bad274 | 162 | |
lipoyang | 1:3f38c4bad274 | 163 | void TimerHandler(void) |
lipoyang | 1:3f38c4bad274 | 164 | { |
lipoyang | 1:3f38c4bad274 | 165 | // Averaging of dinstance sensors' value |
lipoyang | 1:3f38c4bad274 | 166 | static int cnt = 0; |
lipoyang | 1:3f38c4bad274 | 167 | static uint16_t r_buff[AVE_SIZE] ={0,}; |
lipoyang | 1:3f38c4bad274 | 168 | static uint16_t l_buff[AVE_SIZE] ={0,}; |
lipoyang | 1:3f38c4bad274 | 169 | static uint32_t r_acc = 0; |
lipoyang | 1:3f38c4bad274 | 170 | static uint32_t l_acc = 0; |
lipoyang | 1:3f38c4bad274 | 171 | |
lipoyang | 3:7c90c4811843 | 172 | uint16_t r = sensor_r.read_u16(); |
lipoyang | 1:3f38c4bad274 | 173 | r_acc -= (uint32_t)r_buff[cnt]; |
lipoyang | 1:3f38c4bad274 | 174 | r_acc += (uint32_t)r; |
lipoyang | 1:3f38c4bad274 | 175 | r_buff[cnt] = r; |
lipoyang | 1:3f38c4bad274 | 176 | |
lipoyang | 3:7c90c4811843 | 177 | uint16_t l = sensor_l.read_u16(); |
lipoyang | 1:3f38c4bad274 | 178 | l_acc -= (uint32_t)l_buff[cnt]; |
lipoyang | 1:3f38c4bad274 | 179 | l_acc += (uint32_t)l; |
lipoyang | 1:3f38c4bad274 | 180 | l_buff[cnt] = l; |
lipoyang | 1:3f38c4bad274 | 181 | |
lipoyang | 1:3f38c4bad274 | 182 | cnt++; |
lipoyang | 1:3f38c4bad274 | 183 | if(cnt >= AVE_SIZE){ |
lipoyang | 1:3f38c4bad274 | 184 | cnt = 0; |
lipoyang | 1:3f38c4bad274 | 185 | uint16_t r_ave = (uint16_t)(r_acc / AVE_SIZE); |
lipoyang | 1:3f38c4bad274 | 186 | uint16_t l_ave = (uint16_t)(l_acc / AVE_SIZE); |
lipoyang | 1:3f38c4bad274 | 187 | // Stop! Obstacle Ahead |
lipoyang | 1:3f38c4bad274 | 188 | if( (r_ave >= DISTANCE_STOP) && (l_ave >= DISTANCE_STOP) ) |
lipoyang | 0:c5082e68ff72 | 189 | { |
lipoyang | 3:7c90c4811843 | 190 | //motor(0); |
lipoyang | 3:7c90c4811843 | 191 | motor_break(); |
lipoyang | 1:3f38c4bad274 | 192 | StopFlag = true; |
lipoyang | 1:3f38c4bad274 | 193 | }else{ |
lipoyang | 3:7c90c4811843 | 194 | //StopFlag = false; |
lipoyang | 0:c5082e68ff72 | 195 | } |
lipoyang | 0:c5082e68ff72 | 196 | } |
lipoyang | 0:c5082e68ff72 | 197 | } |
lipoyang | 0:c5082e68ff72 | 198 | |
lipoyang | 0:c5082e68ff72 | 199 | int main(void) |
lipoyang | 0:c5082e68ff72 | 200 | { |
lipoyang | 0:c5082e68ff72 | 201 | // initialize servo & motor |
lipoyang | 0:c5082e68ff72 | 202 | servo_pwm.period_ms(20); |
lipoyang | 3:7c90c4811843 | 203 | servo_pwm.pulsewidth_us(0); |
lipoyang | 3:7c90c4811843 | 204 | // servo(0); |
lipoyang | 0:c5082e68ff72 | 205 | motor(0); |
lipoyang | 0:c5082e68ff72 | 206 | |
lipoyang | 0:c5082e68ff72 | 207 | Ticker ticker; |
lipoyang | 1:3f38c4bad274 | 208 | ticker.attach_us(TimerHandler, 20000); // 20msec interval |
lipoyang | 0:c5082e68ff72 | 209 | |
lipoyang | 0:c5082e68ff72 | 210 | // initialize BLE |
lipoyang | 0:c5082e68ff72 | 211 | ble.init(); |
lipoyang | 3:7c90c4811843 | 212 | |
lipoyang | 3:7c90c4811843 | 213 | ble.onConnection(connectionCallback); |
lipoyang | 0:c5082e68ff72 | 214 | ble.onDisconnection(disconnectionCallback); |
lipoyang | 0:c5082e68ff72 | 215 | ble.onDataWritten(WrittenHandler); |
lipoyang | 0:c5082e68ff72 | 216 | |
lipoyang | 0:c5082e68ff72 | 217 | //pc.baud(9600); |
lipoyang | 1:3f38c4bad274 | 218 | //pc.printf("BLE initialized\r\n"); |
lipoyang | 0:c5082e68ff72 | 219 | |
lipoyang | 0:c5082e68ff72 | 220 | // setup advertising |
lipoyang | 0:c5082e68ff72 | 221 | ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED); |
lipoyang | 0:c5082e68ff72 | 222 | ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
lipoyang | 0:c5082e68ff72 | 223 | ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME, |
lipoyang | 0:c5082e68ff72 | 224 | (const uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME) - 1); |
lipoyang | 3:7c90c4811843 | 225 | ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS, |
lipoyang | 3:7c90c4811843 | 226 | // ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, |
lipoyang | 0:c5082e68ff72 | 227 | (const uint8_t *)UUID_BLUEPROPO, sizeof(UUID_BLUEPROPO)); |
lipoyang | 0:c5082e68ff72 | 228 | ble.setAdvertisingInterval(160); // 100ms; in multiples of 0.625ms. |
lipoyang | 0:c5082e68ff72 | 229 | ble.addService(serviceBluePropo); |
lipoyang | 0:c5082e68ff72 | 230 | ble.startAdvertising(); |
lipoyang | 0:c5082e68ff72 | 231 | //pc.printf("Advertising Start \r\n"); |
lipoyang | 0:c5082e68ff72 | 232 | |
lipoyang | 0:c5082e68ff72 | 233 | // main loop (wait for BLE event) |
lipoyang | 0:c5082e68ff72 | 234 | while(true) |
lipoyang | 0:c5082e68ff72 | 235 | { |
lipoyang | 0:c5082e68ff72 | 236 | ble.waitForEvent(); |
lipoyang | 0:c5082e68ff72 | 237 | } |
lipoyang | 0:c5082e68ff72 | 238 | } |