Práctica 7. Control de articulaciones de modelos de Robots. FRO

Transcripción

1 Práctica 7. Control de articulaciones de modelos de Robots. FRO Objetivos: Corroborar la cinemática directa de un robot Familiarizarse con la etapa del control de los actuadores de un robot a escala Materiales 1 brazo robot a escala 1 arduino 1 1 PC con IDE de arduino 1 Cable para arduino 1 Fuente de poder variable de 1 amperio 1 Breadboard 2 resistencia de 1k 4 potenciómetros de 10 k Conectores Procedimiento: 1. Cargue y corra el programa 1 (control por medio de potenciómetros), verifique el efecto que produce la variación de los potenciómetros. 2. Anote cuál es la posición de extremo del robot cuando las articulaciones tienen 0 grados 3. Anote cuál es la posición de extremo del robot cuando las articulaciones tienen 90 grados 4. Cargue y corra el programa 2 (control por medio de datos del monitor serial) 5. Llene la siguiente tabla midiendo la posición y la orientación del extremo del robot (en la tabla se pueden utilizar otros valores de q si fuera necesario) No. q1 q2 q3 q4 x y z Teta Compare los datos obtenidos con los valores que resultan del análisis con el modelo cinemático directo de ese robot (obtener el modelo cinemático directo si es necesario) 7. Cargue y corra el programa 3 (control del robot datos en el código) 8. Describa lo que realiza el programa 9. Cargue y corra el programa 4 (control robot desde matlab 2018) 10. Corra desde matlab el programa paarduino.m 11. Describa lo que realiza el programa

2 Programa 1: Control por medio de potenciómetros. /* Aplicacion que mueve cuatro Servos (articulaciones) a la posicion indicada por los potenciometros y manda la lectura en grados al monitor serial */ #include <Servo.h> //incluimos una biblioteca de funciones para manejar Servomotores // se declaran variables que identifican los pines de salida para los servos int pinservo1 = 9; //llamamos pinservo1 al pin 9 int pinservo2 = 7; //llamamos pinservo2 al pin 7 int pinservo3 = 5; //llamamos pinservo3 al pin 5 int pinservo4 = 4; //llamamos pinservo4 al pin 4 // se declara los pines de entradas analógicas int pinpot1 = 0; //llamamos pinpot1 al pin 0 int pinpot2 = 1; //llamamos pinpot2 al pin 1 int pinpot3 = 2; //llamamos pinpot3 al pin 2 int pinpot4 = 3; //llamamos pinpot4 al pin 3 // se declaran los objetos tipo servo, uno para cada servo a utilizar Servo miservo1; //Creamos un elemento Servo en el sistema Servo miservo2; //Creamos un elemento Servo en el sistema Servo miservo3; //Creamos un elemento Servo en el sistema Servo miservo4; //Creamos un elemento Servo en el sistema

3 void setup() // se indica el pin de salida de cada servo vinculandolo con el objeto servo miservo1.attach(pinservo1); //Indicamos que el servo esta conectado a pinservo miservo2.attach(pinservo2); //Indicamos que el servo esta conectado a pinservo miservo3.attach(pinservo3); //Indicamos que el servo esta conectado a pinservo miservo4.attach(pinservo4); //Indicamos que el servo esta conectado a pinservo // velocidad de transmision de datos por monitor serial Serial.begin(9600); //Funcion bucle de Arduino. Corazon del programa void loop() // se declaran las variables de salida de los ADC datos de 0 a 1023 int valorpot1 = 0; int valorpot2 = 0; int valorpot3 = 0; int valorpot4 = 0; // se declaran las variables de angulos, rango 0 a 179 grados int valorservo1 = 0; int valorservo2 = 0;

