RCBControllerでモータを制御します。うおーるぼっとも動かせました。
Dependencies: BLE_API_Native_IRC TB6612FNG2 mbed
Fork of BLE_RCBController by
- 古いBLEライブラリを使っているのでプラットフォームは”Nordic nRF51822”を選択してください。
- ライブラリ類はUpdateしないでください。コンパイルエラーになります。
うまく接続できない時は、iPhone/iPadのBluetoothをOFF->ONしてキャッシュをクリアしてみてください。
RCBControllerでうおーるぼっとを操縦する例
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コネクタからとります。
main.cpp
- Committer:
- robo8080
- Date:
- 2014-09-13
- Revision:
- 1:4f33a5a25063
- Parent:
- 0:8c643bfe55b7
File content as of revision 1:4f33a5a25063:
#include "mbed.h" #include "nRF51822n.h" #include "RCBController.h" #include "TB6612.h" #define DBG 0 nRF51822n nrf; Serial pc(USBTX, USBRX); /* LEDs for indication: */ 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; static const uint16_t RCBController_Characteristic_uuid = 0xFFF1; GattService RCBControllerService (RCBController_service_uuid); GattCharacteristic Controller (RCBController_Characteristic_uuid,10, 10, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE); /* Advertising data and parameters */ GapAdvertisingData advData; GapAdvertisingData scanResponse; GapAdvertisingParams advParams ( GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED ); uint16_t uuid16_list[] = { RCBController_service_uuid }; RCBController controller; // GapEvent class GapEventHandler : public GapEvents { virtual void onConnected(void) { ConnectStateLed = 0; left = 0; right = 0; #if DBG pc.printf("Connected\n\r"); #endif } virtual void onDisconnected(void) { nrf.getGap().startAdvertising(advParams); ConnectStateLed = 1; left = 0; right = 0; #if DBG pc.printf("Disconnected\n\r"); #endif } }; // GattEvent class GattServerEventHandler : public GattServerEvents { virtual void onDataWritten(uint16_t charHandle) { if (charHandle == Controller.handle) { nrf.getGattServer().readValue(Controller.handle, &controller.data[0], sizeof(controller)); #if DBG 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], 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)); } } }; /**************************************************************************/ /*! @brief Program entry point */ /**************************************************************************/ int main(void) { #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()); /* Initialise the nRF51822 */ nrf.init(); nrf.getGattServer().setEventHandler(new GattServerEventHandler()); /* Make sure we get a clean start */ nrf.reset(); /* Add BLE-Only flag and complete service list to the advertising data */ // 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); /* RCBController Service */ RCBControllerService.addCharacteristic(Controller); nrf.getGattServer().addService(RCBControllerService); /* Start advertising (make sure you've added all your data first) */ nrf.getGap().startAdvertising(advParams); ConnectStateLed = 1; ControllerStateLed = 1; for (;;) { wait(1); } }