testServos.ino 1.67 KB
Newer Older
Julián Pérez's avatar
Julián Pérez committed
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
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74

#include <Servo.h>

Servo servo1, servo2, servo3, servo4;
int pos1, pos2, pos3, pos4 = 0;
int min1 = 600;
int max1 = 2400;

int min2 = 600;
int max2 = 1900;

int min3 = 800;
int max3 = 2300;

int min4 = 1500;
int max4 = 2400;

void setup()
{
  Serial.begin(9600);
  servo1.attach(11, min1, max1); //600,2500 BASE
  servo2.attach(10, min2, max2); //600,1900 LEFT
  servo3.attach(9, min3, max3);  //1100,2300 RIGHT
  servo4.attach(6, min4, max4);  //1500,2400 PINZA
}

void loop()
{

  //TEST BASE----------------------------
  //    for (pos1 = max1; pos1 >= min1; pos1 -= 10) {
  //      servo1.writeMicroseconds(pos1);
  //      delay(15);
  //    }
  //    for (pos1 = min1; pos1 <= max1; pos1 += 10) {
  //      servo1.writeMicroseconds(pos1);
  //      delay(15);
  //    }


  //TEST RIGHT----------------------------
  //   servo3.writeMicroseconds(1500);
  //    for (pos3 = min3; pos3 <= max3; pos3 += 10) {
  //      servo3.writeMicroseconds(pos3);
  //      delay(15);
  //    }
  //    for (pos3 = max3; pos3 >= min3; pos3 -= 10) {
  //      servo3.write(pos3);
  //      delay(15);
  //    }


  //TEST LEFT----------------------------
  //servo2.writeMicroseconds(1500);
//  for (pos2 = min2; pos2 <= max2; pos2 += 10) {
//    servo2.writeMicroseconds(pos2);
//    delay(15);
//  }
//  for (pos2 = max2; pos2 >= min2; pos2 -= 10) {
//    servo2.write(pos2);
//    delay(15);
//  }

  //TEST PINZA---------------------------
//  servo4.writeMicroseconds(1500);
      for (pos4 = max4; pos4 >= min4; pos4 -= 10) {
        servo4.writeMicroseconds(pos4);
        delay(15);
      }
      for (pos4 = min4; pos4 <= max4; pos4 += 10) {
        servo4.writeMicroseconds(pos4);
        delay(15);
      }
}