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@2:9189fa810ef8, 2015-02-21 (annotated)

Committer:
lipoyang
Date:
Sat Feb 21 07:55:05 2015 +0000
Revision:
2:9189fa810ef8
Parent:
1:3f38c4bad274
Child:
3:7c90c4811843
??????????????

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 // // break
lipoyang 0:c5082e68ff72 91 // tb6612_pwma = 1;
lipoyang 0:c5082e68ff72 92 // tb6612_ain1 = 1;
lipoyang 0:c5082e68ff72 93 // tb6612_ain2 = 1;
lipoyang 0:c5082e68ff72 94 }
lipoyang 0:c5082e68ff72 95 }
lipoyang 0:c5082e68ff72 96
lipoyang 2:9189fa810ef8 97 // you must adjust center position
lipoyang 2:9189fa810ef8 98 #define RL_ADJ (-120)
lipoyang 2:9189fa810ef8 99
lipoyang 0:c5082e68ff72 100 // RC servo
lipoyang 2:9189fa810ef8 101 // rl : -1.0 ? 1.0
lipoyang 2:9189fa810ef8 102 void servo (float rl)
lipoyang 0:c5082e68ff72 103 {
lipoyang 2:9189fa810ef8 104 servo_pwm.pulsewidth_us(1500 + RL_ADJ - (int)(500.0 * rl));
lipoyang 0:c5082e68ff72 105 }
lipoyang 0:c5082e68ff72 106
lipoyang 0:c5082e68ff72 107 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
lipoyang 0:c5082e68ff72 108 {
lipoyang 0:c5082e68ff72 109 //pc.printf("Disconnected \r\n");
lipoyang 0:c5082e68ff72 110 //pc.printf("Restart advertising \r\n");
lipoyang 1:3f38c4bad274 111
lipoyang 0:c5082e68ff72 112 ble.startAdvertising();
lipoyang 0:c5082e68ff72 113 }
lipoyang 0:c5082e68ff72 114
lipoyang 0:c5082e68ff72 115 void WrittenHandler(const GattCharacteristicWriteCBParams *Handler)
lipoyang 1:3f38c4bad274 116 {
lipoyang 0:c5082e68ff72 117 if (Handler->charHandle == charStick.getValueAttribute().getHandle())
lipoyang 0:c5082e68ff72 118 {
lipoyang 0:c5082e68ff72 119 uint16_t bytesRead;
lipoyang 0:c5082e68ff72 120 ble.readCharacteristicValue(charStick.getValueAttribute().getHandle(), payload, &bytesRead);
lipoyang 0:c5082e68ff72 121 memcpy( &stickData.bytes[0], payload, sizeof(stickData));
lipoyang 1:3f38c4bad274 122
lipoyang 1:3f38c4bad274 123 //pc.printf("DATA:%02X %02X\n\r",stickData.bytes[0],stickData.bytes[1]);
lipoyang 1:3f38c4bad274 124
lipoyang 0:c5082e68ff72 125 float m = (float)stickData.value.fb / 128.0;
lipoyang 0:c5082e68ff72 126 motor(m);
lipoyang 1:3f38c4bad274 127 // if(!StopFlag) motor(m);
lipoyang 0:c5082e68ff72 128 float s = (float)stickData.value.lr / 128.0;
lipoyang 0:c5082e68ff72 129 servo(s);
lipoyang 0:c5082e68ff72 130 }
lipoyang 0:c5082e68ff72 131 }
lipoyang 1:3f38c4bad274 132
lipoyang 1:3f38c4bad274 133 // Distance sensor averaging number
lipoyang 1:3f38c4bad274 134 #define AVE_SIZE 8
lipoyang 1:3f38c4bad274 135
lipoyang 2:9189fa810ef8 136 // Distance constant (for Sharp GP2Y0E03)
lipoyang 1:3f38c4bad274 137 // Vout[V] = 2.369V - 0.0369*d[cm]
lipoyang 1:3f38c4bad274 138 // sample = Vout[V]/3.3V * 0xFFFF
lipoyang 1:3f38c4bad274 139 // sample = 47190 - 733.66*d[cm]
lipoyang 1:3f38c4bad274 140 // 10cm : 39853
lipoyang 1:3f38c4bad274 141 // 20cm : 32516
lipoyang 1:3f38c4bad274 142 // 30cm : 25180
lipoyang 1:3f38c4bad274 143 // 40cm : 17843
lipoyang 1:3f38c4bad274 144 // 50cm : 10507
lipoyang 1:3f38c4bad274 145 // 60cm : 3170
lipoyang 1:3f38c4bad274 146 // 64cm : 0
lipoyang 1:3f38c4bad274 147 #define DISTANCE_STOP 17843
lipoyang 1:3f38c4bad274 148
lipoyang 1:3f38c4bad274 149 void TimerHandler(void)
lipoyang 1:3f38c4bad274 150 {
lipoyang 1:3f38c4bad274 151 // Averaging of dinstance sensors' value
lipoyang 1:3f38c4bad274 152 static int cnt = 0;
lipoyang 1:3f38c4bad274 153 static uint16_t r_buff[AVE_SIZE] ={0,};
lipoyang 1:3f38c4bad274 154 static uint16_t l_buff[AVE_SIZE] ={0,};
lipoyang 1:3f38c4bad274 155 static uint32_t r_acc = 0;
lipoyang 1:3f38c4bad274 156 static uint32_t l_acc = 0;
lipoyang 1:3f38c4bad274 157
lipoyang 1:3f38c4bad274 158 uint16_t r = sensor_r;
lipoyang 1:3f38c4bad274 159 r_acc -= (uint32_t)r_buff[cnt];
lipoyang 1:3f38c4bad274 160 r_acc += (uint32_t)r;
lipoyang 1:3f38c4bad274 161 r_buff[cnt] = r;
lipoyang 1:3f38c4bad274 162
lipoyang 1:3f38c4bad274 163 uint16_t l = sensor_l;
lipoyang 1:3f38c4bad274 164 l_acc -= (uint32_t)l_buff[cnt];
lipoyang 1:3f38c4bad274 165 l_acc += (uint32_t)l;
lipoyang 1:3f38c4bad274 166 l_buff[cnt] = l;
lipoyang 1:3f38c4bad274 167
lipoyang 1:3f38c4bad274 168 cnt++;
lipoyang 1:3f38c4bad274 169 if(cnt >= AVE_SIZE){
lipoyang 1:3f38c4bad274 170 cnt = 0;
lipoyang 1:3f38c4bad274 171 uint16_t r_ave = (uint16_t)(r_acc / AVE_SIZE);
lipoyang 1:3f38c4bad274 172 uint16_t l_ave = (uint16_t)(l_acc / AVE_SIZE);
lipoyang 1:3f38c4bad274 173 // Stop! Obstacle Ahead
lipoyang 1:3f38c4bad274 174 if( (r_ave >= DISTANCE_STOP) && (l_ave >= DISTANCE_STOP) )
lipoyang 0:c5082e68ff72 175 {
lipoyang 1:3f38c4bad274 176 motor(0);
lipoyang 1:3f38c4bad274 177 StopFlag = true;
lipoyang 1:3f38c4bad274 178 }else{
lipoyang 1:3f38c4bad274 179 StopFlag = false;
lipoyang 0:c5082e68ff72 180 }
lipoyang 0:c5082e68ff72 181 }
lipoyang 0:c5082e68ff72 182 }
lipoyang 0:c5082e68ff72 183
lipoyang 0:c5082e68ff72 184 int main(void)
lipoyang 0:c5082e68ff72 185 {
lipoyang 0:c5082e68ff72 186 // initialize servo & motor
lipoyang 0:c5082e68ff72 187 servo_pwm.period_ms(20);
lipoyang 2:9189fa810ef8 188 servo(0);
lipoyang 0:c5082e68ff72 189 motor(0);
lipoyang 0:c5082e68ff72 190
lipoyang 0:c5082e68ff72 191 Ticker ticker;
lipoyang 1:3f38c4bad274 192 ticker.attach_us(TimerHandler, 20000); // 20msec interval
lipoyang 0:c5082e68ff72 193
lipoyang 0:c5082e68ff72 194 // initialize BLE
lipoyang 0:c5082e68ff72 195 ble.init();
lipoyang 0:c5082e68ff72 196 ble.onDisconnection(disconnectionCallback);
lipoyang 0:c5082e68ff72 197 ble.onDataWritten(WrittenHandler);
lipoyang 0:c5082e68ff72 198
lipoyang 0:c5082e68ff72 199 //pc.baud(9600);
lipoyang 1:3f38c4bad274 200 //pc.printf("BLE initialized\r\n");
lipoyang 0:c5082e68ff72 201
lipoyang 0:c5082e68ff72 202 // setup advertising
lipoyang 0:c5082e68ff72 203 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
lipoyang 0:c5082e68ff72 204 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
lipoyang 0:c5082e68ff72 205 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
lipoyang 0:c5082e68ff72 206 (const uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME) - 1);
lipoyang 1:3f38c4bad274 207 // ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
lipoyang 0:c5082e68ff72 208 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
lipoyang 0:c5082e68ff72 209 (const uint8_t *)UUID_BLUEPROPO, sizeof(UUID_BLUEPROPO));
lipoyang 0:c5082e68ff72 210 ble.setAdvertisingInterval(160); // 100ms; in multiples of 0.625ms.
lipoyang 0:c5082e68ff72 211 ble.addService(serviceBluePropo);
lipoyang 0:c5082e68ff72 212 ble.startAdvertising();
lipoyang 0:c5082e68ff72 213 //pc.printf("Advertising Start \r\n");
lipoyang 0:c5082e68ff72 214
lipoyang 0:c5082e68ff72 215 // main loop (wait for BLE event)
lipoyang 0:c5082e68ff72 216 while(true)
lipoyang 0:c5082e68ff72 217 {
lipoyang 0:c5082e68ff72 218 ble.waitForEvent();
lipoyang 0:c5082e68ff72 219 }
lipoyang 0:c5082e68ff72 220 }