SG90.cpp

00001 #include "mbed.h"
00002 #include "SG90.h"
00003 
00004     
00005 //Initialize the position variables to neutral
00006 
00007 servo::servo() : pwm1(PTA5), pwm2(PTA12), pwm3(PTA4), pwm4(PTA5) 
00008     {
00009       
00010       init();  
00011     }
00012 
00013 
00014 
00015 void servo::init() //Initialize all servo pwm's to a periode of 20ms
00016     {
00017         s1=s2=s3=s4=1400; //Initialize the position variables to neutral
00018         pwm1.period_ms(20);
00019         pwm1.pulsewidth_us(s1);
00020         pwm2.period_ms(20);
00021         pwm2.pulsewidth_us(s2);
00022         pwm3.period_ms(20);
00023         pwm3.pulsewidth_us(s3);
00024         pwm4.period_ms(20);
00025         pwm4.pulsewidth_us(s4);
00026     }
00027 
00028 void servo::right(int a)    //Lower the pulsewidth to make the servo turn clockwise
00029     {
00030         switch (a)
00031         {
00032             case 1:
00033                 if (s1 >= 540) s1 = (s1 - 20); 
00034                 pwm1.pulsewidth_us(s1); 
00035                 break;
00036                 
00037             case 2:
00038                 if (s2 >= 540)s2 =s2 -20;
00039                 pwm2.pulsewidth_us(s2);
00040                 break;
00041                 
00042             case 3:
00043                 if (s3 >= 540)s3 = s3 -20;
00044                 pwm3.pulsewidth_us(s3);
00045                 break;
00046                 
00047             case 4:
00048                 if (s4 >= 540)s4 = s4 -20;
00049                 pwm4.pulsewidth_us(s4);
00050                 break;
00051                 
00052             default :
00053                 break;  
00054         } 
00055     }
00056            
00057 void servo::left(int a)          //Raise the pulsewidth to make the servo turn counter clockwise
00058     {
00059          switch (a)
00060         {
00061             case 1:
00062                 if (s1 <=2300) s1 = s1 + 20; 
00063                 pwm1.pulsewidth_us(s1); 
00064                 break;
00065                 
00066             case 2:
00067                 if (s2 <=2300)s2 =s2 +20;
00068                 pwm2.pulsewidth_us(s2);
00069                 break;
00070                 
00071             case 3:
00072                 if (s2 <=2300)s3 = s3 +20;
00073                 pwm3.pulsewidth_us(s3);
00074                 break;
00075                 
00076             case 4:
00077                 if (s2 <=2300)s4 = s4 +20;
00078                 pwm4.pulsewidth_us(s4);
00079                 break;
00080                 
00081             default :
00082                 break;  
00083         } 
00084     }
00085 void servo::position(int a, int p) //Position control of the servor, a is the servo and p is the position in degrees
00086     {
00087           int pw;
00088           pw = 500 + (p*10); 
00089           switch (a)
00090         {
00091             case 1:
00092                 pwm1.pulsewidth_us(pw); 
00093                 break;
00094                 
00095             case 2:
00096                 
00097                 pwm2.pulsewidth_us(pw);
00098                 break;
00099                 
00100             case 3:
00101                 
00102                 pwm3.pulsewidth_us(pw);
00103                 break;
00104                 
00105             case 4:
00106                 
00107                 pwm4.pulsewidth_us(pw);
00108                 break;
00109                 
00110             default :
00111                 break;  
00112         } 
00113     
00114    }