Robô manipulador 6 graus com servos de múltiplos movimentos simultâneos

O sketch a seguir permite o controle de um robô do tipo manipulador com seis servos. Um "shield" foi construído para  possibilitar a simplificação do circuito. A montagem pode ser visualizada nas imagens seguintes:

O sketch possibilita o controle dos seis servos simultaneamente, resultando em movimentos mais suaves. Um buser conectado ao pino D9, gera um som a cada movimento.

// Programa de controle do manipulador de 6 graus

#include <Servo.h>

Servo myservo0;

Servo myservo1;

Servo myservo2;

Servo myservo3;

Servo myservo4;

Servo myservo5;

int retardo = 10;

int potpin0 = 0;

int val0 = 84;

int potpin1 = 1;

int val1 = 78;

int potpin2 = 2;

int val2 = 150;

int potpin3 = 3;

int val3 = 98;

int potpin4 = 4;

int val4 = 57;

int potpin5 = 5;

int val5 = 90;

int pausa = 1000;

int pausaMove = 10;

void setup() {

  Serial.begin(9600);

  myservo0.attach(2);

  myservo1.attach(3);

  myservo2.attach(4);

  myservo3.attach(5);

  myservo4.attach(6);

  myservo5.attach(7);

  pinMode(13, OUTPUT);

  //Posiçao Inicial

  myservo0.write(val0);

  myservo1.write(val1);

  myservo2.write(val2);

  myservo3.write(val3);

  myservo4.write(val4);

  myservo5.write(val5);

}

void loop() {

  /*

  Cada um dos parâmetros da função "Movimento" representa a próxima posição dos servos;

  As variáveis que receberão os parâmetros estão na ordem:

  val0, val1, val2, val3, val4, val5;

  */ 

  Movimento (167,102,167,170,57,48);

  delay(pausa);

  

  Movimento (77,65,101,180,34,52);

  delay(pausa);

  

  Movimento (15,97,173,170,34,30);

  delay(pausa);

  Movimento (77,65,101,180,34,52);

  delay(pausa);

  Movimento (84,78,150,115,57,90);

  delay(pausa);

  Movimento (77,65,101,180,34,52);

  delay(pausa);

  

  tone(9,1000,1000);

  delay(3000);

}

void Movimento(int servo0, int servo1, int servo2, int servo3, int servo4, int servo5){

  

  tone(9,800,500);

  

 for (int i = 0; i <= 100; i++) {

    float a = map(i, 0, 100, val0 * 100 , servo0 * 100);

    a = a / 100;

    myservo0.write(a);

    float b = map(i, 0, 100, val1 * 100 , servo1 * 100);

    b = b / 100;

    myservo1.write(b);

    float c = map(i, 0, 100, val2 * 100, servo2 * 100);

    c = c / 100;

    myservo2.write(c);

    float d = map(i, 0, 100, val3 * 100, servo3 * 100);

    d = d / 100;

    myservo3.write(d);

    float e = map(i, 0, 100, val4 * 100 , servo4 * 100);

    e = e / 100;

    myservo4.write(e);

    float f = map(i, 0, 100, val5 * 100 , servo5 * 100);

    f = f / 100;

    myservo5.write(f);

    delay(pausaMove);  

/*

    Serial.print(" Tempo de espera : ");

    Serial.print(pausaMove);

    Serial.print(" Val0 : ");

    Serial.print(a);

    Serial.print(" Val1 : ");

    Serial.print(b);

    Serial.print(" Val2 : ");

    Serial.print(c);

    Serial.print(" Val3 : ");

    Serial.print(d);

    Serial.print(" Val4 : ");

    Serial.print(e);

    Serial.print(" Val5 : ");

    Serial.println(f);

*/

  }

  val0 = servo0;

  val1 = servo1;

  val2 = servo2;

  val3 = servo3;

  val4 = servo4;

  val5 = servo5; 

}