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

Files at this revision

API Documentation at this revision

Comitter:
robo8080
Date:
Sat Sep 13 06:34:26 2014 +0000
Parent:
0:8c643bfe55b7
Commit message:
test1

Changed in this revision

BLE_API_Native_IRC/hw/nRF51822n/projectconfig.h Show annotated file Show diff for this revision Revisions of this file
TB6612FNG2.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 8c643bfe55b7 -r 4f33a5a25063 BLE_API_Native_IRC/hw/nRF51822n/projectconfig.h
--- a/BLE_API_Native_IRC/hw/nRF51822n/projectconfig.h	Thu Jul 10 14:21:52 2014 +0000
+++ b/BLE_API_Native_IRC/hw/nRF51822n/projectconfig.h	Sat Sep 13 06:34:26 2014 +0000
@@ -113,8 +113,10 @@
     #define CFG_GAP_APPEARANCE                         BLE_APPEARANCE_GENERIC_TAG
     #define CFG_GAP_LOCAL_NAME                         "mbed HRM1017"
 
-    #define CFG_GAP_CONNECTION_MIN_INTERVAL_MS         50/*500*/                      /**< Minimum acceptable connection interval */
-    #define CFG_GAP_CONNECTION_MAX_INTERVAL_MS         100/*1000*/                     /**< Maximum acceptable connection interval */
+//    #define CFG_GAP_CONNECTION_MIN_INTERVAL_MS         50/*500*/                      /**< Minimum acceptable connection interval */
+//    #define CFG_GAP_CONNECTION_MAX_INTERVAL_MS         100/*1000*/                     /**< Maximum acceptable connection interval */
+    #define CFG_GAP_CONNECTION_MIN_INTERVAL_MS         5/*500*/                      /**< Minimum acceptable connection interval */
+    #define CFG_GAP_CONNECTION_MAX_INTERVAL_MS         10/*1000*/                     /**< Maximum acceptable connection interval */
     #define CFG_GAP_CONNECTION_SUPERVISION_TIMEOUT_MS  4000                     /**< Connection supervisory timeout */
     #define CFG_GAP_CONNECTION_SLAVE_LATENCY           0                        /**< Slave Latency in number of connection events. */
 
diff -r 8c643bfe55b7 -r 4f33a5a25063 TB6612FNG2.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TB6612FNG2.lib	Sat Sep 13 06:34:26 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/jksoft/code/TB6612FNG2/#051a7ecff13e
diff -r 8c643bfe55b7 -r 4f33a5a25063 main.cpp
--- a/main.cpp	Thu Jul 10 14:21:52 2014 +0000
+++ b/main.cpp	Sat Sep 13 06:34:26 2014 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "nRF51822n.h"
 #include "RCBController.h"
+#include "TB6612.h"
 
 #define DBG 0
 
@@ -10,6 +11,157 @@
 DigitalOut  ConnectStateLed(LED1);
 PwmOut  ControllerStateLed(LED2);
 
