Kenji Arai / Mbed OS BLE_RCBController2_PWM

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of BLE_RCBController2_Motor by robo 8080

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
Child:
6:1aff9f05d487
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