File content as of revision 0:14b64813c04d:

PwmOut PWMservo1(p21);
PwmOut PWMservo2(p22);
PwmOut PWMservo3(p23);
PwmOut PWMservo4(p24);

class Servo
{

public:

    Servo(int, int, int, int, int, int, int);
    int getPWforPosMax (int);
    int getPWforPosIdle (void);
    int getPWforPosMin (int);
    int getPWforPosLow (int);

    int offset;
    int cor;
    int diffForPosMin;
    int diffForPosLow;
    int diffForPosMax;
    int idlePW;
    int PW;
    int reverse_true;

};

Servo::Servo (int of, int mn, int mx, int lw, int idl, int cr, int rev)
{
    offset = of;
    cor = cr;
    diffForPosMin = mn;
    diffForPosMax = mx;
    diffForPosLow = lw;
    idlePW = idl;
    reverse_true = rev;
}

int Servo::getPWforPosMax (int cntr)
{
    if(reverse_true) {
        return(idlePW + offset - diffForPosMax - cor + cntr);
    } else {
        return(idlePW + offset + diffForPosMax + cor - cntr);
    }
}

int Servo::getPWforPosLow (int cntr_)
{
    if(reverse_true) {
        return(idlePW + offset + diffForPosLow - cor - cntr_);
    } else {
        return(idlePW + offset - diffForPosLow + cor + cntr_);
    }
}

int Servo::getPWforPosMin (int cntr_)
{
    if(reverse_true) {
        return(idlePW + offset + diffForPosMin - cor - cntr_);
    } else {
        return(idlePW + offset - diffForPosMin + cor + cntr_);
    }
}

int Servo::getPWforPosIdle ()
{
    if(reverse_true) {
        return(idlePW + offset - cor);
    } else {
        return(idlePW + offset + cor);
    }
}

Servo Servo_1(0, 200, 250, 100, 1500, 0, 0);
Servo Servo_2(0, 200, 250, 100, 1500, 0, 1);
Servo Servo_3(0, 200, 250, 100, 1500, 0, 0);
Servo Servo_4(0, 200, 250, 100, 1500, 0, 1);

//Servo Servo_1(100, 200, 250, 100, 1500, 0, 0);        OLD VALUES (OFFSET)
//Servo Servo_2(30, 200, 250, 100, 1500, 0, 1);
//Servo Servo_3(-10, 200, 250, 100, 1500, 0, 0);
//Servo Servo_4(-120, 200, 250, 100, 1500, 0, 1);

void movePosTop(void)
{
    PWMservo1.pulsewidth_us(Servo_1.getPWforPosMax(0));
    PWMservo2.pulsewidth_us(Servo_2.getPWforPosMax(0));
    PWMservo3.pulsewidth_us(Servo_3.getPWforPosMax(0));
    PWMservo4.pulsewidth_us(Servo_4.getPWforPosMax(0));
}

void movePosTopSlow(void)
{
    int cntr_ = 400;
    while(cntr_ > 0) {
        wait(0.0005);
        PWMservo1.pulsewidth_us(Servo_1.getPWforPosMax(cntr_));
        PWMservo2.pulsewidth_us(Servo_2.getPWforPosMax(cntr_));
        PWMservo3.pulsewidth_us(Servo_3.getPWforPosMax(cntr_));
        PWMservo4.pulsewidth_us(Servo_4.getPWforPosMax(cntr_));
        cntr_--;
    }
}

void movePosLowSlow(void)
{
    int cntr_ = 400;
    while(cntr_ > 0) {
        wait(0.001);
        PWMservo1.pulsewidth_us(Servo_1.getPWforPosLow(cntr_));
        PWMservo2.pulsewidth_us(Servo_2.getPWforPosLow(cntr_));
        PWMservo3.pulsewidth_us(Servo_3.getPWforPosLow(cntr_));
        PWMservo4.pulsewidth_us(Servo_4.getPWforPosLow(cntr_));
        cntr_--;
    }
}

