robo 8080 / Mbed 2 deprecated BLE_RCBController_Motor

RCBControllerでモータを制御します。うおーるぼっとも動かせました。

Dependencies:   BLE_API_Native_IRC TB6612FNG2 mbed

Fork of BLE_RCBController by Junichi Katsu

  • 古いBLEライブラリを使っているのでプラットフォームは”Nordic nRF51822”を選択してください。
  • ライブラリ類はUpdateしないでください。コンパイルエラーになります。

うまく接続できない時は、iPhone/iPadのBluetoothをOFF－＞ONしてキャッシュをクリアしてみてください。

RCBControllerでうおーるぼっとを操縦する例 /media/uploads/robo8080/img_1671.jpg

RCBControllerでの操縦は次の4種類あります。 それぞれうおーるぼっとの動きが異なりますので試してみてください。

  • 左十字ボタン
  • 左のみアナログ
  • 右のみアナログ
  • 両方アナログ

うおーるぼっと(LPC1768のソケット)とHRM1017の接続はこれです。

LPC1768 ー HRM1017

p11 ーーー P0_0

p12 ーーー P0_1

p13 ーーー P0_28

p14 ーーー P0_29

p21 ーーー P0_30

p22 ーーー P0_25

GND ーーー GND

HRM1017の電源はうおーるぼっとのUSBコネクタからとります。 /media/uploads/robo8080/img_1674.jpg

main.cpp@1:4f33a5a25063, 2014-09-13 (annotated)

