File content as of revision 0:4192ed0f6a57:

/*

 BY: www.emcu.eu
 Date: 08-12-2017
 Scatola Magica - Ver. 1.0
 Servo Motor: MG996R 
 This example was tested on NUCLEO-F401RE
 If you use a terminal emulator (9600,8,1) on your PC 
    you also see the commands sends do RC motor.
 
 https://os.mbed.com/users/emcu/code/MG996R_Drive/
    
---------------------------------------------------------------------------
 
 Servo Motor:
 MG996R - http://www.electronicoscaldas.com/datasheet/MG996R_Tower-Pro.pdf
 
 RED    - + Vcc from 4,5 to 7,2V
 BLACK  - GND
 ORANGE - PWM
 
  Duty - example: 1.9 (mS)
  \  / 
   \/
   __
__|  |_____________
     ... 20mS......
     
 For rotate to right use value 1 for Duty
 For rotate to left use value 5 for Duty
  
 Vedere queste spiegazioni: 
 http://www.adrirobot.it/servotester/il_servomotore.htm
 
 
 
 NOTE:
 
 For rotate to Right (DESTRA) use this command:
 Move(Right);
 For rotate to Left (SINISTRA) use this command:
 Move(Left);
 
 You also has the possibility to send a command like below:
 Move(1.5):
  
*/

#include "mbed.h"

DigitalOut Drive(PA_10);  // RC apre scatola
DigitalOut DriveI(PB_3);  // RC spegne interruttore
DigitalIn  IN1(PB_5);     // Ingresso Interruttore
DigitalOut myled(LED1);
DigitalOut LedRUN(PB_4);

#define PWMoff 20 // Waiting for end PWD
#define Right   1 // Move to Right -> DESTRA   or DX
#define Left    5 // Move to LEFT  <- SINISTRA or SX
#define DLY     0.5

void Move(double);
void MovDiTest(void);
void SpegniInterruttore(void);
void FintoSpegniInterruttore(void);

void Seq1(void);
void Seq2(void);
void Seq3(void);
void Seq4(void);
void Seq5(void);
void Seq6(void);

double p=0;

void MoveI(double ON)
{ 
DriveI=1;
wait_ms(ON);
DriveI=0;
wait_ms(20);
}


void Move(double ON)
{ 
Drive=1;
wait_ms(ON);
Drive=0;
wait_ms(20);
}

int main() 
{  
    int NumeroDiAperture=0;
    int n = 0;
    
    IN1.mode(PullUp);

    printf("By: www.emcu.eu - Ver.1.0 \n\r");

    // Porta motore Interruttore in posizione di RESET
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<30; n++)
        { 
        MoveI(Left); 
        LedRUN = !LedRUN; 
        } 
    wait(DLY);  
    LedRUN = 1;
    
    // Porta motore Apertura Scatola in posizione di RESET
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);
    
    while(1) 
        {
        if (IN1 == 1)
            { // Button is pressed
                //myled = 1;
                LedRUN = 1;
                printf("Start sequenza \n\r");
                wait(1);
                NumeroDiAperture++;
                
                if (NumeroDiAperture==1)
                    Seq1();
                if (NumeroDiAperture==2)
                    Seq2();                    
                if (NumeroDiAperture==3)
                    Seq3();  
                if (NumeroDiAperture==4)
                    Seq4();
                if (NumeroDiAperture==5)
                    Seq5();
                if (NumeroDiAperture==6)
                    Seq6();
                                                                                                
                if (NumeroDiAperture==6)
                    NumeroDiAperture=0;
                
                // MovDiTest();
            }
        else
            // myled = 0;
            LedRUN = 0;
        }

    
//    MovDiTest();

}


void Seq1(void)
{
    int n=0;
    
    printf("\n\rSequenza n.1\n\r");
    printf("Move to 11 times to ->\n\r");
    for(n=0; n<11; n++)
        Move(Right);
    wait(DLY);
    SpegniInterruttore();
    
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);
    printf("FINE - Sequenza n.1\n\r");
}


void Seq2(void)
{
    int n=0;
    
    printf("\n\rSequenza n.2\n\r");
    printf("Move to 11 times to ->\n\r");
    for(n=0; n<11; n++)
        Move(Right);
    wait(0.1);
    SpegniInterruttore();
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);
        
    printf("Move to 9 times to ->\n\r");
    for(n=0; n<9; n++)
        Move(Right);    
    wait(0.5);
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);     
    wait(DLY); 

    printf("\n\rSequenza n.2\n\r");
    printf("Move to 11 times to ->\n\r");
    for(n=0; n<11; n++)
        Move(Right);
    wait(DLY);
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);    
    printf("FINE - Sequenza n.2\n\r");
}


void Seq3(void)
{
    int n=0;
    
    printf("\n\rSequenza n.3\n\r");
    printf("Move to 11 times to ->\n\r");
    for(n=0; n<11; n++)
        Move(Right);
    wait(0.1);
    SpegniInterruttore();
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);
        
    printf("Move to 9 times to ->\n\r");
    for(n=0; n<9; n++)
        Move(Right);    
    wait(0.5);
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);     
    wait(DLY); 
    
    printf("Move to 7 times to ->\n\r");
    for(n=0; n<7; n++)
        Move(Right);    
    wait(0.5);
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);     
    wait(DLY);     
    printf("FINE - Sequenza n.3\n\r");
}


