safwan092

3143- Arm Project

Dec 6th, 2020
687
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.  
  11. //3, 5, 6, 9, 10, 11
  12.  
  13. void setup() {
  14.   servo1.attach(3);
  15.   servo2.attach(5);
  16.   servo3.attach(6);
  17.   servo4.attach(9);
  18.   servo5.attach(10);
  19.   servo6.attach(11);
  20.   Serial.begin(9600);
  21. }
  22.  
  23. void loop() {
  24.  
  25.  
  26.   // begin Pos 1
  27.   /////////////////////////////////////////
  28.   jawOpen();
  29.   servo1.write(0);
  30.   servo2.write(90);
  31.   servo3.write(90);
  32.   servo4.write(0);
  33.   servo5.write(90);
  34.   delay(5000);
  35.   /////////////////////////////////////////
  36.  
  37.  
  38.   // go to A
  39.   /////////////////////////////////////////
  40.   servo1.write(0);//END
  41.   servo2.write(95);
  42.   delay(d1);
  43.   servo2.write(100);
  44.   delay(d1);
  45.   servo2.write(105);
  46.   delay(d1);
  47.   servo2.write(110);
  48.   delay(d1);
  49.   servo2.write(115);
  50.   delay(d1);
  51.   servo2.write(120);
  52.   delay(d1);
  53.   servo2.write(125);
  54.   delay(d1);
  55.   servo2.write(130);
  56.   delay(d1);
  57.   servo6.write(0); //  <-------
  58.   servo2.write(135);
  59.   delay(d1);
  60.   servo2.write(140);
  61.   delay(d1);
  62.   servo2.write(145);
  63.   delay(d1);
  64.   servo2.write(150);
  65.   delay(d1);
  66.   servo2.write(155);
  67.   delay(d1);
  68.   servo2.write(160);
  69.   delay(d1);
  70.   servo2.write(165);
  71.   delay(d1);
  72.   servo2.write(170);//END
  73.   servo3.write(80);//END
  74.   servo4.write(0);// END
  75.   /////////////////////////////////////////
  76.  
  77.  
  78.   // hold bottle
  79.   /////////////////////////////////////////
  80.   delay(1000);
  81.   jawClose();
  82.   delay(2000);
  83.   /////////////////////////////////////////
  84.  
  85.  
  86.   // go up while holding
  87.   /////////////////////////////////////////
  88.   servo2.write(165);
  89.   delay(d1);
  90.   servo2.write(160);
  91.   delay(d1);
  92.   servo2.write(155);
  93.   delay(d1);
  94.   servo2.write(150);
  95.   delay(d1);
  96.   servo2.write(145);
  97.   delay(d1);
  98.   servo2.write(140);
  99.   delay(d1);
  100.   servo2.write(135);
  101.   delay(d1);
  102.   servo2.write(130);
  103.   delay(d1);
  104.   servo2.write(125);
  105.   delay(d1);
  106.   servo2.write(120);
  107.   delay(d1);
  108.   servo2.write(115);
  109.   delay(d1);
  110.   servo2.write(110);
  111.   delay(d1);
  112.   servo2.write(105);
  113.   delay(d1);
  114.   servo2.write(100);//END
  115.   delay(100);
  116.   /////////////////////////////////////////
  117.  
  118.  
  119.   // go to B and hold
  120.   /////////////////////////////////////////
  121.   delay(1000);
  122.   servo1.write(90);
  123.   delay(1000);
  124.   servo2.write(100);
  125.   delay(d1);
  126.   servo2.write(105);
  127.   delay(d1);
  128.   servo2.write(110);
  129.   delay(d1);
  130.   servo2.write(115);
  131.   delay(d1);
  132.   servo2.write(120);
  133.   delay(d1);
  134.   servo2.write(125);
  135.   delay(d1);
  136.   servo2.write(130);
  137.   delay(d1);
  138.   servo2.write(135);
  139.   delay(d1);
  140.   servo2.write(140);
  141.   delay(d1);
  142.   servo2.write(145);
  143.   delay(d1);
  144.   servo2.write(150);
  145.   delay(d1);
  146.   servo2.write(155);
  147.   delay(d1);
  148.   servo2.write(160);
  149.   delay(d1);
  150.   servo2.write(165);
  151.   delay(d1);
  152.   servo2.write(170);//end
  153.   delay(d1);
  154.   /////////////////////////////////////////
  155.  
  156.  
  157.   // release bottle
  158.   /////////////////////////////////////////
  159.   delay(1000);
  160.   jawOpen();
  161.   delay(1000);
  162.   /////////////////////////////////////////
  163.  
  164.  
  165.   // go to Pos 1
  166.   /////////////////////////////////////////
  167.   servo2.write(90);
  168.   servo3.write(90);
  169.   servo4.write(0);
  170.   servo5.write(90);
  171.   delay(5000);
  172.   servo1.write(0);
  173.   /////////////////////////////////////////
  174. }
  175.  
  176. void jawOpen() {
  177.   servo6.write(5);
  178. }
  179. void jawClose() {
  180.   servo6.write(80);
  181. }
  182.  
RAW Paste Data