Diff: main.cpp

Revision:

4:06b0361c5975

Parent:

3:6a9f4ea2e7c6

--- a/main.cpp	Thu Mar 19 03:59:36 2015 +0000
+++ b/main.cpp	Thu Mar 19 06:52:09 2015 +0000
@@ -41,45 +41,16 @@
 #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 );
+const int deg180msec = 2200; // msec.
 
-}
-void SwOff(void)
+//In float degree -90 to +90
+void SetServoDegree(float degree)
 {
-	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 );
+	float range = (float)(deg180msec - deg0msec) / 180;
+    float pwitdh = (float)((degree+90)*range)+(float)deg0msec;
+    servo1.pulsewidth(pwitdh/1000000);
 }
 
-
 // GattEvent
 void onDataWritten(uint16_t charHandle)
 {
@@ -93,24 +64,16 @@
 															   controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
 #endif
 
-   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);
-	}
-*/
-}
-
+        if (controller.data[0] == 1){
+	        SetServoDegree(-45);
+        }
+        else if (controller.data[1] == 0x40){
+	        SetServoDegree(45);
+        }
+        else if (controller.data[1] == 0 && controller.data[0] == 0){
+	        SetServoDegree(0);
+        }
+    }
 }
 
 /**************************************************************************/
@@ -144,7 +107,7 @@
 
     ble.addService(RCBControllerService);
     servo1.period_ms(20);
-    servo1.pulsewidth( 0.00140f );
+    SetServoDegree(0);
 
     while (true) {
         ble.waitForEvent();