Diff: SG90.cpp

Revision:

1:93e46a70966f

Parent:

0:a62b163b1dbb

--- a/SG90.cpp	Sat Apr 05 07:56:43 2014 +0000
+++ b/SG90.cpp	Mon Jun 18 12:02:48 2018 +0000
@@ -1,114 +1,15 @@
-#include "mbed.h"
 #include "SG90.h"
 
-    
-//Initialize the position variables to neutral
-
-servo::servo() : pwm1(PTA5), pwm2(PTA12), pwm3(PTA4), pwm4(PTA5) 
-    {
-      
-      init();  
-    }
-
-
-
-void servo::init() //Initialize all servo pwm's to a periode of 20ms
-    {
-        s1=s2=s3=s4=1400; //Initialize the position variables to neutral
-        pwm1.period_ms(20);
-        pwm1.pulsewidth_us(s1);
-        pwm2.period_ms(20);
-        pwm2.pulsewidth_us(s2);
-        pwm3.period_ms(20);
-        pwm3.pulsewidth_us(s3);
-        pwm4.period_ms(20);
-        pwm4.pulsewidth_us(s4);
-    }
-
-void servo::right(int a)    //Lower the pulsewidth to make the servo turn clockwise
-    {
-        switch (a)
-        {
-            case 1:
-                if (s1 >= 540) s1 = (s1 - 20); 
-                pwm1.pulsewidth_us(s1); 
-                break;
-                
-            case 2:
-                if (s2 >= 540)s2 =s2 -20;
-                pwm2.pulsewidth_us(s2);
-                break;
-                
-            case 3:
-                if (s3 >= 540)s3 = s3 -20;
-                pwm3.pulsewidth_us(s3);
-                break;
-                
-            case 4:
-                if (s4 >= 540)s4 = s4 -20;
-                pwm4.pulsewidth_us(s4);
-                break;
-                
-            default :
-                break;  
-        } 
-    }
-           
-void servo::left(int a)          //Raise the pulsewidth to make the servo turn counter clockwise
-    {
-         switch (a)
-        {
-            case 1:
-                if (s1 <=2300) s1 = s1 + 20; 
-                pwm1.pulsewidth_us(s1); 
-                break;
-                
-            case 2:
-                if (s2 <=2300)s2 =s2 +20;
-                pwm2.pulsewidth_us(s2);
-                break;
-                
-            case 3:
-                if (s2 <=2300)s3 = s3 +20;
-                pwm3.pulsewidth_us(s3);
-                break;
-                
-            case 4:
-                if (s2 <=2300)s4 = s4 +20;
-                pwm4.pulsewidth_us(s4);
-                break;
-                
-            default :
-                break;  
-        } 
-    }
-void servo::position(int a, int p) //Position control of the servor, a is the servo and p is the position in degrees
-    {
-          int pw;
-          pw = 500 + (p*10); 
-          switch (a)
-        {
-            case 1:
-                pwm1.pulsewidth_us(pw); 
-                break;
-                
-            case 2:
-                
-                pwm2.pulsewidth_us(pw);
-                break;
-                
-            case 3:
-                
-                pwm3.pulsewidth_us(pw);
-                break;
-                
-            case 4:
-                
-                pwm4.pulsewidth_us(pw);
-                break;
-                
-            default :
-                break;  
-        } 
-    
-   }    
+SG90::SG90(PinName pwmPin):PwmOut(pwmPin){
+    iPreRotate=1450;
+    period_ms(20);// 50Hz to trigger SG90
+    pulsewidth_us(iPreRotate); //500:90 1450:0 2400:-90   
+}
+void SG90::SetAngle(float fAngle){
+    if (fAngle>90.0f)
+        fAngle=90.0f;
+    else if(fAngle<-90.0f)
+        fAngle=-90.0f;
+    int iRotate=(-(fAngle-90.0f)*950.0f/90.0f)+500.0f;
+    pulsewidth_us(iRotate);
+}
\ No newline at end of file