File content as of revision 5:90f7d451910e:

#include "mbed.h"
#include "Servo.h"

#define NUM_SERVOS 4
Servo servo[NUM_SERVOS]={p24,p23,p22,p21};

void bothForwardOrBack(float min, float max, int direction, int startVEnd); //1 is forward, 2 is backwards
void oneSideForwardOrBack(float min, float max, int direction, int side, int startVEnd);//1 is forward 2 is back, 1 is left 0 is right
void movementCombo(float firstDisplace, float secondDisplace, int forwardVBack, int whichSideFirst);//1 is left, zero is right

Timer t;
float interval1 = 10;
float alreadyPast = 0;
int main() {  
    t.start();   
    float previousSeconds = 0;
    while(1) {   
        float currentSeconds = t.read();
        
        float disp1 = 0.5;
        float disp2 = disp1 + 0.5;
        //THIS PART OF THE FUNCTION RUNS BASED ON A TIME LOOP
        if(currentSeconds-previousSeconds>interval1)
        {
            previousSeconds = currentSeconds;
            disp2 = disp1 + 0.2;
            movementCombo(disp1, disp2, 1, 1);
            wait(0.5);
            movementCombo(disp1, disp2, 2, 1);
            wait(1);
            
            disp1 = 0.9;
            disp2 = 1.0;
            movementCombo(disp1, disp2, 1, 0);
            wait(0.5);
            movementCombo(disp1, disp2, 2, 1);
            wait(1);   
            
            disp1 = 0.1;
            disp2 = disp1 + 0.5;
            movementCombo(disp1, disp2, 1, 0);
            wait(0.5);
            movementCombo(disp1, disp2, 2, 0);
            wait(1);   
        }
        
        //THIS PART OF THE FUNCTION CONSTANTLY RUNS
        movementCombo(disp1, disp2, 1, 1);
        wait(0.5);
        movementCombo(disp1, disp2, 2, 1);  
        wait(1);
    }
    
}
void bothForwardOrBack(float min, float max, int direction, int startVEnd)
{
    //1 is forward 2 is backwards
    float minServo, maxServo;
    if (direction == 1)
    {
        minServo = NUM_SERVOS/2;
        maxServo = NUM_SERVOS;
    }
    else if (direction == 2)
    {
        minServo = 0;
        maxServo = NUM_SERVOS/2;
    }
    float i; 
    int j;
 
    float incr = 0.01;
    if(startVEnd == 0)
    {
        for(i = min; i<max; i+=incr)
        {
            for(j = minServo; j<maxServo; j++)
            {
                servo[j] = i;
            }
            wait(0.005);
        }
    }
    else if(startVEnd == 1)
    {
        for(i = max; i>min; i-=incr)
        {
            for(j = minServo; j<maxServo; j++)
            {
                servo[j] = i;
            }
            wait(0.005);
        }
    }
    printf("finished one run of bothForwardOrBack\n");
}
void movementCombo(float firstDisplace, float secondDisplace, int forwardVBack, int whichSideFirst)
{
    //forward is 1, back is 2
    //1 is left first, 0 is right first
    
    bothForwardOrBack(0.0, firstDisplace, forwardVBack, 0); //moves both ears partway
    wait(1);
    if(whichSideFirst == 1)
    {
        oneSideForwardOrBack(firstDisplace, secondDisplace, forwardVBack, 1, 0); //moves left ear rest of way 
        oneSideForwardOrBack(firstDisplace, secondDisplace, forwardVBack, 1, 1); //moves left ear back to position
        oneSideForwardOrBack(firstDisplace, secondDisplace, forwardVBack, 0, 0); //moves right ear rest of way 
        oneSideForwardOrBack(firstDisplace, secondDisplace, forwardVBack, 0, 1); //moves right ear back to position
    }
    else
    {
        oneSideForwardOrBack(firstDisplace, secondDisplace, forwardVBack, 0, 0); //moves right ear rest of way 
        oneSideForwardOrBack(firstDisplace, secondDisplace, forwardVBack, 0, 1); //moves right ear back to position
        oneSideForwardOrBack(firstDisplace, secondDisplace, forwardVBack, 1, 0); //moves left ear rest of way 
        oneSideForwardOrBack(firstDisplace, secondDisplace, forwardVBack, 1, 1); //moves left ear back to position
    }
    wait(1);
    bothForwardOrBack(0.0, firstDisplace, forwardVBack, 1); //moves both ears to position 0
    
    wait(1);
}
void oneSideForwardOrBack(float min, float max, int direction, int side, int startVEnd)
{
    //direction: one is forwards two is backwards
    //0 is right, 1 is left
    int whichServo = 0;
    if(direction == 1)
    {
        whichServo = whichServo + NUM_SERVOS/2;
    }      
    if(side == 1)
    {
        whichServo = whichServo + 1;
    }

    float incr = 0.01, i;
    if(startVEnd == 0)
    {
        for(i = min; i<max; i+=incr)
        {
            servo[whichServo] = i;
            wait(0.002);
        }
        wait(0.1);
    }
    else if(startVEnd == 1)
    {
        for(i = max; i>min; i-=incr)
        {
            servo[whichServo] = i;
            wait(0.002);
        }
        wait(0.1);
    }
    printf("finished one run of oneSideForwardOrBack\n");
}