Committer:
robo8080
Date:
Sat Sep 13 06:34:26 2014 +0000
Revision:
1:4f33a5a25063
Parent:
0:8c643bfe55b7 
test1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jksoft 0:8c643bfe55b7 1 #include "mbed.h"
jksoft 0:8c643bfe55b7 2 #include "nRF51822n.h"
jksoft 0:8c643bfe55b7 3 #include "RCBController.h"
robo8080 1:4f33a5a25063 4 #include "TB6612.h"
jksoft 0:8c643bfe55b7 5
jksoft 0:8c643bfe55b7 6 #define DBG 0
jksoft 0:8c643bfe55b7 7
jksoft 0:8c643bfe55b7 8 nRF51822n nrf;
jksoft 0:8c643bfe55b7 9 Serial pc(USBTX, USBRX);
jksoft 0:8c643bfe55b7 10 /* LEDs for indication: */
jksoft 0:8c643bfe55b7 11 DigitalOut ConnectStateLed(LED1);
jksoft 0:8c643bfe55b7 12 PwmOut ControllerStateLed(LED2);
jksoft 0:8c643bfe55b7 13
robo8080 1:4f33a5a25063 14 TB6612 left(P0_30,P0_1,P0_0);
robo8080 1:4f33a5a25063 15 TB6612 right(P0_25,P0_29,P0_28);
robo8080 1:4f33a5a25063 16
robo8080 1:4f33a5a25063 17 #define PI 3.141592
robo8080 1:4f33a5a25063 18 #define neutralAngle 25
robo8080 1:4f33a5a25063 19 #define neutralRange 5
robo8080 1:4f33a5a25063 20 #define angleRange 30.0
robo8080 1:4f33a5a25063 21 #define handlingIntensity 0.7
robo8080 1:4f33a5a25063 22 float AccSin2Deg(uint8_t acc) // acc : -90 ... 90
robo8080 1:4f33a5a25063 23 {
robo8080 1:4f33a5a25063 24 return (float)asin((float)(acc-128.0f)/127.0f)*180.0f/PI;
robo8080 1:4f33a5a25063 25 }
robo8080 1:4f33a5a25063 26
robo8080 1:4f33a5a25063 27 float Acc2Speed(uint8_t acc) // acc : 0 ... 255
robo8080 1:4f33a5a25063 28 {
robo8080 1:4f33a5a25063 29 int deg = neutralAngle+(int)AccSin2Deg(acc);
robo8080 1:4f33a5a25063 30 if(deg>-neutralRange && deg<neutralRange)deg=0;
robo8080 1:4f33a5a25063 31 float speed=100*(float)deg/angleRange;
robo8080 1:4f33a5a25063 32 if(speed>100){
robo8080 1:4f33a5a25063 33 speed=100;
robo8080 1:4f33a5a25063 34 } else if(speed<-100){
robo8080 1:4f33a5a25063 35 speed=-100;
robo8080 1:4f33a5a25063 36 }
robo8080 1:4f33a5a25063 37 return speed;
robo8080 1:4f33a5a25063 38 }
robo8080 1:4f33a5a25063 39
robo8080 1:4f33a5a25063 40 void RCBCon(uint8_t *buffer, uint16_t buffer_size)
robo8080 1:4f33a5a25063 41 {
robo8080 1:4f33a5a25063 42 uint16_t game_pad;
robo8080 1:4f33a5a25063 43 game_pad = (buffer[0] << 8) | buffer[1];
robo8080 1:4f33a5a25063 44 // pc.printf("game_pad : %04X\n",game_pad);
robo8080 1:4f33a5a25063 45 float AccX = (100.0f/90.0f)*AccSin2Deg(buffer[6]);//((float)buffer[6] / 255.0)*200.0 - 100.0;
robo8080 1:4f33a5a25063 46 // float AccY = AccSin2Deg(buffer[7]);//((float)buffer[7] / 255.0)*200.0 - 100.0;
robo8080 1:4f33a5a25063 47 float AccY = Acc2Speed(buffer[7]);
robo8080 1:4f33a5a25063 48 // float AccZ = (100.0f/90.0f)*AccSin2Deg(buffer[8]);((float)buffer[8] / 255.0)*200.0 - 100.0;
robo8080 1:4f33a5a25063 49 // pc.printf("acc X : %f Y : %f Z : %f\n",acc[0],acc[1],acc[2]);
robo8080 1:4f33a5a25063 50 float LeftStickX = ((float)buffer[2] / 255.0)*200.0 - 100.0;
robo8080 1:4f33a5a25063 51 float RightStickX = ((float)buffer[4] / 255.0)*200.0 - 100.0;
robo8080 1:4f33a5a25063 52 float LeftStickY = ((float)buffer[3] / 255.0)*200.0 - 100.0;
robo8080 1:4f33a5a25063 53 float RightStickY = ((float)buffer[5] / 255.0)*200.0 - 100.0;
robo8080 1:4f33a5a25063 54 uint8_t status = buffer[9];
robo8080 1:4f33a5a25063 55
robo8080 1:4f33a5a25063 56 if((status & 0x60) == 0x20) { // Accelerometer ON
robo8080 1:4f33a5a25063 57 float leftData;
robo8080 1:4f33a5a25063 58 float rightData;
robo8080 1:4f33a5a25063 59 float xHandling=(float)AccX*handlingIntensity;
robo8080 1:4f33a5a25063 60 if(game_pad != 0x0020) { // A button
robo8080 1:4f33a5a25063 61 right = 0;
robo8080 1:4f33a5a25063 62 left = 0;
robo8080 1:4f33a5a25063 63 } else {
robo8080 1:4f33a5a25063 64 leftData = AccY;
robo8080 1:4f33a5a25063 65 rightData = AccY;
robo8080 1:4f33a5a25063 66 if(AccY==0) {
robo8080 1:4f33a5a25063 67 leftData= AccX;
robo8080 1:4f33a5a25063 68 rightData= -AccX;
robo8080 1:4f33a5a25063 69 } else if(AccY>0) {
robo8080 1:4f33a5a25063 70 if(AccX>0) {
robo8080 1:4f33a5a25063 71 rightData=AccY-(int)xHandling; //r-x
robo8080 1:4f33a5a25063 72 } else {
robo8080 1:4f33a5a25063 73 leftData=AccY+(int)xHandling; //l-x
robo8080 1:4f33a5a25063 74 }
robo8080 1:4f33a5a25063 75 } else {
robo8080 1:4f33a5a25063 76 if(AccX>0) {
robo8080 1:4f33a5a25063 77 leftData=AccY-(int)xHandling; //l-x
robo8080 1:4f33a5a25063 78 } else {
robo8080 1:4f33a5a25063 79 rightData=AccY+(int)xHandling; //r-x
robo8080 1:4f33a5a25063 80 }
robo8080 1:4f33a5a25063 81 }
robo8080 1:4f33a5a25063 82 left = (int)leftData;
robo8080 1:4f33a5a25063 83 right = (int)rightData;
robo8080 1:4f33a5a25063 84 }
robo8080 1:4f33a5a25063 85 } else if((status & 0x10)&&(status & 0x08)) { // L : Analog R : Analog
robo8080 1:4f33a5a25063 86 left = (int)LeftStickY;
robo8080 1:4f33a5a25063 87 right = (int)RightStickY;
robo8080 1:4f33a5a25063 88 } else if(status & 0x10) { // L : Analog
robo8080 1:4f33a5a25063 89 if((LeftStickX < 15) && (LeftStickX > -15)) {
robo8080 1:4f33a5a25063 90 right = LeftStickY;
robo8080 1:4f33a5a25063 91 left = LeftStickY;
robo8080 1:4f33a5a25063 92 }else if((LeftStickY < 15) && (LeftStickY > -15)) {
robo8080 1:4f33a5a25063 93 right = -LeftStickX;
robo8080 1:4f33a5a25063 94 left = LeftStickX;
robo8080 1:4f33a5a25063 95 } else {
robo8080 1:4f33a5a25063 96 right = 0;
robo8080 1:4f33a5a25063 97 left = 0;
robo8080 1:4f33a5a25063 98 }
robo8080 1:4f33a5a25063 99 } else if(status & 0x08) { // R : Analog
robo8080 1:4f33a5a25063 100 float leftData;
robo8080 1:4f33a5a25063 101 float rightData;
robo8080 1:4f33a5a25063 102 if(RightStickY < 0) {
robo8080 1:4f33a5a25063 103 RightStickX *= -1.0f;
robo8080 1:4f33a5a25063 104 }
robo8080 1:4f33a5a25063 105 if(RightStickX >= 0) {
robo8080 1:4f33a5a25063 106 leftData = RightStickY + RightStickX;
robo8080 1:4f33a5a25063 107 rightData = RightStickY;
robo8080 1:4f33a5a25063 108 } else {
robo8080 1:4f33a5a25063 109 leftData = RightStickY;
robo8080 1:4f33a5a25063 110 rightData = RightStickY - RightStickX;
robo8080 1:4f33a5a25063 111 }
robo8080 1:4f33a5a25063 112 if(leftData > 100) {
robo8080 1:4f33a5a25063 113 leftData = 100;
robo8080 1:4f33a5a25063 114 } else if(leftData < -100) {
robo8080 1:4f33a5a25063 115 leftData = -100;
robo8080 1:4f33a5a25063 116 }
robo8080 1:4f33a5a25063 117 if(rightData > 100) {
robo8080 1:4f33a5a25063 118 rightData = 100;
robo8080 1:4f33a5a25063 119 } else if(rightData < -100) {
robo8080 1:4f33a5a25063 120 rightData = -100;
robo8080 1:4f33a5a25063 121 }
robo8080 1:4f33a5a25063 122 left = (int)leftData;
robo8080 1:4f33a5a25063 123 right = (int)rightData;
robo8080 1:4f33a5a25063 124 } else if((status & 0x60) == 0x40) { // L : Accelerometer
robo8080 1:4f33a5a25063 125 if(game_pad != 0x0020) { // A button
robo8080 1:4f33a5a25063 126 right = 0;
robo8080 1:4f33a5a25063 127 left = 0;
robo8080 1:4f33a5a25063 128 } else {
robo8080 1:4f33a5a25063 129 if((LeftStickX < 15) && (LeftStickX > -15)) {
robo8080 1:4f33a5a25063 130 right = LeftStickY;
robo8080 1:4f33a5a25063 131 left = LeftStickY;
robo8080 1:4f33a5a25063 132 } else if((LeftStickY < 15) && (LeftStickY > -15)) {
robo8080 1:4f33a5a25063 133 right = -LeftStickX;
robo8080 1:4f33a5a25063 134 left = LeftStickX;
robo8080 1:4f33a5a25063 135 } else {
robo8080 1:4f33a5a25063 136 right = 0;
robo8080 1:4f33a5a25063 137 left = 0;
robo8080 1:4f33a5a25063 138 }
robo8080 1:4f33a5a25063 139 }
robo8080 1:4f33a5a25063 140 } else { // Digital button
robo8080 1:4f33a5a25063 141 switch(game_pad)
robo8080 1:4f33a5a25063 142 {
robo8080 1:4f33a5a25063 143 case 0x0001:
robo8080 1:4f33a5a25063 144 left = 50;
robo8080 1:4f33a5a25063 145 right = 50;
robo8080 1:4f33a5a25063 146 break;
robo8080 1:4f33a5a25063 147 case 0x0002:
robo8080 1:4f33a5a25063 148 left = -50;
robo8080 1:4f33a5a25063 149 right = -50;
robo8080 1:4f33a5a25063 150 break;
robo8080 1:4f33a5a25063 151 case 0x0004:
robo8080 1:4f33a5a25063 152 left = 50;
robo8080 1:4f33a5a25063 153 right = -50;
robo8080 1:4f33a5a25063 154 break;
robo8080 1:4f33a5a25063 155 case 0x0008:
robo8080 1:4f33a5a25063 156 left = -50;
robo8080 1:4f33a5a25063 157 right = 50;
robo8080 1:4f33a5a25063 158 break;
robo8080 1:4f33a5a25063 159 default:
robo8080 1:4f33a5a25063 160 left = 0;
robo8080 1:4f33a5a25063 161 right = 0;
robo8080 1:4f33a5a25063 162 }
robo8080 1:4f33a5a25063 163 }
robo8080 1:4f33a5a25063 164 }
jksoft 0:8c643bfe55b7 165
jksoft 0:8c643bfe55b7 166 /* RCBController Service */
jksoft 0:8c643bfe55b7 167 static const uint16_t RCBController_service_uuid = 0xFFF0;
jksoft 0:8c643bfe55b7 168 static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
jksoft 0:8c643bfe55b7 169 GattService RCBControllerService (RCBController_service_uuid);
jksoft 0:8c643bfe55b7 170 GattCharacteristic Controller (RCBController_Characteristic_uuid,10, 10,
jksoft 0:8c643bfe55b7 171 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
jksoft 0:8c643bfe55b7 172 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
jksoft 0:8c643bfe55b7 173
jksoft 0:8c643bfe55b7 174 /* Advertising data and parameters */
jksoft 0:8c643bfe55b7 175 GapAdvertisingData advData;
jksoft 0:8c643bfe55b7 176 GapAdvertisingData scanResponse;
jksoft 0:8c643bfe55b7 177 GapAdvertisingParams advParams ( GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED );
jksoft 0:8c643bfe55b7 178
jksoft 0:8c643bfe55b7 179 uint16_t uuid16_list[] = { RCBController_service_uuid };
jksoft 0:8c643bfe55b7 180
jksoft 0:8c643bfe55b7 181 RCBController controller;
jksoft 0:8c643bfe55b7 182
jksoft 0:8c643bfe55b7 183 // GapEvent
jksoft 0:8c643bfe55b7 184 class GapEventHandler : public GapEvents
jksoft 0:8c643bfe55b7 185 {
jksoft 0:8c643bfe55b7 186
jksoft 0:8c643bfe55b7 187 virtual void onConnected(void)
jksoft 0:8c643bfe55b7 188 {
jksoft 0:8c643bfe55b7 189 ConnectStateLed = 0;
robo8080 1:4f33a5a25063 190 left = 0;
robo8080 1:4f33a5a25063 191 right = 0;
jksoft 0:8c643bfe55b7 192 #if DBG
jksoft 0:8c643bfe55b7 193 pc.printf("Connected\n\r");
jksoft 0:8c643bfe55b7 194 #endif
jksoft 0:8c643bfe55b7 195 }
jksoft 0:8c643bfe55b7 196
jksoft 0:8c643bfe55b7 197 virtual void onDisconnected(void)
jksoft 0:8c643bfe55b7 198 {
jksoft 0:8c643bfe55b7 199 nrf.getGap().startAdvertising(advParams);
jksoft 0:8c643bfe55b7 200 ConnectStateLed = 1;
robo8080 1:4f33a5a25063 201 left = 0;
robo8080 1:4f33a5a25063 202 right = 0;
jksoft 0:8c643bfe55b7 203 #if DBG
jksoft 0:8c643bfe55b7 204 pc.printf("Disconnected\n\r");
jksoft 0:8c643bfe55b7 205 #endif
jksoft 0:8c643bfe55b7 206 }
jksoft 0:8c643bfe55b7 207 };
jksoft 0:8c643bfe55b7 208
jksoft 0:8c643bfe55b7 209 // GattEvent
jksoft 0:8c643bfe55b7 210 class GattServerEventHandler : public GattServerEvents
jksoft 0:8c643bfe55b7 211 {
jksoft 0:8c643bfe55b7 212 virtual void onDataWritten(uint16_t charHandle)
jksoft 0:8c643bfe55b7 213 {
jksoft 0:8c643bfe55b7 214 if (charHandle == Controller.handle) {
jksoft 0:8c643bfe55b7 215 nrf.getGattServer().readValue(Controller.handle, &controller.data[0], sizeof(controller));
jksoft 0:8c643bfe55b7 216 #if DBG
jksoft 0:8c643bfe55b7 217 pc.printf("DATA:%d %d %d %d %d %d %d %d %d %d\n\r",controller.data[0],controller.data[1],controller.data[2],controller.data[3],controller.data[4],
jksoft 0:8c643bfe55b7 218 controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
jksoft 0:8c643bfe55b7 219 #endif
jksoft 0:8c643bfe55b7 220 ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;;
robo8080 1:4f33a5a25063 221 RCBCon(&controller.data[0], sizeof(controller));
jksoft 0:8c643bfe55b7 222 }
jksoft 0:8c643bfe55b7 223
jksoft 0:8c643bfe55b7 224 }
jksoft 0:8c643bfe55b7 225 };
jksoft 0:8c643bfe55b7 226
jksoft 0:8c643bfe55b7 227 /**************************************************************************/
jksoft 0:8c643bfe55b7 228 /*!
jksoft 0:8c643bfe55b7 229 @brief Program entry point
jksoft 0:8c643bfe55b7 230 */
jksoft 0:8c643bfe55b7 231 /**************************************************************************/
jksoft 0:8c643bfe55b7 232 int main(void)
jksoft 0:8c643bfe55b7 233 {
jksoft 0:8c643bfe55b7 234 #if DBG
jksoft 0:8c643bfe55b7 235 pc.printf("Start\n\r");
jksoft 0:8c643bfe55b7 236 #endif
robo8080 1:4f33a5a25063 237 left = 0;
robo8080 1:4f33a5a25063 238 right = 0;
jksoft 0:8c643bfe55b7 239 /* Setup an event handler for GAP events i.e. Client/Server connection events. */
jksoft 0:8c643bfe55b7 240 nrf.getGap().setEventHandler(new GapEventHandler());
jksoft 0:8c643bfe55b7 241
jksoft 0:8c643bfe55b7 242 /* Initialise the nRF51822 */
jksoft 0:8c643bfe55b7 243 nrf.init();
jksoft 0:8c643bfe55b7 244
jksoft 0:8c643bfe55b7 245 nrf.getGattServer().setEventHandler(new GattServerEventHandler());
jksoft 0:8c643bfe55b7 246
jksoft 0:8c643bfe55b7 247 /* Make sure we get a clean start */
jksoft 0:8c643bfe55b7 248 nrf.reset();
jksoft 0:8c643bfe55b7 249
jksoft 0:8c643bfe55b7 250 /* Add BLE-Only flag and complete service list to the advertising data */
robo8080 1:4f33a5a25063 251 // advData.addFlags(GapAdvertisingData::BREDR_NOT_SUPPORTED);
robo8080 1:4f33a5a25063 252 advData.addFlags((GapAdvertisingData::Flags)(GapAdvertisingData::LE_GENERAL_DISCOVERABLE |
robo8080 1:4f33a5a25063 253 GapAdvertisingData::BREDR_NOT_SUPPORTED));
jksoft 0:8c643bfe55b7 254 advData.addData(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
jksoft 0:8c643bfe55b7 255 (uint8_t*)uuid16_list, sizeof(uuid16_list));
jksoft 0:8c643bfe55b7 256 nrf.getGap().setAdvertisingData(advData, scanResponse);
jksoft 0:8c643bfe55b7 257
jksoft 0:8c643bfe55b7 258 /* RCBController Service */
jksoft 0:8c643bfe55b7 259 RCBControllerService.addCharacteristic(Controller);
jksoft 0:8c643bfe55b7 260 nrf.getGattServer().addService(RCBControllerService);
jksoft 0:8c643bfe55b7 261
jksoft 0:8c643bfe55b7 262 /* Start advertising (make sure you've added all your data first) */
jksoft 0:8c643bfe55b7 263 nrf.getGap().startAdvertising(advParams);
jksoft 0:8c643bfe55b7 264 ConnectStateLed = 1;
jksoft 0:8c643bfe55b7 265 ControllerStateLed = 1;
jksoft 0:8c643bfe55b7 266
jksoft 0:8c643bfe55b7 267 for (;;)
jksoft 0:8c643bfe55b7 268 {
jksoft 0:8c643bfe55b7 269 wait(1);
jksoft 0:8c643bfe55b7 270 }
jksoft 0:8c643bfe55b7 271 }
jksoft 0:8c643bfe55b7 272