Diff: main.cpp

Revision:

1:3f38c4bad274

Parent:

0:c5082e68ff72

Child:

2:9189fa810ef8

--- a/main.cpp	Thu Jan 29 23:30:56 2015 +0000
+++ b/main.cpp	Fri Jan 30 01:46:36 2015 +0000
@@ -16,20 +16,6 @@
 
 #include "mbed.h"
 #include "BLEDevice.h"
-//#include "Servo.h"
-
-
-//#define BLE_UUID_TXRX_SERVICE            0x0000 /**< The UUID of the Nordic UART Service. */
-//#define BLE_UUID_TX_CHARACTERISTIC       0x0002 /**< The UUID of the TX Characteristic. */
-//#define BLE_UUIDS_RX_CHARACTERISTIC      0x0003 /**< The UUID of the RX Characteristic. */
-
-//#define TXRX_BUF_LEN                     20
-
-//#define DIGITAL_OUT_PIN                  P0_9       //TXD
-//#define DIGITAL_IN_PIN                   P0_10      //CTS
-//#define PWM_PIN                          P0_11      //RXD
-//#define SERVO_PIN                        P0_8       //RTS
-//#define ANALOG_IN_PIN                    P0_4       //P04
 
 BLEDevice  ble;
 
@@ -71,50 +57,16 @@
 // BluePropo service
 GattService         serviceBluePropo(UUID_BLUEPROPO, chars, sizeof(chars) / sizeof(GattCharacteristic *));
 
-
-//DigitalOut      LED_SET(DIGITAL_OUT_PIN);
-//DigitalIn       BUTTON(DIGITAL_IN_PIN);
-//PwmOut          PWM(PWM_PIN);
-//AnalogIn        ANALOG(ANALOG_IN_PIN);
-//Servo           MYSERVO(SERVO_PIN);
-
-//Serial pc(USBTX, USBRX);
-
-//static uint8_t analog_enabled = 0;
-//static uint8_t old_state = 0;
-
-// The Nordic UART Service
-//static const uint8_t uart_base_uuid[] = {0x71, 0x3D, 0, 0, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
-//static const uint8_t uart_tx_uuid[]   = {0x71, 0x3D, 0, 3, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
-//static const uint8_t uart_rx_uuid[]   = {0x71, 0x3D, 0, 2, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
-//static const uint8_t uart_base_uuid_rev[] = {0x1E, 0x94, 0x8D, 0xF1, 0x48, 0x31, 0x94, 0xBA, 0x75, 0x4C, 0x3E, 0x50, 0, 0, 0x3D, 0x71};
-
-
+// pin asign
+DigitalOut tb6612_ain1(P0_10); // AIN1:   P0_10 (D2)
+DigitalOut tb6612_ain2(P0_9);  // AIN2:   P0_9  (D1)
+PwmOut     tb6612_pwma(P0_11); // PWMA:   P0_11 (D0)
+PwmOut     servo_pwm  (P0_8);  // SERVO:  P0_8  (D3)
+AnalogIn   sensor_l   (P0_4);  // SENS-L: P0_4  (A3)
+AnalogIn   sensor_r   (P0_5);  // SENS-R: P0_5  (A4)
 
-//uint8_t txPayload[TXRX_BUF_LEN] = {0,};
-//uint8_t rxPayload[TXRX_BUF_LEN] = {0,};
-
-//static uint8_t rx_buf[TXRX_BUF_LEN];
-//static uint8_t rx_len=0;
-
-/*
-GattCharacteristic  txCharacteristic (uart_tx_uuid, txPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
-                                      
-GattCharacteristic  rxCharacteristic (uart_rx_uuid, rxPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
-                                      
-GattCharacteristic *uartChars[] = {&txCharacteristic, &rxCharacteristic};
-
-GattService         uartService(uart_base_uuid, uartChars, sizeof(uartChars) / sizeof(GattCharacteristic *));
-*/
-
-// pin asign
-DigitalOut tb6612_ain1(P0_10); // AIN1:  P0_10 (D2)
-DigitalOut tb6612_ain2(P0_9);  // AIN2:  P0_9  (D1)
-PwmOut     tb6612_pwma(P0_11); // PWMA:  P0_11 (D0)
-PwmOut     servo_pwm  (P0_8);  // SERVO: P0_8  (D3)
-
-// SENS-L: P0_4 (A3)
-// SENS-R: P0_5 (A4)
+// flag for safety
+bool StopFlag = false;
 
 // DC motor driver (TB6612)
 void motor (float speed)
@@ -152,127 +104,78 @@
 {
     //pc.printf("Disconnected \r\n");
     //pc.printf("Restart advertising \r\n");
+    
     ble.startAdvertising();
 }
 
 void WrittenHandler(const GattCharacteristicWriteCBParams *Handler)