void Seq4(void)
{
    int n=0;
    int m=0;
    
    printf("\n\rSequenza n.4\n\r");
    printf("Move to 11 times to ->\n\r");
    for(n=0; n<11; n++)
        Move(Right);
    wait(0.1);
    SpegniInterruttore();
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);

    wait(0.2);        
    for (m=0; m<5; m++)
        {                  
        printf("Move to 9 times to ->\n\r");
        for(n=0; n<8; n++)
            Move(Right);    
        wait(0.2);
        printf("Rotate all to <- \n\r");
        // Sposta tutto a SX 
        for(n=0; n<20; n++)
            Move(Left);  
        
        //wait(0.05);    
        }         
    printf("FINE - Sequenza n.4\n\r");
}


void Seq5(void)
{
    int n=0;
    int m=0;
    
    printf("\n\rSequenza n.5\n\r");
    printf("Move to 11 times to ->\n\r");
    for(n=0; n<11; n++)
        Move(Right);
    wait(0.1);
    SpegniInterruttore();
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);

    wait(0.2);        
    for (m=0; m<5; m++)
        {                  
        printf("Move to 9 times to ->\n\r");
        for(n=0; n<8; n++)
            Move(Right);    
        wait(0.2);
        printf("Rotate all to <- \n\r");
        // Sposta tutto a SX 
        for(n=0; n<20; n++)
            Move(Left);  
        
        //wait(0.05);    
        }         
        
    FintoSpegniInterruttore();
    
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);    
    
    printf("FINE - Sequenza n.5\n\r");
}


void Seq6(void)
{
    int n=0;
    
    printf("\n\rSequenza n.6\n\r");
    printf("Move to 11 times to ->\n\r");
    for(n=0; n<11; n++)
        {
        Move(Right);
        wait(0.1);
        }
    wait(DLY);
    SpegniInterruttore();  
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);
    wait(DLY);
        
    printf("Move to 8 times to ->\n\r");
    for(n=0; n<8; n++)
        {
        Move(Right);
        }
    wait(1.5);
    printf("Move to 3 times to ->\n\r");
    for(n=0; n<3; n++)
        {
        Move(Right);
        }
    wait(1.5);  

    printf("Move to 2 times to <-\n\r");    
    for(n=0; n<2; n++)
        Move(Left);
    wait(1.5);
        
      
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);                
        
    
    printf("FINE - Sequenza n.6\n\r");
}





void SpegniInterruttore(void)
{ 
    int n=0;

    // Movimento per INTERRUTTORE

    printf("Move to 21 times to -> \n\r");
    // Sposta tutto a SX 
    for(n=0; n<21; n++)
        { 
        MoveI(Right);
        LedRUN = !LedRUN; 
        } 
    LedRUN = 1;
    wait(1.5);  
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<30; n++)
        { 
        MoveI(Left); 
        LedRUN = !LedRUN; 
        } 
    wait(DLY);  
    LedRUN = 1;
}    
      

void FintoSpegniInterruttore(void)
{ 
    int n=0;
    
    printf("Finto spegni Interruttore \n\r");
    printf("Move to 11 times to ->\n\r");
    for(n=0; n<11; n++)
        Move(Right);
    wait(DLY);    
    printf("Move to 10 times to -> \n\r");
    // Sposta tutto a SX 
    for(n=0; n<10; n++)
        MoveI(Right); 
    wait(DLY); 
     
    printf("Move to 5 times to -> \n\r");
    // Sposta tutto a SX 
    for(n=0; n<5; n++)
        MoveI(Right); 
    wait(DLY); 
         
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<30; n++)
        MoveI(Left); 
    wait(DLY);  
} 


void MovDiTest(void)
{ 
int n=0;
int m=0;

    printf("Move to 11 times to ->\n\r");
    for(n=0; n<11; n++)
        Move(Right);
    wait(DLY);
    SpegniInterruttore();
    
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);    
        
    wait(DLY);

    printf("Move to 9 times to ->\n\r");
    for(n=0; n<9; n++)
        Move(Right);    
    wait(0.2);
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);     
    wait(DLY);        
    
    printf("Move to 7 times to ->\n\r");
    for(n=0; n<7; n++)
        Move(Right);  
    wait(DLY);            

    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);      
    wait(DLY);

    for (m=0; m<5; m++)
        {                  
        printf("Move to 9 times to ->\n\r");
        for(n=0; n<8; n++)
            Move(Right);    
    
        wait(0.2);

        printf("Rotate all to <- \n\r");
        // Sposta tutto a SX 
        for(n=0; n<20; n++)
            Move(Left);  
        
        //wait(0.05);    
        }     

    wait(DLY);        
    
    printf("Move to 9 times to ->\n\r");
    for(n=0; n<9; n++)
        Move(Right);  
    wait(DLY); 
    
    printf("Rotate all to <- \n\r");
    // Sposta tutto a SX 
    for(n=0; n<20; n++)
        Move(Left);          
    wait(DLY);  
}


/*

    // Sposta tutto a DX - Move all to RIGHT
    for(n=0; n<200; n++)
        Move(Right);

    wait(DLY);

    // Sposta tutto a SX - Move all to LEFT
    for(n=0; n<200; n++)
        Move(Left);
    
    wait(DLY);

*/

/*
        p=0.4;
        myservo = p;
        printf("P = %.2f \n\r", p);
        wait(0.2);
*/