Robotic Claw

1// code by Aryaman Kumar
2
3#include <Servo.h>
4
5Servo servoClaw;    // servo motor to open/close the claw 
6Servo servoHeight;  // servo motor  to raise/lower the claw 
7Servo servoTurn;    // servo motor to turn the claw  
8
9
10int clawAngle = 150;   // servoClaw's angle initialized 
11int heightAngle  = 160; // servoHeight's angle initialized 
12int turnAngle = 0;     // servoTurn's  angle initialized 
13
14// refer the definition of function "move()" below to  understand the code in "setup()"
15
16void setup() {
17  
18   Serial.begin  (9600);
19   // pin mapping
20   servoClaw.attach(3);  
21   servoHeight.attach(4);
22   servoTurn.attach(5);
23
24   // Setting all servo motors to initial angles.  Claw is now right above cup.
25   
26   heightAngle = move(servoHeight,heightAngle,heightAngle);
27   turnAngle = move(servoTurn,turnAngle,turnAngle);
28   clawAngle = move(servoClaw,clawAngle,clawAngle);
29  
30   // moving towards first object and picking it up
31
32   turnAngle = move(servoTurn,turnAngle,90);
33   delay(1000);
34   heightAngle = move(servoHeight,heightAngle,90);
35   delay(1000);
36   clawAngle = move(servoClaw,clawAngle,10);
37   delay(1000);
38   heightAngle  = move(servoHeight,heightAngle,160);
39   delay(1000);
40    
41   // moving  back to cup and dropping the object
42     
43   turnAngle = move(servoTurn,turnAngle,0);
44   delay(1000);
45   heightAngle = move(servoHeight,heightAngle,140);
46   delay(1000);
47   clawAngle = move(servoClaw,clawAngle,150);
48   delay(1000);
49   heightAngle  = move(servoHeight,heightAngle,160);
50   delay(1000);
51
52   // moving towards  second object and picking it up
53    
54   turnAngle = move(servoTurn,turnAngle,160);
55   delay(1000);
56   heightAngle = move(servoHeight,heightAngle,90);
57   delay(1000);
58   clawAngle = move(servoClaw,clawAngle,10);
59   delay(1000);
60   heightAngle  = move(servoHeight,heightAngle,160);
61   delay(1000);
62
63   // moving back  to cup and dropping the object
64       
65   turnAngle = move(servoTurn,turnAngle,0);
66   delay(1000);
67   heightAngle = move(servoHeight,heightAngle,140)
68   delay(1000);
69   clawAngle = move(servoClaw,clawAngle,150);
70   delay(1000);
71   heightAngle  = move(servoHeight,heightAngle,160);
72   delay(1000);
73     
74}
75
76//  No code in loop because it is required to operate only once. 
77void loop() {
78   
79}
80
81// servoTemp is the servo that is to be moved. 
82// currentAngle  is the servo motor's current angle.
83// desiredAngle is the angle it is required  to move to.
84
85// this function moves the servo from its current angle to the  desired angle. Returns new angle. 
86
87
88int move(Servo servoTemp,int currentAngle,  int desiredAngle)
89{
90  if(currentAngle<desiredAngle)
91  {
92     while(currentAngle<=desiredAngle)
93    {
94      servoTemp.write(currentAngle);
95      currentAngle++;
96      delay(10);
97    }  
98  }
99  else
100  {
101     while(currentAngle>=desiredAngle)
102    {
103      servoTemp.write(currentAngle);
104      currentAngle--;
105      delay(10);
106    }  
107  }
108
109   return currentAngle; 
110
111}
112