RuiViana

N_Braco

Nov 5th, 2016
156
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*
  2.    Ligar o coletor do TCRT5000 no port 6 e o emissor no Gnd.
  3.    Ao receber luz fica em LOW e ao não receber fica em HIGH
  4. */
  5. #define Sensor 6                      // Port para entrada do sensor TCRT5000
  6. byte Flag = 0;                        // Flag para saber onde esta posicionado o braco
  7. #include <VarSpeedServo.h>
  8. VarSpeedServo garra;
  9. VarSpeedServo cotov;
  10. VarSpeedServo ombro;
  11. VarSpeedServo base;
  12. //------------------------------
  13. void setup()
  14. {
  15.   pinMode(Sensor, INPUT_PULLUP);      // Define port Sensor como entrada e com Pullup resitor ligado
  16.   garra.attach(2);
  17.   ombro.attach(3);
  18.   cotov.attach(4);
  19.   base.attach(5);
  20.   garra.write(135, 5, false);  // Posicao inicial
  21.   delay(100);
  22.   base.write(110, 5, false);
  23.   delay(100);
  24.   ombro.write(70, 5, false);
  25.   delay(100);
  26.   cotov.write(90, 5, false);
  27.   delay(100);
  28.   delay(2000);
  29.   garra.write(90, 200, true);   // abre
  30.   garra.write(135, 200, true);  // fecha
  31.   garra.write(90, 200, true);   // abre
  32.   garra.write(135, 200, true);  // fecha
  33.   delay(1000);
  34. }
  35. //-----------------------------------
  36. void loop()                           //rotina de trabalho
  37. {
  38.   if (digitalRead(Sensor) == LOW)
  39.   {
  40.     if (Flag == 0)                    // Se o braço esta atras
  41.     {
  42.               // Coloque aqui a rotina para mover o braço pra frente
  43.       Flag = 1;                       // Faz uma vez
  44.     }
  45.     else
  46.     if (Flag == 1)                    // Se o braço esta na frente
  47.     {
  48.               // Coloque aqui a rotina para mover o braço pra tras
  49.       Flag = 0;                       // Faz uma vez
  50.     }
  51.    
  52.   }
  53.   /*
  54.     base.write(180, 60, true);    // esquerda
  55.     delay(100);
  56.     ombro.write(160, 40, false);  // frente
  57.     delay(10);
  58.     cotov.write(50, 20, true);    // desce
  59.     delay(100);
  60.     cotov.write(110, 40, false);  // frente
  61.     delay(10);
  62.     ombro.write(180, 40, false);  // desce
  63.     delay(10);
  64.     garra.write(90, 50, true);    // abre
  65.     delay(500);
  66.     garra.write(135, 20, true);   // fecha
  67.     delay(100);
  68.     ombro.write(150, 20, true);   // sobe
  69.     cotov.write(80, 20, true);    // tras
  70.     delay(100);
  71.     ombro.write(90, 20, false);   // sobe
  72.     delay(10);
  73.     cotov.write(180, 20, true);   // tras
  74.     delay(100);
  75.     base.write(20, 10, true);     // direta
  76.     delay(100);
  77.     cotov.write(80, 20, true);    // desce
  78.     delay(10);
  79.     ombro.write(150, 20, false);  // frente
  80.     delay(10);
  81.     cotov.write(50, 20, true);    // desce
  82.     delay(10);
  83.     cotov.write(110, 20, false);  // desce
  84.     delay(10);
  85.     ombro.write(180, 20, true);   // frente
  86.     delay(100);
  87.     garra.write(90, 20, true);    // abre
  88.     delay(100);
  89.     ombro.write(90, 80, false);   // sobe
  90.     delay(100);
  91.     cotov.write(80, 100, false);  // tras
  92.     delay(100);
  93.     garra.write(135, 50, false);  // fecha
  94.     delay(100);*/
  95. }/*
  96.    Ligar o coletor do TCRT5000 no port 6 e o emissor no Gnd.
  97.    Ao receber luz fica em LOW e ao não receber fica em HIGH
  98. */
  99. #define Sensor 6                      // Port para entrada do sensor TCRT5000
  100. byte Flag = 0;                        // Flag para saber onde esta posicionado o braco
  101. #include <VarSpeedServo.h>
  102. VarSpeedServo garra;
  103. VarSpeedServo cotov;
  104. VarSpeedServo ombro;
  105. VarSpeedServo base;
  106. //------------------------------
  107. void setup()
  108. {
  109.   pinMode(Sensor, INPUT_PULLUP);      // Define port Sensor como entrada e com Pullup resitor ligado
  110.   garra.attach(2);
  111.   ombro.attach(3);
  112.   cotov.attach(4);
  113.   base.attach(5);
  114.   garra.write(135, 5, false);  // Posicao inicial
  115.   delay(100);
  116.   base.write(110, 5, false);
  117.   delay(100);
  118.   ombro.write(70, 5, false);
  119.   delay(100);
  120.   cotov.write(90, 5, false);
  121.   delay(100);
  122.   delay(2000);
  123.   garra.write(90, 200, true);   // abre
  124.   garra.write(135, 200, true);  // fecha
  125.   garra.write(90, 200, true);   // abre
  126.   garra.write(135, 200, true);  // fecha
  127.   delay(1000);
  128. }
  129. //-----------------------------------
  130. void loop()                           //rotina de trabalho
  131. {
  132.   if (digitalRead(Sensor) == LOW)
  133.   {
  134.     if (Flag == 0)                    // Se o braço esta atras
  135.     {
  136.               // Coloque aqui a rotina para mover o braço pra frente
  137.       Flag = 1;                       // Faz uma vez
  138.     }
  139.     else
  140.     if (Flag == 1)                    // Se o braço esta na frente
  141.     {
  142.               // Coloque aqui a rotina para mover o braço pra tras
  143.       Flag = 0;                       // Faz uma vez
  144.     }
  145.    
  146.   }
  147.   /*
  148.     base.write(180, 60, true);    // esquerda
  149.     delay(100);
  150.     ombro.write(160, 40, false);  // frente
  151.     delay(10);
  152.     cotov.write(50, 20, true);    // desce
  153.     delay(100);
  154.     cotov.write(110, 40, false);  // frente
  155.     delay(10);
  156.     ombro.write(180, 40, false);  // desce
  157.     delay(10);
  158.     garra.write(90, 50, true);    // abre
  159.     delay(500);
  160.     garra.write(135, 20, true);   // fecha
  161.     delay(100);
  162.     ombro.write(150, 20, true);   // sobe
  163.     cotov.write(80, 20, true);    // tras
  164.     delay(100);
  165.     ombro.write(90, 20, false);   // sobe
  166.     delay(10);
  167.     cotov.write(180, 20, true);   // tras
  168.     delay(100);
  169.     base.write(20, 10, true);     // direta
  170.     delay(100);
  171.     cotov.write(80, 20, true);    // desce
  172.     delay(10);
  173.     ombro.write(150, 20, false);  // frente
  174.     delay(10);
  175.     cotov.write(50, 20, true);    // desce
  176.     delay(10);
  177.     cotov.write(110, 20, false);  // desce
  178.     delay(10);
  179.     ombro.write(180, 20, true);   // frente
  180.     delay(100);
  181.     garra.write(90, 20, true);    // abre
  182.     delay(100);
  183.     ombro.write(90, 80, false);   // sobe
  184.     delay(100);
  185.     cotov.write(80, 100, false);  // tras
  186.     delay(100);
  187.     garra.write(135, 50, false);  // fecha
  188.     delay(100);*/
  189. }
RAW Paste Data