4 int valorservo3 = 0; int valorservo4 = 0; valorpot1 = analogread(pinpot1); //leemos el valor del potenciometro valorservo1 = map(valorpot1, 0, 1023, 0, 179); //convertimos ese valor a grados miservo1.write(valorservo1); hemos calculado //movemos el servo a la posicion que valorpot2 = analogread(pinpot2); //leemos el valor del potenciometro valorservo2 = map(valorpot2, 0, 1023, 0, 179); //convertimos ese valor a grados miservo2.write(valorservo2); hemos calculado //movemos el servo a la posicion que valorpot3 = analogread(pinpot3); //leemos el valor del potenciometro valorservo3 = map(valorpot3, 0, 1023, 0, 179); //convertimos ese valor a grados miservo3.write(valorservo3); hemos calculado //movemos el servo a la posicion que valorpot4 = analogread(pinpot4); //leemos el valor del potenciometro valorservo4 = map(valorpot4, 0, 1023, 0, 179); //convertimos ese valor a grados miservo4.write(valorservo4); hemos calculado //movemos el servo a la posicion que // se escriben datos en monitor serial Serial.print("q1="); Serial.println(valorServo1); Serial.print("q2="); Serial.println(valorServo2); Serial.print("q3="); Serial.println(valorServo3); Serial.print("q4=");

5 Serial.println(valorServo4); Serial.println(" "); // retardo de tiempo para leer datos comodamente delay(100);

6 Programa 2: Control por medio de datos del monitor serial. /* Aplicacion que mueve cuatro Servos (articulaciones) conforme lo que se escribe en el monitor serial */ #include <Servo.h> //incluimos una biblioteca de funciones para manejar Servomotores // se declaran variables que identifican los pines de salida para los servos int pinservo1 = 9; //llamamos pinservo1 al pin 9 int pinservo2 = 7; //llamamos pinservo2 al pin 7 int pinservo3 = 5; //llamamos pinservo3 al pin 5 int pinservo4 = 4; //llamamos pinservo4 al pin 4 // se declaran los objetos tipo servo, uno para cada servo a utilizar Servo miservo1; //Creamos un elemento Servo en el sistema Servo miservo2; //Creamos un elemento Servo en el sistema Servo miservo3; //Creamos un elemento Servo en el sistema Servo miservo4; //Creamos un elemento Servo en el sistema // se declaran las variables de angulos, rango 0 a 179 grados int valorservo1 = 0; int valorservo2 = 0; int valorservo3 = 0; int valorservo4 = 0;

7 int input1=0; int input2=0; int input3=0; void setup() // se indica el pin de salida de cada servo vinculandolo con el objeto servo miservo1.attach(pinservo1); //Indicamos que el servo esta conectado a pinservo miservo2.attach(pinservo2); //Indicamos que el servo esta conectado a pinservo miservo3.attach(pinservo3); //Indicamos que el servo esta conectado a pinservo miservo4.attach(pinservo4); //Indicamos que el servo esta conectado a pinservo // velocidad de transmision de datos por monitor serial Serial.begin(9600); //Funcion bucle de Arduino. Corazon del programa void loop() // se introducen datos en monitor serial Serial.println("introduzca nuevo q1"); delay(5000);

8 if (Serial.available()>0) valorservo1=serial.parseint(); Serial.print("q1="); Serial.println(valorServo1); Serial.println("introduzca nuevo q2"); delay(5000); if (Serial.available()>0) valorservo2=serial.parseint(); Serial.print("q2="); Serial.println(valorServo2); Serial.println("introduzca nuevo q3"); delay(5000); if (Serial.available()>0) valorservo3=serial.parseint(); Serial.print("q3="); Serial.println(valorServo3); Serial.println("introduzca nuevo q4"); delay(5000); if (Serial.available()>0) valorservo4=serial.parseint();

9 Serial.print("q4="); Serial.println(valorServo4); Serial.println(" "); // Se mandan los datos a los servos miservo1.write(valorservo1); hemos calculado //movemos el servo a la posicion que miservo2.write(valorservo2); hemos calculado //movemos el servo a la posicion que miservo3.write(valorservo3); hemos calculado //movemos el servo a la posicion que miservo4.write(valorservo4); hemos calculado //movemos el servo a la posicion que // se presentan datos en monitor serial Serial.print("q1="); Serial.println(valorServo1); Serial.print("q2="); Serial.println(valorServo2); Serial.print("q3="); Serial.println(valorServo3); Serial.print("q4=");

