Diff: main.cpp

Revision:

0:4192ed0f6a57

Child:

1:e1acd469838f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Dec 10 02:26:18 2017 +0000
@@ -0,0 +1,527 @@
+/*
+
+ 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);
+*/