robo 8080
RCBControllerでモータを制御します。うおーるぼっとも動かせました。
Dependencies: BLE_API TB6612FNG2 mbed nRF51822
Fork of
BLE_RCBController2
by 
Junichi Katsu
うまく接続できない時は、iPhone/iPadのBluetoothをOFF－＞ONしてキャッシュをクリアしてみてください。
ライブラリ類をUpdateするとコンパイル出来なくなります。インポートした物をそのまま使って下さい。
RCBControllerでうおーるぼっとを操縦する例
This robot has switch, line sensors and motors. It controls by mbed.
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:
- 5:1c04bd9f8457
- Parent:
- 2:dd85fdc18224
--- a/main.cpp Wed Aug 20 13:41:01 2014 +0000
+++ b/main.cpp Wed Sep 17 01:49:34 2014 +0000
@@ -1,8 +1,11 @@
#include "mbed.h"
#include "BLEDevice.h"
#include "RCBController.h"
+#include "TB6612.h"
-#define DBG 1
+#define DBG 0
+
+const static char DEVICE_NAME[] = "mbed HRM1017";
BLEDevice ble;
Serial pc(USBTX, USBRX);
@@ -10,6 +13,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;
@@ -24,7 +178,7 @@
RCBController controller;
-void onConnected(uint16_t h)
+void onConnected(Gap::Handle_t handle, const Gap::ConnectionParams_t *params)
{
ConnectStateLed = 0;
#if DBG
@@ -32,10 +186,12 @@
#endif
}
-void onDisconnected(uint16_t h)
+void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
{
ble.startAdvertising();
ConnectStateLed = 1;
+ left = 0;
+ right = 0;
#if DBG
pc.printf("Disconnected\n\r");
#endif
@@ -43,11 +199,11 @@
// GattEvent
-void onDataWritten(uint16_t charHandle)
+void onDataWritten(uint16_t charHandle, const GattCharacteristicWriteCBParams *params)
{
- if (charHandle == ControllerChar.getHandle()) {
+ if (charHandle == ControllerChar.getValueAttribute().getHandle()) {
uint16_t bytesRead;
- ble.readCharacteristicValue(ControllerChar.getHandle(),RCBControllerPayload, &bytesRead);
+ ble.readCharacteristicValue(ControllerChar.getValueAttribute().getHandle(),RCBControllerPayload, &bytesRead);
memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller));
#if DBG
@@ -55,6 +211,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));
}
}
@@ -69,8 +226,12 @@
#if DBG
pc.printf("Start\n\r");
#endif
+ ConnectStateLed = 1;
+ left = 0;
+ right = 0;
ble.init();
+ ble.setDeviceName((uint8_t *)DEVICE_NAME);
ble.onConnection(onConnected);
ble.onDisconnection(onDisconnected);
ble.onDataWritten(onDataWritten);