10 Serial.println(valorServo4); Serial.println(" "); // retardo de tiempo para leer datos comodamente delay(100);

11 Programa 3: Control del robot datos en el código. /* código Aplicacion que permite el movimiento del robot entre tres puntos, la trayectoria de los servos está en el */ #include <Servo.h> // Incluimos la biblioteca de funciones "Servo", para manejar //Servomotores // se declaran variables que identifican los pines de salida para los servos int pinservo1 = 9; // llamamos pinservo1 al pin 9 int pinservo2 = 7; // llamamos pinservo2 al pin 7 int pinservo3 = 5; // llamamos pinservo3 al pin 5 int pinservo4 = 4; // llamamos pinservo4 al pin 4 int pinservo5 = 3; // llamamos pinservo5 al pin 3 // se declara el pin de entrada para habilitar movimiento int habil=11; // se declaran arreglos con los datos de los angulos a los que se moveran los servos int q1[18]=0,20,40,59,56,53,49,48,48,47,47,46,46,46,45,45,45,45; int q2[18]=90,101,112,124,126,127,129,126,124,121,113,107,101,90,85,79,74,67; int q3[18]=90,70,40,10,3,0,0,0,0,0,0,0,0,10,20,30,40,45; int q4[18]=93,101,110,119,119,118,118,116,114,112,108,104,100,95,85,80,75,72; // se declaran los objetos tipo servo, uno para cada servo a utilizar

12 Servo miservo1; // Creamos un elemento Servo en el sistema Servo miservo2; // Creamos un elemento Servo en el sistema Servo miservo3; // Creamos un elemento Servo en el sistema Servo miservo4; // Creamos un elemento Servo en el sistema Servo miservo5; // Creamos un elemento Servo en el sistema void setup() // se indica el pin de salida de cada servo vinculandolo con el objeto servo miservo1.attach(pinservo1); // Indicamos que el servomotor esta conectado al pinservo miservo2.attach(pinservo2); // Indicamos que el servomotor esta conectado al pinservo miservo3.attach(pinservo3); // Indicamos que el servomotor esta conectado al pinservo miservo4.attach(pinservo4); // Indicamos que el servomotor esta conectado al pinservo miservo5.attach(pinservo5); // Indicamos que el servomotor esta conectado al pinservo // se indica que la variable habil es una entrada pinmode(habil,input); Serial.begin(9600); void loop() if (digitalread(habil) == 1) // hay movimiento si la entrada está a 5v (el movimiento se repite // continuamente por eso al finalizar el movimiento hay que deshabilitar)

13 miservo5.write(180); // se abre la pinza delay(2000); for (int i=0; i<6; i++) // lazo para el movimiento de los servos hacia el punto 1 miservo1.write(q1[i]); //movemos el servo1 miservo2.write(q2[i]); //movemos el servo2 miservo3.write(q3[i]); //movemos el servo3 miservo4.write(q4[i]); //movemos el servo4 Serial.print("q1="); Serial.println(q1[i]); miservo5.write(0); // se cierra la pinza

14 delay(2000); for (int i=6; i<12; i++) // lazo para el movimiento de los servos hacia el punto 2 miservo1.write(q1[i]); //movemos el servo1 miservo2.write(q2[i]); //movemos el servo2 miservo3.write(q3[i]); //movemos el servo3 miservo4.write(q4[i]); //movemos el servo4 Serial.print("q1="); Serial.println(q1[i]); miservo5.write(180); // se abre la pinza delay(2000); for (int i=12; i<18; i++) // lazo para el movimiento de los servos hacia home

15 miservo1.write(q1[i]); //movemos el servo1 miservo2.write(q2[i]); //movemos el servo2 miservo3.write(q3[i]); //movemos el servo3 miservo4.write(q4[i]); //movemos el servo4 Serial.print("q1="); Serial.println(q1[i]); miservo5.write(0); // se cierra la pinza delay(2000); if (digitalread(habil) == 0) // el movimiento está inhabilitado miservo1.write(q1[5]); miservo2.write(q2[5]); miservo3.write(q3[5]); miservo4.write(180);