void movePosLow(void)
{
    int cntr_ = 400;
    while(cntr_ > 0) {
        wait(0.0003);
        PWMservo1.pulsewidth_us(Servo_1.getPWforPosLow(cntr_));
        PWMservo2.pulsewidth_us(Servo_2.getPWforPosLow(cntr_));
        PWMservo3.pulsewidth_us(Servo_3.getPWforPosLow(cntr_));
        PWMservo4.pulsewidth_us(Servo_4.getPWforPosLow(cntr_));
        cntr_--;
    }
}

void movePosBottomFast(void)
{
    PWMservo1.pulsewidth_us(Servo_1.getPWforPosMin(0));
    PWMservo2.pulsewidth_us(Servo_2.getPWforPosMin(0));
    PWMservo3.pulsewidth_us(Servo_3.getPWforPosMin(0));
    PWMservo4.pulsewidth_us(Servo_4.getPWforPosMin(0));
}

void movePosBottom(void)
{
    int cntr_ = 400;
    while(cntr_ > 0) {
        wait(0.0003);
        PWMservo1.pulsewidth_us(Servo_1.getPWforPosMin(cntr_));
        PWMservo2.pulsewidth_us(Servo_2.getPWforPosMin(cntr_));
        PWMservo3.pulsewidth_us(Servo_3.getPWforPosMin(cntr_));
        PWMservo4.pulsewidth_us(Servo_4.getPWforPosMin(cntr_));
        cntr_--;
    }
}

void movePosBottomSlow(void)
{
    int cntr_ = 400;
    while(cntr_ > 0) {
        wait(0.0005);
        PWMservo1.pulsewidth_us(Servo_1.getPWforPosMin(cntr_));
        PWMservo2.pulsewidth_us(Servo_2.getPWforPosMin(cntr_));
        PWMservo3.pulsewidth_us(Servo_3.getPWforPosMin(cntr_));
        PWMservo4.pulsewidth_us(Servo_4.getPWforPosMin(cntr_));
        cntr_--;
    }
}

void movePosIdle(void)
{
    PWMservo1.pulsewidth_us(Servo_1.getPWforPosIdle());
    PWMservo2.pulsewidth_us(Servo_2.getPWforPosIdle());
    PWMservo3.pulsewidth_us(Servo_3.getPWforPosIdle());
    PWMservo4.pulsewidth_us(Servo_4.getPWforPosIdle());

}

void correctPlateToLeft(int val)
{
    int corVal = val;
    Servo_1.cor += -corVal;
    Servo_4.cor += -corVal;
    Servo_2.cor +=  corVal;
    Servo_3.cor +=  corVal;
}

void correctPlateToRight(int val)
{
    int corVal = val;
    Servo_2.cor += -corVal;
    Servo_3.cor += -corVal;
    Servo_1.cor +=  corVal;
    Servo_4.cor +=  corVal;
}

void correctPlateToFront(int val)
{
    int corVal = val;
    Servo_1.cor += -corVal;
    Servo_2.cor += -corVal;
    Servo_3.cor += corVal;
    Servo_4.cor += corVal;
}

void correctPlateToBack(int val)
{
    int corVal = val;
    Servo_1.cor +=  corVal;
    Servo_2.cor +=  corVal;
    Servo_3.cor += -corVal;
    Servo_4.cor += -corVal;
}

void resetCorrection(void)
{
    Servo_2.cor =  0;
    Servo_3.cor =  0;
    Servo_1.cor =  0;
    Servo_4.cor =  0;
}


void UpDownx2(void)
{
    movePosTopSlow();
    wait(0.1);
    movePosBottomSlow();
    wait(0.1);
    movePosTopSlow();
    wait(0.1);
    movePosBottomSlow();
    wait(0.1);
}



void initServos()
{
    PWMservo1.period(0.004);          // servo requires a 20ms period
    PWMservo2.period(0.004);
    PWMservo3.period(0.004);
    PWMservo4.period(0.004);
}