1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

#define C_SERVO 0 // claw servo port
#define CLAW_UP 155 // claw up position (set_servo_position
#define CLAW_DOWN 50 // same for down.


void raise_claw()
{
    int servo_pos = get_servo_position(C_SERVO);
    int servo_change = 10;
    int servo_increment = 2;
    for (;servo_pos < (CLAW_UP - servo_change);)
      {
        servo_pos = servo_pos + servo_change;
        set_servo_position(servo_pos);
        sleep(0.1);
    }
    for (;servo_pos < CLAW_UP;)
      {
        servo_pos = servo_pos + servo_increment;
        set_servo_position(servo_pos);
        sleep(0.1);
    }
}
    
void drop_claw()
{
    int servo_pos = get_servo_position(C_SERVO);
    int servo_change = 10;
    int servo_increment = 2;
    for (;servo_pos > (CLAW_DOWN + servo_change);)
      {
        servo_pos = servo_pos - servo_change;
        set_servo_position(servo_pos);
        sleep(0.1);
    }
    for (;servo_pos > CLAW_DOWN;)
      {
        servo_pos = servo_pos - servo_increment;
        set_servo_position(servo_pos);
        sleep(0.1);
    }
}