main.cpp

00001 /*
00002  * This repo was forked from jksoft/BLE_RCBController2.
00003  * The copyrights of the original repo belongs to Junichi Katsu.
00004  */
00005 
00006 /*
00007  * Copyright (C) 2014 Bizan Nishimura (@lipoyang)
00008  *
00009  * Licensed under the Apache License, Version 2.0 (the "License");
00010  * you may not use this file except in compliance with the License.
00011  * You may obtain a copy of the License at
00012  *
00013  *     http://www.apache.org/licenses/LICENSE-2.0
00014  *
00015  * Unless required by applicable law or agreed to in writing, software
00016  * distributed under the License is distributed on an "AS IS" BASIS,
00017  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00018  * See the License for the specific language governing permissions and
00019  * limitations under the License.
00020  */
00021 
00022 /*
00023  * Library Dependencies:
00024  * - BLE_API (revision 111)
00025  * - HRM1017
00026  *   - HRM1017/projectconfig.h
00027  *     connection interval constants are modified. (No need?)
00028  *     CFG_GAP_CONNECTION_MIN_INTERVAL_MS 30 
00029  *     CFG_GAP_CONNECTION_MAX_INTERVAL_MS 50
00030  */
00031 
00032 #include "mbed.h"
00033 #include "BLEDevice.h"
00034 
00035 #define DBG 1
00036 
00037 // BLE device object
00038 BLEDevice  ble;
00039 
00040 // BluePropo service UUID
00041 //static const uint16_t UUID_BLUEPROPO = 0xFFF0;
00042 static const uint8_t UUID_BLUEPROPO[] = 
00043 { 0xc4, 0x9d, 0xfd, 0x1b, 0x86, 0x04, 0x41, 0xd2, 0x89, 0x43, 0x13, 0x6f, 0x21, 0x4d, 0xd0, 0xbf };
00044 
00045 // BluePropo::Stick characteristic UUID
00046 //static const uint16_t UUID_BLUEPROPO_STICK = 0xFFF1;
00047 static const uint8_t UUID_BLUEPROPO_STICK[] =
00048 { 0x74, 0x25, 0xfb, 0xa0, 0x72, 0x15, 0x41, 0x36, 0xaa, 0x3f, 0x07, 0x2a, 0xa0, 0x7d, 0x93, 0x54 };
00049 
00050 // Device Name (for display)
00051 #define DEVICE_NAME "MiniSteer HRM1017"
00052 
00053 // BluePropo::Stick data structure
00054 union StickData
00055 {
00056     struct  {
00057         // F(-128)<- 0 ->B(+127)
00058         signed char fb;
00059         // L(-128)<- 0 ->R(+127)
00060         signed char lr;
00061     }value;
00062     unsigned char bytes[2];
00063 };
00064 StickData stickData;
00065 
00066 // buffer for BluePropo payload
00067 uint8_t payload[10] = {0,};
00068 
00069 // BluePropo::Stick characteristic
00070 GattCharacteristic  charStick (UUID_BLUEPROPO_STICK, payload, 2, 2,
00071                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | 
00072                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
00073 // BluePropo characteristics set
00074 GattCharacteristic *chars[] = {&charStick};
00075 // BluePropo service
00076 GattService         serviceBluePropo(UUID_BLUEPROPO, chars, sizeof(chars) / sizeof(GattCharacteristic *));
00077 
00078 // USB COM port for Debug
00079 Serial  pc(USBTX, USBRX);
00080 
00081 // pin asign
00082 DigitalOut tb6612_ain1(P0_28);
00083 DigitalOut tb6612_ain2(P0_29);
00084 PwmOut     tb6612_pwma(P0_30);
00085 PwmOut     servo_pwm  (P0_12);
00086 
00087 // DC motor driver (TB6612)
00088 void motor (float speed)
00089 {
00090     if (speed > 0) {
00091         // CW
00092         tb6612_pwma = speed;
00093         tb6612_ain1 = 1;
00094         tb6612_ain2 = 0;
00095     } else
00096     if (speed < 0) {
00097         // CCW
00098         tb6612_pwma = - speed;
00099         tb6612_ain1 = 0;
00100         tb6612_ain2 = 1;
00101     } else {
00102         // stop
00103         tb6612_pwma = 1;
00104         tb6612_ain1 = 0;
00105         tb6612_ain2 = 0;
00106 //        // break
00107 //        tb6612_pwma = 1;
00108 //        tb6612_ain1 = 1;
00109 //        tb6612_ain2 = 1;
00110     }
00111 }
00112 
00113 // RC servo
00114 void servo (float deg)
00115 {
00116     servo_pwm.pulsewidth_us(1500 + (int)(500.0 * deg));
00117 }
00118 
00119 // BLE onConnection handler
00120 void onConnected(uint16_t h)
00121 {
00122 #if DBG
00123     pc.printf("Connected\n\r");
00124 #endif
00125 }
00126 
00127 // BLE onDisconnection handler
00128 void onDisconnected(uint16_t h)
00129 {
00130     ble.startAdvertising();
00131 #if DBG
00132     pc.printf("Disconnected\n\r");
00133 #endif
00134 }
00135 
00136 // BLE onDataWritten handler (Gatt event)
00137 void onDataWritten(uint16_t charHandle)
00138 {
00139     if (charHandle == charStick.getHandle()) {
00140         uint16_t bytesRead;
00141         ble.readCharacteristicValue(charStick.getHandle(),payload, &bytesRead);
00142         memcpy( &stickData.bytes[0], payload, sizeof(stickData));
00143 #if DBG
00144 
00145         pc.printf("DATA:%02X %02X\n\r",stickData.bytes[0],stickData.bytes[1]);
00146 #endif
00147         float m = (float)stickData.value.fb / 128.0;
00148         motor(m);
00149         float s = (float)stickData.value.lr / 128.0;
00150         servo(s);
00151     }
00152 }
00153 
00154 // Program entry point
00155 int main(void)
00156 {
00157 #if DBG
00158         pc.printf("Start\n\r");
00159 #endif
00160     // initialize servo & motor
00161     servo_pwm.period_ms(20);
00162     servo(0.5);
00163     motor(0);
00164     
00165     // initialize BLE
00166     ble.init(); 
00167     ble.onConnection(onConnected);
00168     ble.onDisconnection(onDisconnected);
00169     ble.onDataWritten(onDataWritten);
00170     // setup advertising
00171     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00172     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00173     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00174                                     (const uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME) - 1);
00175     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
00176                                     (const uint8_t *)UUID_BLUEPROPO, sizeof(UUID_BLUEPROPO));
00177     ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
00178     ble.startAdvertising();
00179     ble.addService(serviceBluePropo);
00180 
00181     // main loop (wait for BLE event)
00182     while (true) {
00183         ble.waitForEvent();
00184     }
00185 }