Ajuda braço robótico Arduino

[Túlio]

Olá! Boa tarde!

Estou construindo um braço robótico de Inox e usando o Arduino para controlar todo o movimento, e 3 servos motores de 15kgf movem toda a estrutura (usamos chapas de inox de 2mm, deixando a estrutura bem pesada, e fazendo-se necessário o uso desses servos), mas estou enfrentando 2 problemas.

A rotina de movimento do servo é simples: através de um sensor ultrasônico o arduino detecta a presença de alguma peça e então, move os servos para a posição da mesma, pega a peça, e a transporta para outro lugar, conforme pode ser visto no vídeo abaixo.

Mas 2 problemas estão afetando todo o funcionamento, e mesmo revisando a estrutura, a eletrônica e o código várias vezes, não consegui identificar quais são as causas. O primeiro problema ocorre quando o braço vai pegar alguma peça, nesse momento o servo que movimenta a base começa a ter pequenos "espasmos" fazendo com que a garra não consiga pegar a peça. E o segundo problema é logo depois que o braço coloca a peça em outro lugar, o servo da articulação enfrenta problemas para subir a estrutura, o que é meio estranho já que ele consegue subir a estrutura normalmente logo depois de pegar a peça, os dois problemas podem ser vistos no vídeo abaixo, vocês poderiam me ajudar a solucioná-los?

Vídeo:

Código: [Seleccione]

#include "Ultrasonic.h"
#include <Servo.h>;
// Pino VCC -> 5V
// Pino GND -> GND
#define echoPin 13 // Pino 13 -> Echo
#define trigPin 12 // Pino 12 -> Trig
#define LED 8
Ultrasonic ultrasonic(trigPin,echoPin);
int parado = 0;
int inicio = 0;
Servo gservo;
Servo aservo;
Servo bservo;

void setup()
{
gservo.attach(11); //fio do servo da Garra
aservo.attach(9); //fio do servo da Articulação
bservo.attach(10); //fio do servo da Base
}

void mexendo(void) {
    aservo.write(110);
    delay(15);

    bservo.write(180);
    delay (1500);

    gservo.write(100);
    delay(1500);
     
    aservo.write(100);
    delay(1500);
   
    aservo.write(90);
    delay(1500);
   
    aservo.write(60);
    delay(1500);
   
    aservo.write(30);
    delay(1500);
   
    aservo.write(10);
    delay(1500);
   
    aservo.write(5);
    delay(3000);
   
    gservo.write(80);
    delay(1500);
   
    gservo.write(75);
    delay(1000);
   
    gservo.write(72);
    delay(500);
   
    aservo.write(30);
    delay(1500);
   
    aservo.write(60);
    delay(1500);
   
    aservo.write(90);
    delay(1500);
   
    aservo.write(110);
    delay(1500);
   
    bservo.write(50);
    delay (3000);
   
    bservo.write(1);
    delay (3000);
         
    aservo.write(100);
    delay(1500);
   
    aservo.write(90);
    delay(1500);
   
    aservo.write(60);
    delay(1500);
   
    aservo.write(30);
    delay(1500);
   
    aservo.write(10);
    delay(1500);
       
    aservo.write(5);
    delay(1500);
   
    gservo.write(80);
    delay(1500);
           
    aservo.write(30);
    delay(300);
   
    aservo.write(60);
    delay(300);
   
    aservo.write(90);
    delay(300);
   
    aservo.write(110);
    delay(1500);
   
    gservo.write(100);
    delay(3000);
   
    bservo.write(50);
    delay(1500);
   
}

void quieto(void) {
    aservo.write(110);
    delay(1500);

    bservo.write(50);
    delay (1500);

    gservo.write(100);
    delay(1500);
}

void loop(){
  int i=0;
  long distance=ultrasonic.Ranging(CM);
  while(i<4) {
   distance=(distance + ultrasonic.Ranging(CM))/2;
   i++;
  }
    if(distance<10) {
     digitalWrite(LED,HIGH);
     mexendo();
     inicio=1;
  } else {
    digitalWrite(LED,LOW);
    if(inicio==0) {
      quieto();
      delay(1000);
      parado=1;
      }
   
  }
 
}



[tarquinio]

Boas.
Em relação à parte dos espasmos, a coisa funcionaria muito mais suave se em vez de fazeres os movimentos com os delays pelo meio, fizesses de forma a que ele estivesse sempre a mexer mas o movimento fosse mais lento, e de forma que pudesses controlar a velocidade.
Eu tentaria implementar uma função do tipo:

Código: [Seleccione]

moveTo (start, end, duration)
em que darias a posição inicial e final do servo, e o tempo que queres que o movimento demore. Nesta função, em vez de fazeres um delay, o que fazes é estar sempre a fazer servo.write(), com um valor que vai variando entre os valores "start" e "end" com a passagem do tempo. Esse valor é calculado fácilmente usando a função millis() do arduino.

[Túlio]

Olá tarquinio! Obrigado pela resposta!

Vou tentar implementar essa função, mas você sabe se existe alguma biblioteca pronta compatível com ela? Pois acredito que a biblioteca servo não seja compatível.

[tarquinio]

Não sei se já haverá alguma biblioteca feita que já faca isso... Até é capaz de já haver alguma coisa. De qualquer modo, o que eu sugeri faz-se apenas com algumas linhas de código, e é compativel seja com que library for, não é nada de especial. Apenas faz vários writes por cada movimento, em vez de fazer apenas um.
A idéia é simplesmente conseguir controlar melhor a velocidade das rotações. Da maneira que estás a fazer mandas o serbo para uma posição e ele mexe-se à velocidade máxima o que nestes casos não será a melhor idéia. Daí que em vez de mandares o servo ir logo para a posição final e depois esperar um bocado, podes mandá-lo andar em passinhos pequenos, tendo mais controle sobre a velocidade.

[Túlio]

Entendi.. vou aplicar esta função.. Obrigado!

[Njay]

Vê aqui Túlio, isto vai-te ajudar imenso nessa função:

http://arduino.cc/en/Reference/For

[Túlio]

Obrigado Njay!!