File content as of revision 0:a62b163b1dbb:

#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;  
        } 
    
   }