-{   
-//    uint8_t buf[TXRX_BUF_LEN];
-//    uint16_t bytesRead;
-    
+{
     if (Handler->charHandle == charStick.getValueAttribute().getHandle()) 
     {
         uint16_t bytesRead;
         ble.readCharacteristicValue(charStick.getValueAttribute().getHandle(), payload, &bytesRead);
         memcpy( &stickData.bytes[0], payload, sizeof(stickData));
-#if DBG
-
-        pc.printf("DATA:%02X %02X\n\r",stickData.bytes[0],stickData.bytes[1]);
-#endif
+        
+        //pc.printf("DATA:%02X %02X\n\r",stickData.bytes[0],stickData.bytes[1]);
+        
         float m = (float)stickData.value.fb / 128.0;
         motor(m);
+//        if(!StopFlag) motor(m);
         float s = (float)stickData.value.lr / 128.0;
         servo(s);
-
-/*
-        ble.readCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(), buf, &bytesRead);
-        memset(txPayload, 0, TXRX_BUF_LEN);
-        memcpy(txPayload, buf, TXRX_BUF_LEN);       
-        
-        //for(index=0; index<bytesRead; index++)
-            //pc.putc(buf[index]);
-            
-        if(buf[0] == 0x01)
-        {
-            if(buf[1] == 0x01)
-                LED_SET = 1;
-            else
-                LED_SET = 0;    
-        }
-        else if(buf[0] == 0xA0)
-        {
-            if(buf[1] == 0x01)
-                analog_enabled = 1;
-            else
-                analog_enabled = 0;
-        }
-        else if(buf[0] == 0x02)
-        {
-            float value = (float)buf[1]/255;
-            PWM = value;
-        }
-        else if(buf[0] == 0x03)
-        {
-            MYSERVO.write(buf[1]);
-        }
-        else if(buf[0] == 0x04)
-        {
-            analog_enabled = 0;
-            PWM = 0;
-            MYSERVO.write(0);
-            LED_SET = 0;
-            old_state = 0;    
-        }
-*/
     }
 }
-/*
-void uartCB(void)
-{   
-    while(pc.readable())    
-    {
-        rx_buf[rx_len++] = pc.getc();    
-        if(rx_len>=20 || rx_buf[rx_len-1]=='\0' || rx_buf[rx_len-1]=='\n')
+
+// Distance sensor averaging number
+#define AVE_SIZE 8
+
+// Distance constant
+// Vout[V] = 2.369V - 0.0369*d[cm]
+// sample = Vout[V]/3.3V * 0xFFFF
+// sample = 47190 - 733.66*d[cm]
+// 10cm : 39853
+// 20cm : 32516
+// 30cm : 25180
+// 40cm : 17843
+// 50cm : 10507
+// 60cm : 3170
+// 64cm : 0
+#define DISTANCE_STOP 17843
+
+void TimerHandler(void)
+{
+    // Averaging of dinstance sensors' value
+    static int cnt = 0;
+    static uint16_t r_buff[AVE_SIZE] ={0,};
+    static uint16_t l_buff[AVE_SIZE] ={0,};
+    static uint32_t r_acc = 0;
+    static uint32_t l_acc = 0;
+    
+    uint16_t r = sensor_r;
+    r_acc -= (uint32_t)r_buff[cnt];
+    r_acc += (uint32_t)r;
+    r_buff[cnt] = r;
+    
+    uint16_t l = sensor_l;
+    l_acc -= (uint32_t)l_buff[cnt];
+    l_acc += (uint32_t)l;
+    l_buff[cnt] = l;
+    
+    cnt++;
+    if(cnt >= AVE_SIZE){
+        cnt = 0;
+        uint16_t r_ave = (uint16_t)(r_acc / AVE_SIZE);
+        uint16_t l_ave = (uint16_t)(l_acc / AVE_SIZE);
+        // Stop! Obstacle Ahead
+        if( (r_ave >= DISTANCE_STOP) && (l_ave >= DISTANCE_STOP) )
         {
-            ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), rx_buf, rx_len); 
-            pc.printf("RecHandler \r\n");
-            pc.printf("Length: ");
-            pc.putc(rx_len);
-            pc.printf("\r\n");
-            rx_len = 0;
-            break;
+            motor(0);
+            StopFlag = true;
+        }else{
+            StopFlag = false;
         }
     }
 }
-*/
-void m_status_check_handle(void)
-{
-/*
-    uint8_t buf[3];
-    if (analog_enabled)  // if analog reading enabled
-    {
-        // Read and send out
-        float s = ANALOG;
-        uint16_t value = s*1024; 
-        buf[0] = (0x0B);
-        buf[1] = (value >> 8);
-        buf[2] = (value);
-        ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, 3); 
-    }
-    
-    // If digital in changes, report the state
-    if (BUTTON != old_state)
-    {
-        old_state = BUTTON;
-        
-        if (BUTTON == 1)
-        {
-            buf[0] = (0x0A);
-            buf[1] = (0x01);
-            buf[2] = (0x00);    
-            ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, 3); 
-        }
-        else
-        {
-            buf[0] = (0x0A);
-            buf[1] = (0x00);
-            buf[2] = (0x00);
-           ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, 3); 
-        }
-    }
-*/
-}
-
 
 int main(void)
 {
@@ -282,7 +185,7 @@
     motor(0);
     
     Ticker ticker;
-    ticker.attach_us(m_status_check_handle, 200000);
+    ticker.attach_us(TimerHandler, 20000); // 20msec interval
     
     // initialize BLE
     ble.init();
@@ -290,16 +193,14 @@
     ble.onDataWritten(WrittenHandler);  
     
     //pc.baud(9600);
-    //pc.printf("SimpleChat Init \r\n");
-    //pc.attach( uartCB , pc.RxIrq);
+    //pc.printf("BLE initialized\r\n");
     
     // setup advertising 
     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
                                     (const uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME) - 1);
-//    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
-//                                    (const uint8_t *)uart_base_uuid_rev, sizeof(uart_base_uuid));
+//  ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
                                     (const uint8_t *)UUID_BLUEPROPO, sizeof(UUID_BLUEPROPO));
     ble.setAdvertisingInterval(160); // 100ms; in multiples of 0.625ms.