Bizan Nishimura / Mbed 2 deprecated MiniSteer3

AndroidのBLEラジコンプロポアプリ「BLEPropo」と接続し、RCサーボとDCモータを制御するプログラムです。 BLE Nanoで動作を確認しています。 BLEPropo → https://github.com/lipoyang/BLEPropo

Dependencies:   BLE_API mbed

BLEを使ったAndroid用ラジコンプロポアプリ「BLEPropo」に対応するBLE Nano用ファームウェアです。
BLEPropoは、GitHubにて公開中。
https://github.com/lipoyang/BLEPropo
/media/uploads/lipoyang/blepropo_ui.png
ラジコンは、mbed HRM1017とRCサーボやDCモータを組み合わせて作ります。
/media/uploads/lipoyang/ministeer3.jpg
回路図
/media/uploads/lipoyang/ministeer3.pdf

main.cpp@5:7f89fca19a9e, 2015-03-14 (annotated)

Committer:
lipoyang
Date:
Sat Mar 14 12:02:48 2015 +0000
Revision:
5:7f89fca19a9e
Parent:
3:7c90c4811843 
-convert nRF51822 library to a folder

Who changed what in which revision?

UserRevisionLine numberNew 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 }