+TB6612 left(P0_30,P0_1,P0_0);
+TB6612 right(P0_25,P0_29,P0_28);
+
+#define PI 3.141592
+#define neutralAngle 25
+#define neutralRange 5
+#define angleRange 30.0
+#define handlingIntensity 0.7
+float AccSin2Deg(uint8_t acc)    // acc : -90 ... 90
+{
+    return (float)asin((float)(acc-128.0f)/127.0f)*180.0f/PI;
+}
+
+float Acc2Speed(uint8_t acc)    // acc : 0 ... 255
+{
+    int deg = neutralAngle+(int)AccSin2Deg(acc);
+    if(deg>-neutralRange && deg<neutralRange)deg=0;
+    float speed=100*(float)deg/angleRange;
+    if(speed>100){
+        speed=100;
+    } else if(speed<-100){
+        speed=-100;
+    }
+    return speed;
+}
+
+void RCBCon(uint8_t *buffer, uint16_t buffer_size)
+{
+    uint16_t game_pad;
+    game_pad = (buffer[0] << 8) | buffer[1];
+//  pc.printf("game_pad : %04X\n",game_pad);
+    float AccX = (100.0f/90.0f)*AccSin2Deg(buffer[6]);//((float)buffer[6] / 255.0)*200.0 - 100.0;
+//  float AccY = AccSin2Deg(buffer[7]);//((float)buffer[7] / 255.0)*200.0 - 100.0;
+    float AccY = Acc2Speed(buffer[7]);
+//  float AccZ = (100.0f/90.0f)*AccSin2Deg(buffer[8]);((float)buffer[8] / 255.0)*200.0 - 100.0;
+//  pc.printf("acc X : %f Y : %f Z : %f\n",acc[0],acc[1],acc[2]);
+    float LeftStickX  = ((float)buffer[2] / 255.0)*200.0 - 100.0;
+    float RightStickX = ((float)buffer[4] / 255.0)*200.0 - 100.0;
+    float LeftStickY  = ((float)buffer[3] / 255.0)*200.0 - 100.0;
+    float RightStickY = ((float)buffer[5] / 255.0)*200.0 - 100.0;
+    uint8_t status = buffer[9];
+
+    if((status & 0x60) == 0x20) {    // Accelerometer ON
+        float leftData;
+        float rightData;
+        float xHandling=(float)AccX*handlingIntensity;
+        if(game_pad != 0x0020) {      // A button 
+            right = 0;
+            left  = 0;            
+        } else {
+            leftData  = AccY;
+            rightData = AccY;
+            if(AccY==0) {
+                leftData= AccX;
+                rightData= -AccX;
+            } else if(AccY>0) {
+                if(AccX>0) {
+                    rightData=AccY-(int)xHandling; //r-x
+                } else {
+                    leftData=AccY+(int)xHandling; //l-x
+                }
+            } else {
+                if(AccX>0) {
+                    leftData=AccY-(int)xHandling; //l-x
+                } else {
+                    rightData=AccY+(int)xHandling; //r-x
+                }
+            }
+            left  = (int)leftData;
+            right = (int)rightData;
+        }
+    } else if((status & 0x10)&&(status & 0x08)) {  // L : Analog R : Analog
+        left  = (int)LeftStickY;
+        right = (int)RightStickY;
+    } else if(status & 0x10) {              // L : Analog
+        if((LeftStickX < 15) && (LeftStickX > -15)) {
+            right = LeftStickY;
+            left  = LeftStickY;            
+        }else if((LeftStickY < 15) && (LeftStickY > -15)) {
+            right = -LeftStickX;
+            left  = LeftStickX;            
+        } else {
+            right = 0;
+            left  = 0;            
+        }
+    } else if(status & 0x08) {              // R : Analog
+        float leftData;
+        float rightData;
+        if(RightStickY < 0) {
+            RightStickX *= -1.0f;
+        }
+        if(RightStickX >= 0) {
+            leftData  = RightStickY + RightStickX;
+            rightData = RightStickY;
+        } else {
+            leftData  = RightStickY;
+            rightData = RightStickY - RightStickX;
+        }
+        if(leftData > 100) {
+            leftData = 100;
+        } else if(leftData < -100) {
+            leftData = -100;
+        }
+        if(rightData > 100) {
+            rightData = 100;
+        } else if(rightData < -100) {
+            rightData = -100;
+        }
+        left  = (int)leftData;
+        right = (int)rightData;
+    } else if((status & 0x60) == 0x40) {    // L : Accelerometer
+        if(game_pad != 0x0020) {            // A button 
+            right = 0;
+            left  = 0;            
+        } else {
+            if((LeftStickX < 15) && (LeftStickX > -15)) {
+                right = LeftStickY;
+                left  = LeftStickY;            
+            } else if((LeftStickY < 15) && (LeftStickY > -15)) {
+                right = -LeftStickX;
+                left  = LeftStickX;            
+            } else {
+                right = 0;
+                left  = 0;            
+            }
+        }
+    } else {                        // Digital button
+        switch(game_pad)
+        {
+            case 0x0001:
+                left = 50;
+                right = 50;
+                break;
+            case 0x0002:
+                left = -50;
+                right = -50;
+                break;
+            case 0x0004:
+                left = 50;
+                right = -50;
+                break;
+            case 0x0008:
+                left = -50;
+                right = 50;
+                break;
+            default:
+                left = 0;
+                right = 0;
+        }
+    }
+}
 
 /* RCBController Service */
 static const uint16_t RCBController_service_uuid = 0xFFF0;
@@ -35,6 +187,8 @@
     virtual void onConnected(void)
     {
         ConnectStateLed = 0;
+        left = 0;
+        right = 0;
 #if DBG
 		pc.printf("Connected\n\r");
 #endif
@@ -44,6 +198,8 @@
     {
         nrf.getGap().startAdvertising(advParams);
 		ConnectStateLed = 1;
+        left = 0;
+        right = 0;
 #if DBG
 		pc.printf("Disconnected\n\r");
 #endif
@@ -62,7 +218,7 @@
 															   controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
 #endif
 			ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;;
-			
+			RCBCon(&controller.data[0], sizeof(controller));
 		}
 		 
 	}
@@ -78,6 +234,8 @@
 #if DBG
 		pc.printf("Start\n\r");
 #endif
+    left = 0;
+    right = 0;
     /* Setup an event handler for GAP events i.e. Client/Server connection events. */
     nrf.getGap().setEventHandler(new GapEventHandler());
     
@@ -90,7 +248,9 @@
     nrf.reset();    
 
     /* Add BLE-Only flag and complete service list to the advertising data */
-    advData.addFlags(GapAdvertisingData::BREDR_NOT_SUPPORTED);
+//    advData.addFlags(GapAdvertisingData::BREDR_NOT_SUPPORTED);
+	advData.addFlags((GapAdvertisingData::Flags)(GapAdvertisingData::LE_GENERAL_DISCOVERABLE |
+												 GapAdvertisingData::BREDR_NOT_SUPPORTED));
     advData.addData(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, 
                    (uint8_t*)uuid16_list, sizeof(uuid16_list));
     nrf.getGap().setAdvertisingData(advData, scanResponse);