robo 8080
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でうおーるぼっとを操縦する例
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コネクタからとります。
Diff: main.cpp
- Revision:
- 1:4f33a5a25063
- Parent:
- 0:8c643bfe55b7
--- 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);