#include <Servo.h>
#define closePin A4
const int SERVOS = 4;
Servo myservo[SERVOS];
long duration, distance;
int PIN[SERVOS], value[SERVOS], idle[SERVOS], currentAngle[SERVOS], MIN[SERVOS], MAX[SERVOS], INITANGLE[SERVOS];
int afrom[] = {80, 80}, ato[] = {100, 60};
int DELAYTIME = 200;
void setup() {
Serial.begin(9600);
pinMode(closePin, INPUT);
PIN[0] = 5; MIN[0] = 25; MAX[0] = 145; INITANGLE[0] = 90;
PIN[1] = 6; MIN[1] = 10; MAX[1] = 40; INITANGLE[1] = 40;
PIN[2] = 7; MIN[2] = 90; MAX[2] = 179; INITANGLE[2] = 90;
PIN[3] = 8; MIN[3] = 0; MAX[3] = 179; INITANGLE[3] = 90;
for (int i = 0; i < SERVOS; i++) {
//myservo[i].attach(PIN[i]);
//myservo[i].write(INITANGLE[i]);
value[i] = 0;
idle[i] = 0;
}
}
void loop() {
armfromto(afrom, ato);
delay(5000);
armfromto(ato, afrom);
delay(5000);
}
void armfromto(int *arrf, int *arrt) {
int lD[2], tmpAngle[2];
int maxD;
tmpAngle[0] = myservo[2].read();
tmpAngle[1] = myservo[3].read();
maxD = abs(arrt[0] - arrf[0]);
if(abs(arrt[1] - arrf[1])>maxD)
maxD = abs(arrt[1] - arrf[1]);
if(arrt[0] - arrf[0]>0)
lD[0] = 1;
else if(arrt[0] - arrf[0]<0)
lD[0] = -1;
else
lD[0] = 0;
if(arrt[1] - arrf[1]>0)
lD[1] = 1;
else if(arrt[1] - arrf[1]<0)
lD[1] = -1;
else
lD[1] = 0;
Serial.print("arrf[0]=");
Serial.print(arrf[0]);
Serial.print(" arrt[0]=");
Serial.print(arrt[0]);
Serial.print(" arrf[1]=");
Serial.print(arrf[1]);
Serial.print(" arrt[1]=");
Serial.print(arrt[1]);
Serial.print(" lD[0]=");
Serial.print(lD[0]);
Serial.print(" lD[1]=");
Serial.println(lD[1]);
for (int i = 0; i < maxD; i++) {
tmpAngle[0] = myservo[2].read();
tmpAngle[1] = myservo[3].read();
if(tmpAngle[0]!=arrt[0] & tmpAngle[0]*lD[0]>arrt[0]*lD[0]){
tmpAngle[0] += lD[0];
if (!myservo[2].attached()){
myservo[2].attach(PIN[2]);
}
Serial.print("tmpAngle[0]=");
Serial.println(tmpAngle[0]);
myservo[2].write(tmpAngle[0]);
}else{
myservo[2].detach();
}
if(tmpAngle[1]!=arrt[1] & tmpAngle[1]*lD[1]<arrt[1]*lD[1]){
tmpAngle[1] += lD[1];
if (!myservo[3].attached()){
myservo[3].attach(PIN[3]);
}
myservo[3].write(tmpAngle[1]);
}else{
myservo[3].detach();
}
delay(20);
}
}