Controle da garra do akira com suavização

#include <Servo.h>

Servo garra_d;
Servo garra_e;
Servo elevador;

int x =0;
float pos_d = 90;
int pos_e= 115;
int controle = 50;
int z=0;
int pos_elev=130;

void setup()
{
garra_d.attach(6);
garra_e.attach (7);
elevador.attach (8);
garra_d.write (pos_d);
garra_e.write (pos_e);
elevador.write (pos_elev);
Serial.begin (9600);
}

void loop()
{
  //abaixa a garra
for (z=0; z<=40;z++){
  elevador.write (pos_elev);
  pos_elev = pos_elev-1;
  Serial.println (pos_elev);
  delay (25);
}
delay (5000);
//subir a garra
for (z=40; z>=0;z--){
  elevador.write (pos_elev);
  pos_elev = pos_elev+1;
  Serial.println (pos_elev);
  delay (25);
}
delay (5000);
//abrir a garra
for (x=0; x<=controle;x++){
  Serial.print (pos_d);
  Serial.print ("\t");
  Serial.println (pos_e);
  garra_d.write (pos_d);
  garra_e.write (pos_e);
  delay (25);
  pos_d = pos_d+1.7;
  pos_e = pos_e-1;
}
delay (5000);
//fechar a garra
for (x=controle; x>=0;x--){
  Serial.print (pos_d);
  Serial.print ("\t");
  Serial.println (pos_e);
  garra_d.write (pos_d);
  garra_e.write (pos_e);
  delay (25);
  pos_d = pos_d-1.7;
  pos_e = pos_e+1;
}
delay (5000);
}//fim do loop

Depois de executar este programa, tornar cada for uma função void para controlar a garra:
void abrir_garra();
void fechar_garra();
void elevar_garra();
void abaixar_garra();

Acrescentar a função do servo e testar com objeto a distância de ser pego na garra.