16

17 Programa 4: Control robot desde matlab /* Aplicacion que permite el movimiento del robot entre tres puntos, la trayectoria de los servos es mandada através de puerto serial desde matlab */ #include <Servo.h> // Incluimos la biblioteca de funciones "Servo", para manejar //Servomotores // se declaran variables que identifican los pines de salida para los servos int pinservo1 = 9; // llamamos pinservo1 al pin 9 int pinservo2 = 7; // llamamos pinservo2 al pin 7 int pinservo3 = 5; // llamamos pinservo3 al pin 5 int pinservo4 = 4; // llamamos pinservo4 al pin 4 int pinservo5 = 3; // llamamos pinservo5 al pin 3 // se declara el pin de entrada para habilitar movimiento int habil=11; // se declaran arreglos con los datos de los angulos a los que se moveran los servos int q1[18]=90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90; int q2[18]=90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90; int q3[18]=90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90; int q4[18]=90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90; // se declaran los objetos tipo servo, uno para cada servo a utilizar

18 Servo miservo1; // Creamos un elemento Servo en el sistema Servo miservo2; // Creamos un elemento Servo en el sistema Servo miservo3; // Creamos un elemento Servo en el sistema Servo miservo4; // Creamos un elemento Servo en el sistema Servo miservo5; // Creamos un elemento Servo en el sistema void setup() // se indica el pin de salida de cada servo vinculandolo con el objeto servo miservo1.attach(pinservo1); // Indicamos que el servomotor esta conectado al pinservo miservo2.attach(pinservo2); // Indicamos que el servomotor esta conectado al pinservo miservo3.attach(pinservo3); // Indicamos que el servomotor esta conectado al pinservo miservo4.attach(pinservo4); // Indicamos que el servomotor esta conectado al pinservo miservo5.attach(pinservo5); // Indicamos que el servomotor esta conectado al pinservo // se indica que la variable habil es una entrada pinmode(habil,input); Serial.begin(9600); void loop() if (digitalread(habil) == 1) // hay movimiento si la entrada está a 5v (el movimiento se repite // continuamente por eso al finalizar el movimiento hay que deshabilitar)

19 if (Serial.available()>0) for (int i=0; i<18; i++) q1[i]=serial.parseint(); for (int i=0; i<18; i++) q2[i]=serial.parseint(); for (int i=0; i<18; i++) q3[i]=serial.parseint(); for (int i=0; i<18; i++) q4[i]=serial.parseint(); miservo5.write(180); // se abre la pinza delay(2000); for (int i=0; i<6; i++) // lazo para el movimiento de los servos hacia el punto 1

20 miservo1.write(q1[i]); //movemos el servo1 miservo2.write(q2[i]); //movemos el servo2 miservo3.write(q3[i]); //movemos el servo3 miservo4.write(q4[i]); //movemos el servo4 Serial.print("q1="); Serial.println(q1[i]); miservo5.write(0); // se cierra la pinza delay(2000); for (int i=6; i<12; i++) // lazo para el movimiento de los servos hacia el punto 2 miservo1.write(q1[i]); //movemos el servo1 miservo2.write(q2[i]); //movemos el servo2

21 miservo3.write(q3[i]); //movemos el servo3 miservo4.write(q4[i]); //movemos el servo4 Serial.print("q1="); Serial.println(q1[i]); miservo5.write(180); // se abre la pinza delay(2000); for (int i=12; i<18; i++) // lazo para el movimiento de los servos hacia home miservo1.write(q1[i]); //movemos el servo1 miservo2.write(q2[i]); //movemos el servo2 miservo3.write(q3[i]); //movemos el servo3 miservo4.write(q4[i]); //movemos el servo4 Serial.print("q1=");

22 Serial.println(q1[i]); miservo5.write(0); // se cierra la pinza delay(2000); if (digitalread(habil) == 0) // el movimiento está inhabilitado miservo1.write(q1[5]); miservo2.write(q2[5]); miservo3.write(q3[5]); miservo4.write(180);