Diff: SG90.cpp

Revision:

0:a62b163b1dbb

diff -r 000000000000 -r a62b163b1dbb SG90.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SG90.cpp	Sat Apr 05 07:56:43 2014 +0000
@@ -0,0 +1,114 @@
+#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;  
+        } 
+    
+   }