BY: www.emcu.eu Dates: 08-12-2017 Magic Box - 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 do you also see the commands
Fork of ScatolaMagica by
main.cpp
- Committer:
- emcu
- Date:
- 2017-12-10
- Revision:
- 1:e1acd469838f
- Parent:
- 0:4192ed0f6a57
File content as of revision 1:e1acd469838f:
/* 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. --------------------------------------------------------------------------- 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); */