main.cpp

00001 /*
00002  * Copyright (C) 2015 Bizan Nishimura (@lipoyang)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "mbed.h"
00018 #include "BLEDevice.h"
00019 
00020 BLEDevice  ble;
00021 
00022 // BluePropo service UUID
00023 //static const uint16_t UUID_BLUEPROPO = 0xFFF0;
00024 static const uint8_t UUID_BLUEPROPO[] = 
00025 { 0xc4, 0x9d, 0xfd, 0x1b, 0x86, 0x04, 0x41, 0xd2, 0x89, 0x43, 0x13, 0x6f, 0x21, 0x4d, 0xd0, 0xbf };
00026 
00027 // BluePropo::Stick characteristic UUID
00028 //static const uint16_t UUID_BLUEPROPO_STICK = 0xFFF1;
00029 static const uint8_t UUID_BLUEPROPO_STICK[] =
00030 { 0x74, 0x25, 0xfb, 0xa0, 0x72, 0x15, 0x41, 0x36, 0xaa, 0x3f, 0x07, 0x2a, 0xa0, 0x7d, 0x93, 0x54 };
00031 
00032 // Device Name (for display)
00033 #define DEVICE_NAME "MiniSteer BLE Nano"
00034 
00035 // BluePropo::Stick data structure
00036 union StickData
00037 {
00038     struct  {
00039         // F(-128)<- 0 ->B(+127)
00040         signed char fb;
00041         // L(-128)<- 0 ->R(+127)
00042         signed char lr;
00043     }value;
00044     unsigned char bytes[2];
00045 };
00046 StickData stickData;
00047 
00048 // buffer for BluePropo payload
00049 uint8_t payload[10] = {0,};
00050 
00051 // BluePropo::Stick characteristic
00052 GattCharacteristic  charStick (UUID_BLUEPROPO_STICK, payload, 2, 2,
00053                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | 
00054                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
00055 // BluePropo characteristics set
00056 GattCharacteristic *chars[] = {&charStick};
00057 // BluePropo service
00058 GattService         serviceBluePropo(UUID_BLUEPROPO, chars, sizeof(chars) / sizeof(GattCharacteristic *));
00059 
00060 // pin asign
00061 DigitalOut tb6612_ain1(P0_10); // AIN1:   P0_10 (D2)
00062 DigitalOut tb6612_ain2(P0_9);  // AIN2:   P0_9  (D1)
00063 PwmOut     tb6612_pwma(P0_11); // PWMA:   P0_11 (D0)
00064 PwmOut     servo_pwm  (P0_8);  // SERVO:  P0_8  (D3)
00065 AnalogIn   sensor_l   (P0_4);  // SENS-L: P0_4  (A3)
00066 AnalogIn   sensor_r   (P0_5);  // SENS-R: P0_5  (A4)
00067 
00068 // flag for safety
00069 bool StopFlag = false;
00070 
00071 // DC motor driver (TB6612)
00072 void motor (float speed)
00073 {
00074     if (speed > 0) {
00075         // CW
00076         tb6612_pwma = speed;
00077         tb6612_ain1 = 1;
00078         tb6612_ain2 = 0;
00079     } else
00080     if (speed < 0) {
00081         // CCW
00082         tb6612_pwma = - speed;
00083         tb6612_ain1 = 0;
00084         tb6612_ain2 = 1;
00085     } else {
00086         // stop
00087         tb6612_pwma = 1;
00088         tb6612_ain1 = 0;
00089         tb6612_ain2 = 0;
00090     }
00091 }
00092 
00093 void motor_break()
00094 {
00095     // break
00096     tb6612_pwma = 1;
00097     tb6612_ain1 = 1;
00098     tb6612_ain2 = 1;
00099 }
00100 
00101 // you must adjust center position
00102 #define RL_ADJ      (-120)
00103 #define RL_CENTER   (1500)
00104 #define RL_RANGE    (200.0)
00105 
00106 // RC servo
00107 // rl : -1.0 ? 1.0
00108 void servo (float rl)
00109 {
00110     servo_pwm.pulsewidth_us(RL_CENTER + RL_ADJ - (int)(RL_RANGE * rl));
00111 }
00112 
00113 void connectionCallback(Gap::Handle_t handle, Gap::addr_type_t peerAddrType, const Gap::address_t peerAddr, const Gap::ConnectionParams_t *params)
00114 {
00115     servo(0);
00116 }
00117 
00118 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
00119 {
00120     //pc.printf("Disconnected \r\n");
00121     //pc.printf("Restart advertising \r\n");
00122     
00123     servo_pwm.pulsewidth_us(0);
00124     
00125     ble.startAdvertising();
00126 }
00127 
00128 void WrittenHandler(const GattCharacteristicWriteCBParams *Handler)
00129 {
00130     if (Handler->charHandle == charStick.getValueAttribute().getHandle()) 
00131     {
00132         uint16_t bytesRead;
00133         ble.readCharacteristicValue(charStick.getValueAttribute().getHandle(), payload, &bytesRead);
00134         memcpy( &stickData.bytes[0], payload, sizeof(stickData));
00135         
00136         //pc.printf("DATA:%02X %02X\n\r",stickData.bytes[0],stickData.bytes[1]);
00137         
00138         float m = (float)stickData.value.fb / 128.0;
00139         if(!StopFlag) motor(m);
00140         if( StopFlag && (m == 0)) StopFlag = false;
00141 //        motor(m);
00142         float s = (float)stickData.value.lr / 128.0;
00143         servo(s);
00144     }
00145 }
00146 
00147 // Distance sensor averaging number
00148 #define AVE_SIZE 8
00149 
00150 // Distance constant (for Sharp GP2Y0E03)
00151 // Vout[V] = 2.369V - 0.0369*d[cm]
00152 // sample = Vout[V]/3.3V * 0xFFFF
00153 // sample = 47190 - 733.66*d[cm]
00154 // 10cm : 39853 / 64 = 623
00155 // 20cm : 32516 / 64 = 508
00156 // 30cm : 25180 / 64 = 393
00157 // 40cm : 17843 / 64 = 278
00158 // 50cm : 10507 / 64 = 164
00159 // 60cm : 3170 / 64 = 50
00160 // 64cm : 0
00161 #define DISTANCE_STOP 278
00162 
00163 void TimerHandler(void)
00164 {
00165     // Averaging of dinstance sensors' value
00166     static int cnt = 0;
00167     static uint16_t r_buff[AVE_SIZE] ={0,};
00168     static uint16_t l_buff[AVE_SIZE] ={0,};
00169     static uint32_t r_acc = 0;
00170     static uint32_t l_acc = 0;
00171     
00172     uint16_t r = sensor_r.read_u16();
00173     r_acc -= (uint32_t)r_buff[cnt];
00174     r_acc += (uint32_t)r;
00175     r_buff[cnt] = r;
00176     
00177     uint16_t l = sensor_l.read_u16();
00178     l_acc -= (uint32_t)l_buff[cnt];
00179     l_acc += (uint32_t)l;
00180     l_buff[cnt] = l;
00181     
00182     cnt++;
00183     if(cnt >= AVE_SIZE){
00184         cnt = 0;
00185         uint16_t r_ave = (uint16_t)(r_acc / AVE_SIZE);
00186         uint16_t l_ave = (uint16_t)(l_acc / AVE_SIZE);
00187         // Stop! Obstacle Ahead
00188         if( (r_ave >= DISTANCE_STOP) && (l_ave >= DISTANCE_STOP) )
00189         {
00190             //motor(0);
00191             motor_break();
00192             StopFlag = true;
00193         }else{
00194             //StopFlag = false;
00195         }
00196     }
00197 }
00198 
00199 int main(void)
00200 {
00201     // initialize servo & motor
00202     servo_pwm.period_ms(20);
00203     servo_pwm.pulsewidth_us(0);
00204 //    servo(0);
00205     motor(0);
00206     
00207     Ticker ticker;
00208     ticker.attach_us(TimerHandler, 20000); // 20msec interval
00209     
00210     // initialize BLE
00211     ble.init();
00212     
00213     ble.onConnection(connectionCallback);
00214     ble.onDisconnection(disconnectionCallback);
00215     ble.onDataWritten(WrittenHandler);  
00216     
00217     //pc.baud(9600);
00218     //pc.printf("BLE initialized\r\n");
00219     
00220     // setup advertising 
00221     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00222     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00223     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00224                                     (const uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME) - 1);
00225   ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
00226 //    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
00227                                     (const uint8_t *)UUID_BLUEPROPO, sizeof(UUID_BLUEPROPO));
00228     ble.setAdvertisingInterval(160); // 100ms; in multiples of 0.625ms. 
00229     ble.addService(serviceBluePropo);
00230     ble.startAdvertising(); 
00231     //pc.printf("Advertising Start \r\n");
00232     
00233     // main loop (wait for BLE event)
00234     while(true)
00235     {
00236         ble.waitForEvent(); 
00237     }
00238 }