// Move Servos to Start Position //int Zero[6]={75,140,117,135,75,110}; void servosToZero(){ for(int i=0; i<6; i++){ Zero[i] = map(Zero[i], 0, 180, SMIN, SMAX); pwm.setPWM(i+1, 0, Zero[i]); } delay(500); } void openP(){ for(int i=Zero[5]; i>=openPincers; i--){ pwm.setPWM(6, 0, i); delay(fast); } } void closeP(){ for(int i=openPincers; i<=closePincers; i++){ pwm.setPWM(6, 0, i); delay(fast); } } void tOut(){ int c=0; startZERO(); //Move till over the piece do{ pwm.setPWM(1, 0, OUTs[0]); delay(fast); if(OUTs[0]>=OUTu[0]){ OUTs[0]--; } pwm.setPWM(2, 0, OUTs[1]); delay(fast); if(OUTs[1]>=OUTu[1]){ OUTs[1]--; } if (c>20){ pwm.setPWM(3, 0, OUTs[2]); delay(fast); if(OUTs[2]>=OUTu[2]){ OUTs[2]--; } } pwm.setPWM(4, 0, OUTs[3]); delay(fast); if(OUTs[3]>=OUTu[3]){ OUTs[3]--; } pwm.setPWM(5, 0, OUTs[4]); delay(fast); if(OUTs[4]<=OUTu[4]){ OUTs[4]++; } c++; }while((OUTs[0]>=OUTu[0])||(OUTs[1]>=OUTu[1])||(OUTs[2]>=OUTu[2])||(OUTs[3]>=OUTu[3])||(OUTs[4]<=OUTu[4])); openP(); startUP(); //Move to Zero c=0; do{ if(c>80){ pwm.setPWM(1, 0, OUTs[0]); delay(fast); if(OUTs[0]<=Zero[0]){ OUTs[0]++; } } pwm.setPWM(2, 0, OUTs[1]); delay(fast); if(OUTs[1]<=Zero[1]){ OUTs[1]++; } if(c>50){ pwm.setPWM(3, 0, OUTs[2]); delay(fast); if(OUTs[2]<=Zero[2]){ OUTs[2]++; } } if(c>30){ pwm.setPWM(4, 0, OUTs[3]); delay(fast); if(OUTs[3]<=Zero[3]){ OUTs[3]++; } } pwm.setPWM(5, 0, OUTs[4]); delay(fast); if(OUTs[4]>=Zero[4]){ OUTs[4]--; } c++; }while((OUTs[0]<=Zero[0])||(OUTs[1]<=Zero[1])||(OUTs[2]<=Zero[2])||(OUTs[3]<=Zero[3])||(OUTs[4]>=Zero[4])); }