Diff: main.cpp

Revision:

0:1be634fcf3e8

diff -r 000000000000 -r 1be634fcf3e8 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 16 21:07:54 2015 +0000
@@ -0,0 +1,104 @@
+#include "mbed.h"
+#include "BLEPeripheral.h"
+#include "motordriver.h"
+#include "stdint.h"
+
+//*******************************
+// Must define Serial serial and SPI spi
+// BLE_RDY and BLE_REQ
+//*******************************
+
+Serial serial(USBTX, USBRX);
+// The SPI construct, REQN and RDYN IO construct should be modified manually
+// It depend on the board you are using and the REQN&RDYN configuration on BLE Shield
+SPI spi(p5, p6, p7);      
+DigitalInOut BLE_RDY(p8);  
+DigitalInOut BLE_REQ(p9);
+DigitalInOut BLE_RST(p10);
+
+//Motor Pins
+Motor left(p21, p22, p23, 1); //pwm fwd ref has brake feature
+Motor right(p26,p25,p24, 1);
+
+//maybe ir sensors
+
+/*----- BLE Utility -------------------------------------------------------------------------*/
+// create peripheral instance, see pinouts above
+BLEPeripheral            blePeripheral        = BLEPeripheral(&BLE_REQ, &BLE_RDY, &BLE_RST);
+ 
+// create service
+BLEService               uartService          = BLEService("713d0000503e4c75ba943148f18d941e");
+ 
+// create characteristic
+BLECharacteristic    txCharacteristic = BLECharacteristic("713d0002503e4c75ba943148f18d941e", BLENotify, 20);
+BLECharacteristic    rxCharacteristic = BLECharacteristic("713d0003503e4c75ba943148f18d941e", BLEWriteWithoutResponse, 20);
+/*--------------------------------------------------------------------------------------------*/
+ 
+
+unsigned char buf[16] = {0};
+unsigned char len = 0;
+
+
+int main()
+{
+  
+  serial.baud(9600);
+  serial.printf("Serial begin!\r\n");
+ 
+/*----- BLE Utility ---------------------------------------------*/
+  // set advertised local name and service UUID
+  blePeripheral.setLocalName("BLE Shield");
+  
+  blePeripheral.setAdvertisedServiceUuid(uartService.uuid());
+  
+  // add service and characteristic
+  blePeripheral.addAttribute(uartService);
+  blePeripheral.addAttribute(rxCharacteristic);
+  blePeripheral.addAttribute(txCharacteristic);
+ 
+  // begin initialization
+  blePeripheral.begin();
+/*---------------------------------------------------------------*/
+  serial.printf("BLE UART Peripheral begin!\r\n");
+  
+  while(1)
+  {
+      BLECentral central = blePeripheral.central();
+    
+      if (central) 
+      {
+        // central connected to peripheral
+        serial.printf("Connected to central\r\n");
+        
+        int count = 0;
+        float leftval = 0;
+        float rightval = 0;
+        while (central.connected()) 
+        {
+          // central still connected to peripheral
+          if (rxCharacteristic.written()) 
+          {
+            count = 0;
+            unsigned char len = rxCharacteristic.valueLength();
+            const unsigned char *val = rxCharacteristic.value();
+            
+            //1 = north, 2 = south, 3 = east, 4 = west
+            leftval = sqrt(pow((float)val[0]/255,2) + pow((float)val[2]/255,2)) - sqrt(pow((float)val[1]/255,2) + pow((float)val[3]/255,2));
+            rightval = sqrt(pow((float)val[0]/255,2) + pow((float)val[3]/255,2)) - sqrt(pow((float)val[1]/255,2) + pow((float)val[2]/255,2));
+            
+            if (leftval > 1) leftval = 1;
+            if (leftval < -1) leftval = -1;
+            if (rightval > 1) rightval = 1;
+            if (rightval < -1) rightval = -1;
+            
+            left.speed(leftval);
+            right.speed(rightval);
+            
+            serial.printf("%f,   %f\n", leftval, rightval);
+          }
+        }
+        // central disconnected
+        serial.printf("Disconnected from central\r\n");
+      }
+    }
+}
\ No newline at end of file