robo 8080 / Mbed 2 deprecated BLE_RCBController2_Motor

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

Dependencies:   BLE_API TB6612FNG2 mbed nRF51822

Fork of BLE_RCBController2 by Junichi Katsu

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

ライブラリ類をUpdateするとコンパイル出来なくなります。インポートした物をそのまま使って下さい。

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

Components / Wallbot
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

/media/uploads/robo8080/img_1711.jpg

/media/uploads/robo8080/img_1703.jpg

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

main.cpp@5:1c04bd9f8457, 2014-09-17 (annotated)

Committer:
robo8080
Date:
Wed Sep 17 01:49:34 2014 +0000
Revision:
5:1c04bd9f8457
Parent:
2:dd85fdc18224 
test1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jksoft 0:8c643bfe55b7 1 #include "mbed.h"
jksoft 1:48f6e08a3ac2 2 #include "BLEDevice.h"
jksoft 0:8c643bfe55b7 3 #include "RCBController.h"
robo8080 5:1c04bd9f8457 4 #include "TB6612.h"
jksoft 0:8c643bfe55b7 5
robo8080 5:1c04bd9f8457 6 #define DBG 0
robo8080 5:1c04bd9f8457 7
robo8080 5:1c04bd9f8457 8 const static char DEVICE_NAME[] = "mbed HRM1017";
jksoft 0:8c643bfe55b7 9
jksoft 1:48f6e08a3ac2 10 BLEDevice ble;
jksoft 0:8c643bfe55b7 11 Serial pc(USBTX, USBRX);
jksoft 0:8c643bfe55b7 12 /* LEDs for indication: */
jksoft 0:8c643bfe55b7 13 DigitalOut ConnectStateLed(LED1);
jksoft 0:8c643bfe55b7 14 PwmOut ControllerStateLed(LED2);
jksoft 0:8c643bfe55b7 15
robo8080 5:1c04bd9f8457 16 TB6612 left(P0_30,P0_1,P0_0);
robo8080 5:1c04bd9f8457 17 TB6612 right(P0_25,P0_29,P0_28);
robo8080 5:1c04bd9f8457 18
robo8080 5:1c04bd9f8457 19 #define PI 3.141592
robo8080 5:1c04bd9f8457 20 #define neutralAngle 25
robo8080 5:1c04bd9f8457 21 #define neutralRange 5
robo8080 5:1c04bd9f8457 22 #define angleRange 30.0
robo8080 5:1c04bd9f8457 23 #define handlingIntensity 0.7
robo8080 5:1c04bd9f8457 24 float AccSin2Deg(uint8_t acc) // acc : -90 ... 90
robo8080 5:1c04bd9f8457 25 {
robo8080 5:1c04bd9f8457 26 return (float)asin((float)(acc-128.0f)/127.0f)*180.0f/PI;
robo8080 5:1c04bd9f8457 27 }
robo8080 5:1c04bd9f8457 28
robo8080 5:1c04bd9f8457 29 float Acc2Speed(uint8_t acc) // acc : 0 ... 255
robo8080 5:1c04bd9f8457 30 {
robo8080 5:1c04bd9f8457 31 int deg = neutralAngle+(int)AccSin2Deg(acc);
robo8080 5:1c04bd9f8457 32 if(deg>-neutralRange && deg<neutralRange)deg=0;
robo8080 5:1c04bd9f8457 33 float speed=100*(float)deg/angleRange;
robo8080 5:1c04bd9f8457 34 if(speed>100){
robo8080 5:1c04bd9f8457 35 speed=100;
robo8080 5:1c04bd9f8457 36 } else if(speed<-100){
robo8080 5:1c04bd9f8457 37 speed=-100;
robo8080 5:1c04bd9f8457 38 }
robo8080 5:1c04bd9f8457 39 return speed;
robo8080 5:1c04bd9f8457 40 }
robo8080 5:1c04bd9f8457 41
robo8080 5:1c04bd9f8457 42 void RCBCon(uint8_t *buffer, uint16_t buffer_size)
robo8080 5:1c04bd9f8457 43 {
robo8080 5:1c04bd9f8457 44 uint16_t game_pad;
robo8080 5:1c04bd9f8457 45 game_pad = (buffer[0] << 8) | buffer[1];
robo8080 5:1c04bd9f8457 46 // pc.printf("game_pad : %04X\n",game_pad);
robo8080 5:1c04bd9f8457 47 float AccX = (100.0f/90.0f)*AccSin2Deg(buffer[6]);//((float)buffer[6] / 255.0)*200.0 - 100.0;
robo8080 5:1c04bd9f8457 48 // float AccY = AccSin2Deg(buffer[7]);//((float)buffer[7] / 255.0)*200.0 - 100.0;
robo8080 5:1c04bd9f8457 49 float AccY = Acc2Speed(buffer[7]);
robo8080 5:1c04bd9f8457 50 // float AccZ = (100.0f/90.0f)*AccSin2Deg(buffer[8]);((float)buffer[8] / 255.0)*200.0 - 100.0;
robo8080 5:1c04bd9f8457 51 // pc.printf("acc X : %f Y : %f Z : %f\n",acc[0],acc[1],acc[2]);
robo8080 5:1c04bd9f8457 52 float LeftStickX = ((float)buffer[2] / 255.0)*200.0 - 100.0;
robo8080 5:1c04bd9f8457 53 float RightStickX = ((float)buffer[4] / 255.0)*200.0 - 100.0;
robo8080 5:1c04bd9f8457 54 float LeftStickY = ((float)buffer[3] / 255.0)*200.0 - 100.0;
robo8080 5:1c04bd9f8457 55 float RightStickY = ((float)buffer[5] / 255.0)*200.0 - 100.0;
robo8080 5:1c04bd9f8457 56 uint8_t status = buffer[9];
robo8080 5:1c04bd9f8457 57
robo8080 5:1c04bd9f8457 58 if((status & 0x60) == 0x20) { // Accelerometer ON
robo8080 5:1c04bd9f8457 59 float leftData;
robo8080 5:1c04bd9f8457 60 float rightData;
robo8080 5:1c04bd9f8457 61 float xHandling=(float)AccX*handlingIntensity;
robo8080 5:1c04bd9f8457 62 if(game_pad != 0x0020) { // A button
robo8080 5:1c04bd9f8457 63 right = 0;
robo8080 5:1c04bd9f8457 64 left = 0;
robo8080 5:1c04bd9f8457 65 } else {
robo8080 5:1c04bd9f8457 66 leftData = AccY;
robo8080 5:1c04bd9f8457 67 rightData = AccY;
robo8080 5:1c04bd9f8457 68 if(AccY==0) {
robo8080 5:1c04bd9f8457 69 leftData= AccX;
robo8080 5:1c04bd9f8457 70 rightData= -AccX;
robo8080 5:1c04bd9f8457 71 } else if(AccY>0) {
robo8080 5:1c04bd9f8457 72 if(AccX>0) {
robo8080 5:1c04bd9f8457 73 rightData=AccY-(int)xHandling; //r-x
robo8080 5:1c04bd9f8457 74 } else {
robo8080 5:1c04bd9f8457 75 leftData=AccY+(int)xHandling; //l-x
robo8080 5:1c04bd9f8457 76 }
robo8080 5:1c04bd9f8457 77 } else {
robo8080 5:1c04bd9f8457 78 if(AccX>0) {
robo8080 5:1c04bd9f8457 79 leftData=AccY-(int)xHandling; //l-x
robo8080 5:1c04bd9f8457 80 } else {
robo8080 5:1c04bd9f8457 81 rightData=AccY+(int)xHandling; //r-x
robo8080 5:1c04bd9f8457 82 }
robo8080 5:1c04bd9f8457 83 }
robo8080 5:1c04bd9f8457 84 left = (int)leftData;
robo8080 5:1c04bd9f8457 85 right = (int)rightData;
robo8080 5:1c04bd9f8457 86 }
robo8080 5:1c04bd9f8457 87 } else if((status & 0x10)&&(status & 0x08)) { // L : Analog R : Analog
robo8080 5:1c04bd9f8457 88 left = (int)LeftStickY;
robo8080 5:1c04bd9f8457 89 right = (int)RightStickY;
robo8080 5:1c04bd9f8457 90 } else if(status & 0x10) { // L : Analog
robo8080 5:1c04bd9f8457 91 if((LeftStickX < 15) && (LeftStickX > -15)) {
robo8080 5:1c04bd9f8457 92 right = LeftStickY;
robo8080 5:1c04bd9f8457 93 left = LeftStickY;
robo8080 5:1c04bd9f8457 94 }else if((LeftStickY < 15) && (LeftStickY > -15)) {
robo8080 5:1c04bd9f8457 95 right = -LeftStickX;
robo8080 5:1c04bd9f8457 96 left = LeftStickX;
robo8080 5:1c04bd9f8457 97 } else {
robo8080 5:1c04bd9f8457 98 right = 0;
robo8080 5:1c04bd9f8457 99 left = 0;
robo8080 5:1c04bd9f8457 100 }
robo8080 5:1c04bd9f8457 101 } else if(status & 0x08) { // R : Analog
robo8080 5:1c04bd9f8457 102 float leftData;
robo8080 5:1c04bd9f8457 103 float rightData;
robo8080 5:1c04bd9f8457 104 if(RightStickY < 0) {
robo8080 5:1c04bd9f8457 105 RightStickX *= -1.0f;
robo8080 5:1c04bd9f8457 106 }
robo8080 5:1c04bd9f8457 107 if(RightStickX >= 0) {
robo8080 5:1c04bd9f8457 108 leftData = RightStickY + RightStickX;
robo8080 5:1c04bd9f8457 109 rightData = RightStickY;
robo8080 5:1c04bd9f8457 110 } else {
robo8080 5:1c04bd9f8457 111 leftData = RightStickY;
robo8080 5:1c04bd9f8457 112 rightData = RightStickY - RightStickX;
robo8080 5:1c04bd9f8457 113 }
robo8080 5:1c04bd9f8457 114 if(leftData > 100) {
robo8080 5:1c04bd9f8457 115 leftData = 100;
robo8080 5:1c04bd9f8457 116 } else if(leftData < -100) {
robo8080 5:1c04bd9f8457 117 leftData = -100;
robo8080 5:1c04bd9f8457 118 }
robo8080 5:1c04bd9f8457 119 if(rightData > 100) {
robo8080 5:1c04bd9f8457 120 rightData = 100;
robo8080 5:1c04bd9f8457 121 } else if(rightData < -100) {
robo8080 5:1c04bd9f8457 122 rightData = -100;
robo8080 5:1c04bd9f8457 123 }
robo8080 5:1c04bd9f8457 124 left = (int)leftData;
robo8080 5:1c04bd9f8457 125 right = (int)rightData;
robo8080 5:1c04bd9f8457 126 } else if((status & 0x60) == 0x40) { // L : Accelerometer
robo8080 5:1c04bd9f8457 127 if(game_pad != 0x0020) { // A button
robo8080 5:1c04bd9f8457 128 right = 0;
robo8080 5:1c04bd9f8457 129 left = 0;
robo8080 5:1c04bd9f8457 130 } else {
robo8080 5:1c04bd9f8457 131 if((LeftStickX < 15) && (LeftStickX > -15)) {
robo8080 5:1c04bd9f8457 132 right = LeftStickY;
robo8080 5:1c04bd9f8457 133 left = LeftStickY;
robo8080 5:1c04bd9f8457 134 } else if((LeftStickY < 15) && (LeftStickY > -15)) {
robo8080 5:1c04bd9f8457 135 right = -LeftStickX;
robo8080 5:1c04bd9f8457 136 left = LeftStickX;
robo8080 5:1c04bd9f8457 137 } else {
robo8080 5:1c04bd9f8457 138 right = 0;
robo8080 5:1c04bd9f8457 139 left = 0;
robo8080 5:1c04bd9f8457 140 }
robo8080 5:1c04bd9f8457 141 }
robo8080 5:1c04bd9f8457 142 } else { // Digital button
robo8080 5:1c04bd9f8457 143 switch(game_pad)
robo8080 5:1c04bd9f8457 144 {
robo8080 5:1c04bd9f8457 145 case 0x0001:
robo8080 5:1c04bd9f8457 146 left = 50;
robo8080 5:1c04bd9f8457 147 right = 50;
robo8080 5:1c04bd9f8457 148 break;
robo8080 5:1c04bd9f8457 149 case 0x0002:
robo8080 5:1c04bd9f8457 150 left = -50;
robo8080 5:1c04bd9f8457 151 right = -50;
robo8080 5:1c04bd9f8457 152 break;
robo8080 5:1c04bd9f8457 153 case 0x0004:
robo8080 5:1c04bd9f8457 154 left = 50;
robo8080 5:1c04bd9f8457 155 right = -50;
robo8080 5:1c04bd9f8457 156 break;
robo8080 5:1c04bd9f8457 157 case 0x0008:
robo8080 5:1c04bd9f8457 158 left = -50;
robo8080 5:1c04bd9f8457 159 right = 50;
robo8080 5:1c04bd9f8457 160 break;
robo8080 5:1c04bd9f8457 161 default:
robo8080 5:1c04bd9f8457 162 left = 0;
robo8080 5:1c04bd9f8457 163 right = 0;
robo8080 5:1c04bd9f8457 164 }
robo8080 5:1c04bd9f8457 165 }
robo8080 5:1c04bd9f8457 166 }
jksoft 0:8c643bfe55b7 167
jksoft 0:8c643bfe55b7 168 /* RCBController Service */
jksoft 0:8c643bfe55b7 169 static const uint16_t RCBController_service_uuid = 0xFFF0;
jksoft 0:8c643bfe55b7 170 static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
jksoft 1:48f6e08a3ac2 171 uint8_t RCBControllerPayload[10] = {0,};
jksoft 1:48f6e08a3ac2 172
jksoft 1:48f6e08a3ac2 173 GattCharacteristic ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10,
jksoft 0:8c643bfe55b7 174 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
jksoft 0:8c643bfe55b7 175 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
jksoft 1:48f6e08a3ac2 176 GattCharacteristic *ControllerChars[] = {&ControllerChar};
jksoft 1:48f6e08a3ac2 177 GattService RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *));
jksoft 0:8c643bfe55b7 178
jksoft 0:8c643bfe55b7 179 RCBController controller;
jksoft 0:8c643bfe55b7 180
robo8080 5:1c04bd9f8457 181 void onConnected(Gap::Handle_t handle, const Gap::ConnectionParams_t *params)
jksoft 0:8c643bfe55b7 182 {
jksoft 1:48f6e08a3ac2 183 ConnectStateLed = 0;
jksoft 0:8c643bfe55b7 184 #if DBG
jksoft 1:48f6e08a3ac2 185 pc.printf("Connected\n\r");
jksoft 0:8c643bfe55b7 186 #endif
jksoft 1:48f6e08a3ac2 187 }
jksoft 0:8c643bfe55b7 188
robo8080 5:1c04bd9f8457 189 void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
jksoft 1:48f6e08a3ac2 190 {
jksoft 1:48f6e08a3ac2 191 ble.startAdvertising();
jksoft 1:48f6e08a3ac2 192 ConnectStateLed = 1;
robo8080 5:1c04bd9f8457 193 left = 0;
robo8080 5:1c04bd9f8457 194 right = 0;
jksoft 0:8c643bfe55b7 195 #if DBG
jksoft 1:48f6e08a3ac2 196 pc.printf("Disconnected\n\r");
jksoft 0:8c643bfe55b7 197 #endif
jksoft 1:48f6e08a3ac2 198 }
jksoft 1:48f6e08a3ac2 199
jksoft 0:8c643bfe55b7 200
jksoft 0:8c643bfe55b7 201 // GattEvent
robo8080 5:1c04bd9f8457 202 void onDataWritten(uint16_t charHandle, const GattCharacteristicWriteCBParams *params)
jksoft 0:8c643bfe55b7 203 {
robo8080 5:1c04bd9f8457 204 if (charHandle == ControllerChar.getValueAttribute().getHandle()) {
jksoft 1:48f6e08a3ac2 205 uint16_t bytesRead;
robo8080 5:1c04bd9f8457 206 ble.readCharacteristicValue(ControllerChar.getValueAttribute().getHandle(),RCBControllerPayload, &bytesRead);
jksoft 1:48f6e08a3ac2 207 memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller));
jksoft 0:8c643bfe55b7 208 #if DBG
jksoft 1:48f6e08a3ac2 209
jksoft 1:48f6e08a3ac2 210 pc.printf("DATA:%02X %02X %d %d %d %d %d %d %d %02X\n\r",controller.data[0],controller.data[1],controller.data[2],controller.data[3],controller.data[4],
jksoft 0:8c643bfe55b7 211 controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
jksoft 0:8c643bfe55b7 212 #endif
jksoft 1:48f6e08a3ac2 213 ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;
robo8080 5:1c04bd9f8457 214 RCBCon(&controller.data[0], sizeof(controller));
jksoft 1:48f6e08a3ac2 215 }
jksoft 0:8c643bfe55b7 216
jksoft 1:48f6e08a3ac2 217 }
jksoft 0:8c643bfe55b7 218
jksoft 0:8c643bfe55b7 219 /**************************************************************************/
jksoft 0:8c643bfe55b7 220 /*!
jksoft 0:8c643bfe55b7 221 @brief Program entry point
jksoft 0:8c643bfe55b7 222 */
jksoft 0:8c643bfe55b7 223 /**************************************************************************/
jksoft 0:8c643bfe55b7 224 int main(void)
jksoft 0:8c643bfe55b7 225 {
jksoft 0:8c643bfe55b7 226 #if DBG
jksoft 0:8c643bfe55b7 227 pc.printf("Start\n\r");
jksoft 0:8c643bfe55b7 228 #endif
robo8080 5:1c04bd9f8457 229 ConnectStateLed = 1;
robo8080 5:1c04bd9f8457 230 left = 0;
robo8080 5:1c04bd9f8457 231 right = 0;
jksoft 1:48f6e08a3ac2 232
jksoft 2:dd85fdc18224 233 ble.init();
robo8080 5:1c04bd9f8457 234 ble.setDeviceName((uint8_t *)DEVICE_NAME);
jksoft 1:48f6e08a3ac2 235 ble.onConnection(onConnected);
jksoft 1:48f6e08a3ac2 236 ble.onDisconnection(onDisconnected);
jksoft 1:48f6e08a3ac2 237 ble.onDataWritten(onDataWritten);
jksoft 1:48f6e08a3ac2 238
jksoft 1:48f6e08a3ac2 239 /* setup advertising */
jksoft 1:48f6e08a3ac2 240 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
jksoft 1:48f6e08a3ac2 241 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
jksoft 1:48f6e08a3ac2 242 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
jksoft 1:48f6e08a3ac2 243 (const uint8_t *)"mbed HRM1017", sizeof("mbed HRM1017") - 1);
jksoft 1:48f6e08a3ac2 244 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
jksoft 1:48f6e08a3ac2 245 (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
jksoft 0:8c643bfe55b7 246
jksoft 1:48f6e08a3ac2 247 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
jksoft 1:48f6e08a3ac2 248 ble.startAdvertising();
jksoft 0:8c643bfe55b7 249
jksoft 1:48f6e08a3ac2 250 ble.addService(RCBControllerService);
jksoft 0:8c643bfe55b7 251
jksoft 1:48f6e08a3ac2 252 while (true) {
jksoft 1:48f6e08a3ac2 253 ble.waitForEvent();
jksoft 0:8c643bfe55b7 254 }
jksoft 0:8c643bfe55b7 255 }
jksoft 0:8c643bfe55b7 256