Diff: main.cpp

Revision:

5:88d466dea486

Parent:

4:ca2ee032bc5a

--- a/main.cpp	Fri Aug 29 04:40:46 2014 +0000
+++ b/main.cpp	Mon Sep 29 11:26:56 2014 +0000
@@ -3,13 +3,17 @@
 #include "RCBController.h"
 #include "TB6612.h"
 
-#define DBG 1
+#define DBG 0
 
 BLEDevice  ble;
 Serial  pc(USBTX, USBRX);
 /* LEDs for indication: */
 DigitalOut  ConnectStateLed(P0_18);
+DigitalOut  testLED(P0_19);
 DigitalOut  outlow(P0_20);
+DigitalIn	sw1(P0_16);
+DigitalIn	sw2(P0_17);
+
 //PwmOut  ControllerStateLed(LED2);
 
 TB6612 left(P0_29,P0_24,P0_23);
@@ -28,7 +32,9 @@
 
 RCBController controller;
 
-void onConnected(uint16_t h)
+int cmode = 0;
+
+void onConnected(Gap::Handle_t handle, const Gap::ConnectionParams_t *params)
 {
     ConnectStateLed = 0;
 #if DBG
@@ -36,7 +42,7 @@
 #endif
 }
 
-void onDisconnected(uint16_t h)
+void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
 {
     ble.startAdvertising();
 	ConnectStateLed = 1;
@@ -47,19 +53,73 @@
 
 
 // GattEvent
-void onDataWritten(uint16_t charHandle)
+void onDataWritten(const GattCharacteristicWriteCBParams *params)
 {
-	if (charHandle == ControllerChar.getHandle()) {
+	if (params->charHandle == ControllerChar.getValueAttribute().getHandle())
+	{
 		uint16_t bytesRead;
-	 	ble.readCharacteristicValue(ControllerChar.getHandle(),RCBControllerPayload, &bytesRead);
-        memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller));
+		memcpy( &controller.data[0], params->data , params->len );
+	 //	ble.readCharacteristicValue(ControllerChar.getHandle(),RCBControllerPayload, &bytesRead);
+     //   memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller));
 #if DBG
 
 		pc.printf("DATA:%02X %02X %d %d %d %d %d %d %d %02X\n\r",controller.data[0],controller.data[1],controller.data[2],controller.data[3],controller.data[4],
 															   controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
 #endif
-			left = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
-			right = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);
+	    float right_factor;
+	    float left_factor;
+
+		left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
+		right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);
+		
+		if( (left_factor != 0.0)||(right_factor != 0.0) )
+		{
+			left = left_factor;
+			right = right_factor;
+		}
+		else
+		{
+			float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 
+
+		    float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
+		    float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
+
+			if( controller.status.B == 1 )
+			{
+				left = left_factor;
+				right = right_factor;
+			}
+			else if( controller.status.A == 1 )
+			{
+				left = -right_factor;
+				right = -left_factor;
+			}
+			else if( controller.status.UP == 1 )
+			{
+				left = 1.0;
+				right = 1.0;
+			}
+			else if( controller.status.DOWN == 1 )
+			{
+				left = -1.0;
+				right = -1.0;
+			}
+			else if( controller.status.RIGHT == 1 )
+			{
+				left = 1.0;
+				right = -1.0;
+			}
+			else if( controller.status.LEFT == 1 )
+			{
+				left = -1.0;
+				right = 1.0;
+			}
+			else
+			{
+				left = 0.0;
+				right = 0.0;	
+			}
+		}
 		//ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;		
 	}
 		 
@@ -74,6 +134,8 @@
 {
 	pc.baud(921600);
 	
+	sw1.mode(PullUp);
+	sw2.mode(PullUp);
 	
 	ConnectStateLed = 1;
 #if DBG
@@ -99,8 +161,19 @@
 
     ble.addService(RCBControllerService);
     
+    testLED = cmode;
+	int old_sw = 0;
 
     while (true) {
+#if 0
+        if(sw1 == 0 && old_sw == 1)
+        {
+        	cmode = !cmode;
+        	testLED = cmode;
+        	wait(0.1);
+        }
+        old_sw = sw1;
+#endif
         ble.waitForEvent();
     }
 }