#include <Servo.h>
#define trigPin 2
#define echoPin 3
#define closePin A4
const int SERVOS = 4;
int PIN[SERVOS],value[SERVOS],idle[SERVOS],currentAngle[SERVOS],MIN[SERVOS],MAX[SERVOS],INITANGLE[SERVOS];
Servo myservo[SERVOS];
int afrom[] = {90, 35, 135}, ato[] = {20, 140, 110}, amiddle[] = {20, 35, 135}, afinal[] = {179,75,95}, aafterfinal[] = {179,35,135};
int anaPins[]={A0,A1,A2,A3};
int DELAYTIME = 200;
boolean counterClock = false;
long duration, distance;
void setup() {
Serial.begin(9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
// 底盤, 臂長, 高低, 爪子
PIN[0] = 4; PIN[1] = 5; PIN[2] = 6; PIN[3] = 7;
MIN[0] = 1; MIN[1] = 20; MIN[2] = 20; MIN[3] = 5;
MAX[0] =179; MAX[1] =170; MAX[2] =170; MAX[3] = 40;
INITANGLE[0] = 90; INITANGLE[1] = 90; INITANGLE[2] = 90; INITANGLE[3] = 40;
for(int i = 0; i < SERVOS; i++){
pinMode(anaPins[i], INPUT);
myservo[i].attach(PIN[i]);
myservo[i].write(INITANGLE[i]);
value[i] = 0;
idle[i] = 0;
}
}
void loop(){
doCruise();
//for(int i = 0; i < 2; i++){
// doServo(i);
//}
}
void doServo(int servoId){
value[servoId] = analogRead(anaPins[servoId]);
currentAngle[servoId] = myservo[servoId].read();
if(value[servoId] < 400) {
idle[servoId] = 0;
if(currentAngle[servoId] > MIN[servoId]){
--currentAngle[servoId];
}
if(!myservo[servoId].attached()){
myservo[servoId].attach(PIN[servoId]);
}
myservo[servoId].write(currentAngle[servoId]);
}else if (value[servoId] > 600) {
idle[servoId] = 0;
if(currentAngle[servoId] < MAX[servoId]){
++currentAngle[servoId];
}
if(!myservo[servoId].attached()){
myservo[servoId].attach(PIN[servoId]);
}
myservo[servoId].write(currentAngle[servoId]);
}else{
++idle[servoId];
}
if(idle[servoId] > 100){
myservo[servoId].detach();
idle[servoId] = 0;
}
}
void doJob(){
int readDistance = analogRead(closePin);
int servoId = 1;
while(readDistance>800){
currentAngle[servoId]++;
if(!myservo[servoId].attached()){
myservo[servoId].attach(PIN[servoId]);
}
myservo[servoId].write(currentAngle[servoId]);
delay(20);
readDistance = analogRead(closePin);
}
Serial.println("Get it!!");
}
void doSearch(){
SonarSensor(trigPin,echoPin);
if(distance<18){
doJob();
}
}
void SonarSensor(int trigP,int echoP){
digitalWrite(trigP, LOW);
delayMicroseconds(2);
digitalWrite(trigP, HIGH);
delayMicroseconds(10);
digitalWrite(trigP, LOW);
duration = pulseIn(echoP, HIGH);
distance = (duration/2) / 29.1;
}
void doCruise(){
int thisServoId = 0;
currentAngle[thisServoId] = myservo[thisServoId].read();
if(currentAngle[thisServoId] > 45){
counterClock = true;
}
if(counterClock){
while(currentAngle[thisServoId]>MIN[thisServoId]){
--currentAngle[thisServoId];
if(!myservo[thisServoId].attached()){
myservo[thisServoId].attach(PIN[thisServoId]);
}
myservo[thisServoId].write(currentAngle[thisServoId]);
currentAngle[thisServoId] = myservo[thisServoId].read();
doSearch();
}
}else{
while(currentAngle[thisServoId]<MAX[thisServoId]){
++currentAngle[thisServoId];
if(!myservo[thisServoId].attached()){
myservo[thisServoId].attach(PIN[thisServoId]);
}
myservo[thisServoId].write(currentAngle[thisServoId]);
currentAngle[thisServoId] = myservo[thisServoId].read();
doSearch();
}
}
}
void armfromto(int *arrf, int *arrt){
int lp[3], seg = 3, sign;
for(int i = 0; i < 3; i++){
lp[i] = abs((arrt[i] - arrf[i])/seg);
}
//delay(DALAYTIME);
for(int i = 0; i < 3; i++){
sign = arrt[i] - arrf[i] > 0 ? 1 : -1;
for(int j = 0; j < lp[i]; j++){
myservo[i].write(arrf[i]+j*seg*sign);
delay(20);
}
delay(DELAYTIME);
}
}
void closeclaw(boolean op){
if(op){
myservo[3].write(1);
}else{
myservo[3].write(30);
}
}