Arduino: Brazo robot con servos ( Robot arm ) Parte 2

Buenas a todos!!!

Buenas a todos!!! Como ya publique hace un tiempo, tenia este brazo robot de 4 ejes hecho con una impresora 3D, en la primera entrada no publique código por un problema que tuve con el PC, pero esta vez va completo.

Bueno todo empezó cuando buscando algo, vaya usted a saber que... encontré un viejo gamepad USB, (a decir verdad encontré varios) y se me ocurrió la idea de usar los thumbsticks o joys para controlar los movimientos del brazo.

Después de desmontar el mando en este modelo vienen los dos joys montados en una misma plaquita. 

Con la ayuda de un multímetro identificamos alimentación y las salidas de los potenciómetros además de los dos pulsadores que tiene cada joy, en mi caso solo tuve que seguir las pistas y hacer una pequeña modificación con la resistencia que venia de casa en el joypad.

Solo nos hace falta:

• Arduino UNO o NANO
• 4 microservos del tipo 9g
• 2 Joystick de 2 ejes y pulsador

Una vez tengamos todas las piezas el montaje es el siguiente:

Los ejes de los joystick controlan los movimientos y los pulsadores que llevan incorporados se utilizan para abrir y cerrar la pinza.

Los joysticks se componen por dos potenciómetros, o lo que es lo mismo, tienen una lectura analógica. Como su rango de lectura va desde los 0 hasta los 1023.

Por otra parte los servos tienen un rango de 0 a 180 que son los grados de movimiento que nos permite el servo. Para hacer esta conversión usaremos la función map():

int value = analogRead(0);
value = map(value, 0, 1023, 0, 180);
Serial.println(value);

Esta función nos devolverá un valor normalizado entre 0 y 180. En este rango de valores tenemos el punto medio del servo en 90.

Los únicos valores que habría que modificar serian los máximos (maxSerXpos) y mínimos (minSerXpos) de cada servo además de el punto medio donde quedaran los ejes una vez soltemos los joysticks.

Se configura justo después del comentario "//estos son los valores para los joysticks en reposo".

#include <servo.h>

Servo Bservo;
Servo Hservo;
Servo Cservo;
Servo Mservo;
Servo Pservo;

int serBpin = 2;  //  Servo Base
int serHpin = 3;  //  Servo Hombro
int serCpin = 4;  //  Servo Codo
int serMpin = 5;  //  Servo Muñeca
int serPpin = 6;  //  Servo Pinza

int rxPin = A1;   
int ryPin = A0;
int lxPin = A3;
int lyPin = A2;
int rButtonPin = 11;
int lButtonPin = 10;


int rxPosition = 0;
int ryPosition = 0;
int lxPosition = 0;
int lyPosition = 0;
int rButtonState = 0;
int lButtonState = 0;

int maxSerBpos = 150;
int maxSerHpos = 90;   // Subir Hombro max = 90
int maxSerCpos = 170;  // Subir Codo  max = 180
int maxSerMpos = 170;  // Subir Muñeca max = 180
int maxSerPpos = 140;  // Abre pinza

int minSerBpos = 70;
int minSerHpos = 30;   //Bajar Hombro min = 30
int minSerCpos = 10;  //Bajar Codo min  = 10
int minSerMpos = 10;  //Bajar Muñeca min = 10
int minSerPpos = 50;  //Bajar Cierra pinza


int serBpos = (maxSerBpos / 2);
int serHpos = (maxSerHpos / 2);
int serCpos = (maxSerCpos / 2);
int serMpos = (maxSerMpos / 2);
int serPpos = (maxSerPpos / 2);

int movingSteep = 20;
int movingGreep = 50;
int steep = 1000;



void setup() {
  Serial.begin(115200);

  pinMode(rxPin, INPUT);
  pinMode(ryPin, INPUT);
  pinMode(lxPin, INPUT);
  pinMode(lyPin, INPUT);
  pinMode(rButtonPin, INPUT_PULLUP);
  pinMode(lButtonPin, INPUT_PULLUP);

  // Pines de salida 
  pinMode(serBpin, OUTPUT);
  pinMode(serHpin, OUTPUT);
  pinMode(serCpin, OUTPUT);
  pinMode(serMpin, OUTPUT);
  pinMode(serPpin, OUTPUT);

  Hservo.attach(serHpin);
  Hservo.write(serHpos);
  delay(200);
  Cservo.attach(serCpin);
  Cservo.write(serCpos);
  delay(200);
  Mservo.attach(serMpin);
  Mservo.write(serMpos);
  delay(200);
  Pservo.attach(serPpin);
  Pservo.write(serPpos);
  delay(200);
  Bservo.attach(serBpin);
  Bservo.write(serBpos);
}

void loop() {
  rxPosition = analogRead(rxPin);
  ryPosition = analogRead(ryPin);
  lxPosition = analogRead(lxPin);
  lyPosition = analogRead(lyPin);

  if (digitalRead(rButtonPin) == LOW) {
    serPpos++;
    Pservo.write(serPpos);
    if (serPpos > maxSerPpos) {
      serPpos = maxSerPpos;
    }
  }
  if (digitalRead(lButtonPin) == LOW) {
    serPpos--;
    Pservo.write(serPpos);
    if (serPpos < minSerPpos) {
      serPpos = minSerPpos;
    }
  }

  if (rxPosition < 505 || rxPosition > 520) {
    if (ryPosition < 510 || ryPosition > 514) {
      // Map values from potentiometers to a smaller range for the servos (0-255)
    }
  }
  if (lxPosition < 510 || lxPosition > 514) {
    if (lyPosition < 510 || lyPosition > 514) {
      // Map values from potentiometers to a smaller range for the servos (0-255)
    }
  }
  if (rxPosition >= 505 || rxPosition <= 520) {
    if (ryPosition >= 510 || ryPosition <= 514) {
      if (lxPosition >= 510 || lxPosition <= 514) {
        if (lyPosition >= 510 || lyPosition <= 514) {

//estos son los valores para los joysticks en reposo

          Bservo.write(100);
          Hservo.write(60);
          Cservo.write(60);
          Mservo.write(150);

        }
      }
    }
  }

  rxPosition = map(rxPosition, 0, 1023, minSerBpos, maxSerBpos);
  ryPosition = map(ryPosition, 0, 1023, minSerHpos, maxSerHpos);
  lxPosition = map(lxPosition, 0, 1023, minSerCpos, maxSerCpos);
  lyPosition = map(lyPosition, 0, 1023, minSerMpos, maxSerMpos);
  Bservo.write(rxPosition);
  Hservo.write(ryPosition);
  Cservo.write(lxPosition);
  Mservo.write(lyPosition);
  delay (10);

  Serial.print("rX: ");
  Serial.print(rxPosition);
  Serial.print(" | rY: ");
  Serial.print(ryPosition);
  Serial.print("lX: ");
  Serial.print(lxPosition);
  Serial.print(" | lY: ");
  Serial.print(lyPosition);
  Serial.print(" | rButton: ");
  Serial.println(serPpos);

}

Y bueno en principio esto es todo, os dejo el vídeo de este "Terminator".

Saludps!!!