Diff: main.cpp

Revision:

3:6a9f4ea2e7c6

Parent:

2:dd85fdc18224

Child:

4:06b0361c5975

--- a/main.cpp	Wed Aug 20 13:30:11 2014 +0000
+++ b/main.cpp	Thu Mar 19 03:59:36 2015 +0000
@@ -8,7 +8,7 @@
 Serial  pc(USBTX, USBRX);
 /* LEDs for indication: */
 DigitalOut  ConnectStateLed(LED1);
-PwmOut  ControllerStateLed(LED2);
+PwmOut  servo1(LED2);
 
 
 /* RCBController Service */
@@ -40,6 +40,44 @@
 	pc.printf("Disconnected\n\r");
 #endif
 }
+const int deg0msec = 600; // msec.
+const int deg180msec = 2350; // msec.
+
+void SwOn(void)
+{
+	float range = (float)(deg180msec - deg0msec)/1000000;  //0.00175
+	float pwitdh;
+	for(int i=128;i<210;i++){
+		pwitdh = (float)(i*range)/256+(float)deg0msec/1000000;
+        servo1.pulsewidth(pwitdh);
+        wait(0.001);
+ 	}
+    wait(0.5);
+	for(int i=210;i>128;i--){
+		pwitdh = (float)(i*range)/256+ (float)deg0msec/1000000;
+        servo1.pulsewidth(pwitdh);
+        wait(0.001);
+	}
+    servo1.pulsewidth( 0.00140f );
+
+}
+void SwOff(void)
+{
+	float range = (float)(deg180msec - deg0msec)/1000000;  //0.00175
+	float pwitdh;
+	for(int i=128;i>40;i--){
+		pwitdh = (float)(i*range)/256+(float)deg0msec/1000000;
+        servo1.pulsewidth(pwitdh);
+        wait(0.001);
+ 	}
+    wait(0.5);
+	for(int i=40;i<128;i++){
+		pwitdh = (float)(i*range)/256+ (float)deg0msec/1000000;
+        servo1.pulsewidth(pwitdh);
+        wait(0.001);
+	}
+    servo1.pulsewidth( 0.00140f );
+}
 
 
 // GattEvent
@@ -54,9 +92,25 @@
 		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
-		ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;		
+
+   if (controller.data[0] ==1){
+      SwOn();
+   }
+   else if (controller.data[1] == 0x40){
+      SwOff();
+   }
+/*
+//		servo1 = (float)controller.status.LeftAnalogLR / 255.0;		
+		float pwitdh = (float)controller.status.LeftAnalogLR / 255.0;		
+float range = (float)(deg180msec - deg0msec)/1000000;  //0.00175
+
+pwitdh = pwitdh *range+(float)deg0msec/1000000;
+    pc.printf("pwitdh %f\n\r",pwitdh);
+        servo1.pulsewidth(pwitdh);
 	}
-		 
+*/
+}
+
 }
 
 /**************************************************************************/
@@ -67,6 +121,8 @@
 int main(void)
 {
 #if DBG
+		ConnectStateLed = 1;
+        pc.baud(115200);  
 		pc.printf("Start\n\r");
 #endif
 	
@@ -87,6 +143,8 @@
     ble.startAdvertising();
 
     ble.addService(RCBControllerService);
+    servo1.period_ms(20);
+    servo1.pulsewidth( 0.00140f );
 
     while (true) {
         ble.waitForEvent();