safwan092

servo_x6_arm_code_test

Nov 26th, 2020
669
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <Servo.h>
  2.  
  3. Servo servo1;
  4. Servo servo2;
  5. Servo servo3;
  6. Servo servo4;
  7. Servo servo5;
  8. Servo servo6;
  9. int d1=70;
  10. //3, 5, 6, 9, 10, 11
  11. void setup() {
  12.   servo1.attach(3);
  13.   servo2.attach(5);
  14.   servo3.attach(6);
  15.   servo4.attach(9);
  16.   servo5.attach(10);
  17.   servo6.attach(11);
  18.   Serial.begin(9600);
  19.  
  20.  
  21.  
  22.   /*
  23.     delay(5000);
  24.     jawOpen();
  25.     servo2.write(40);//20
  26.     delay(100);
  27.     servo2.write(30);//20
  28.     delay(100);
  29.     servo2.write(20);//20
  30.     delay(5000);
  31.     jawClose();
  32.     delay(5000);
  33.     servo2.write(50);
  34.     delay(5000);
  35.     servo1.write(90);
  36.     servo2.write(50);
  37.     servo4.write(80);// head goes down 110
  38.   */
  39.   /*  jawOpen();
  40.     delay(500);
  41.     servo1.write(0);
  42.     servo2.write(0);//20
  43.     servo3.write(60);//80
  44.     servo4.write(30);
  45.     servo5.write(80);
  46.     jawOpen();
  47.     delay(2000);
  48.     jawClose();
  49.     delay(1000);
  50.     servo1.write(90);
  51.     servo2.write(20);//20
  52.     servo3.write(60);//80
  53.     servo4.write(30);
  54.     servo5.write(80);
  55.     jawClose();
  56.     delay(1000);
  57.   */
  58.   /*
  59.     servo1.write(90);
  60.     servo2.write(0);//20
  61.     servo3.write(60);//80
  62.     servo4.write(30);
  63.     servo5.write(80);
  64.     servo6.write(5);
  65.   */
  66.  
  67. }
  68.  
  69. void loop() {
  70.   // begin Pos 1
  71.   jawOpen();
  72.   servo1.write(0);
  73.   servo2.write(90);
  74.   servo3.write(90);
  75.   servo4.write(0);
  76.   servo5.write(90);
  77.   delay(5000);
  78.   // go to A
  79.  
  80.   servo1.write(0);//END
  81.   servo2.write(95);
  82.   delay(d1);
  83.   servo2.write(100);
  84.   delay(d1);
  85.   servo2.write(105);
  86.   delay(d1);
  87.   servo2.write(110);
  88.   delay(d1);
  89.   servo2.write(115);
  90.   delay(d1);
  91.   servo2.write(120);
  92.   delay(d1);
  93.   servo2.write(125);
  94.   delay(d1);
  95.   servo2.write(130);
  96.   delay(d1);
  97.   servo6.write(0); //  <-------
  98.   servo2.write(135);
  99.   delay(d1);
  100.   servo2.write(140);
  101.   delay(d1);
  102.   servo2.write(145);
  103.   delay(d1);
  104.   servo2.write(150);
  105.   delay(d1);
  106.   servo2.write(155);
  107.   delay(d1);
  108.   servo2.write(160);
  109.   delay(d1);
  110.   servo2.write(165);
  111.   delay(d1);
  112.   servo2.write(170);//END
  113.   servo3.write(80);//END
  114.   servo4.write(0);// END
  115.  
  116.   // servo5.write(90);
  117.  
  118.   // hold bottle
  119.   delay(1000);
  120.   jawClose();
  121.   delay(2000);
  122.   // go up while holding
  123.   /*servo2.write(160);
  124.     delay(100);
  125.     servo2.write(150);
  126.     delay(100);
  127.     servo2.write(140);
  128.     delay(100);*/
  129.   servo2.write(165);
  130.   delay(d1);
  131.   servo2.write(160);
  132.   delay(d1);
  133.   servo2.write(155);
  134.   delay(d1);
  135.   servo2.write(150);
  136.   delay(d1);
  137.   servo2.write(145);
  138.   delay(d1);
  139.   servo2.write(140);
  140.   delay(d1);
  141.   servo2.write(135);
  142.   delay(d1);
  143.   servo2.write(130);
  144.   delay(d1);
  145.   servo2.write(125);
  146.   delay(d1);
  147.   servo2.write(120);
  148.   delay(d1);
  149.   servo2.write(115);
  150.   delay(d1);
  151.   servo2.write(110);
  152.   delay(d1);
  153.   servo2.write(105);
  154.   delay(d1);
  155.   servo2.write(100);//END
  156.   delay(100);
  157.   // go to B and hold
  158.   delay(1000);
  159.   servo1.write(90);
  160.   delay(1000);
  161.   servo2.write(100);
  162.   delay(d1);
  163.   servo2.write(105);
  164.   delay(d1);
  165.   servo2.write(110);
  166.   delay(d1);
  167.   servo2.write(115);
  168.   delay(d1);
  169.   servo2.write(120);
  170.   delay(d1);
  171.   servo2.write(125);
  172.   delay(d1);
  173.   servo2.write(130);
  174.   delay(d1);
  175.   servo2.write(135);
  176.   delay(d1);
  177.   servo2.write(140);
  178.   delay(d1);
  179.   servo2.write(145);
  180.   delay(d1);
  181.   servo2.write(150);
  182.   delay(d1);
  183.   servo2.write(155);
  184.   delay(d1);
  185.   servo2.write(160);
  186.   delay(d1);
  187.   servo2.write(165);
  188.   delay(d1);
  189.   servo2.write(170);//end
  190.   delay(d1);
  191.   // release bottle
  192.   delay(1000);
  193.   jawOpen();
  194.   // go to Pos 1
  195.   delay(1000);
  196.  
  197.   servo2.write(90);
  198.   servo3.write(90);
  199.   servo4.write(0);
  200.   servo5.write(90);
  201.   delay(5000);
  202.   servo1.write(0);
  203. }
  204.  
  205. void jawOpen() {
  206.   servo6.write(5);
  207. }
  208. void jawClose() {
  209.   servo6.write(80);
  210. }
  211.  
  212.  
  213. /*
  214.  
  215.   // begin Pos 1
  216.   jawOpen();
  217.   servo1.write(0);
  218.   servo2.write(90);
  219.   servo3.write(90);
  220.   servo4.write(0);
  221.   servo5.write(90);
  222.   delay(5000);
  223.   // go to A
  224.  
  225.   servo1.write(0);//END
  226.   servo2.write(170);//END
  227.   servo3.write(80);//END
  228.   servo4.write(0);// END
  229.  
  230.   // servo5.write(90);
  231.  
  232.   // hold bottle
  233.   delay(1000);
  234.   jawClose();
  235.   delay(2000);
  236.   // go up while holding
  237.  
  238.   servo2.write(100);
  239.   delay(100);
  240.   // go to B and hold
  241.   delay(1000);
  242.   servo1.write(90);
  243.   delay(1000);
  244.   servo2.write(140);
  245.   delay(100);
  246.   servo2.write(150);
  247.   delay(100);
  248.   servo2.write(160);
  249.   delay(100);
  250.   servo2.write(170);
  251.   delay(100);
  252.   // release bottle
  253.   delay(1000);
  254.   jawOpen();
  255.   // go to Pos 1
  256.   delay(1000);
  257.  
  258.   servo2.write(90);
  259.   servo3.write(90);
  260.   servo4.write(0);
  261.   servo5.write(90);
  262.   delay(5000);
  263.   servo1.write(0);
  264. */
RAW Paste Data