Diff: main.cpp

Revision:

1:81cca0074c0d

Parent:

0:1be634fcf3e8

--- a/main.cpp	Thu Apr 16 21:07:54 2015 +0000
+++ b/main.cpp	Wed Apr 22 23:19:46 2015 +0000
@@ -11,94 +11,94 @@
 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);  
+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 pins
+Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
 Motor right(p26,p25,p24, 1);
 
-//maybe ir sensors
+// IR sensors
+//AnalogIn ir_c(p20); // center
+//AnalogIn ir_l(p19); // left
+//AnalogIn ir_r(p18); // right
 
 /*----- 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);
 /*--------------------------------------------------------------------------------------------*/
- 
 
+// Bluetooth variables
 unsigned char buf[16] = {0};
 unsigned char len = 0;
 
+// Motor variables
+float leftval = 0;
+float rightval = 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);
-          }
+    //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");
+
+            while (central.connected()) {
+                // central still connected to peripheral
+                if (rxCharacteristic.written()) {
+                    unsigned char len = rxCharacteristic.valueLength();
+                    const unsigned char *val = rxCharacteristic.value();
+
+                    // Calculate differential motor values from cartesian joystick coordinates
+                    // 0 = north, 1 = south, 2 = east, 3 = 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));
+
+                    // Windup compensation
+                    if (leftval > 1) leftval = 1;
+                    if (leftval < -1) leftval = -1;
+                    if (rightval > 1) rightval = 1;
+                    if (rightval < -1) rightval = -1;
+
+                    // Set motor speed values
+                    left.speed(leftval);
+                    right.speed(rightval);
+
+                    //serial.printf("%f,   %f\n", leftval, rightval);
+                }
+            }
+            // central disconnected
+            //serial.printf("Disconnected from central\r\n");
         }
-        // central disconnected
-        serial.printf("Disconnected from central\r\n");
-      }
     }
 }
\ No newline at end of file