Práctica 7. Control de articulaciones de modelos de Robots. FRO111 2018 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 1 10 20 30 40 2 45 45 45 45 3 80 110 70 100 4 100 100 100 100 6. 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
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
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;
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=");
Serial.println(valorServo4); Serial.println(" "); // retardo de tiempo para leer datos comodamente delay(100);
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;
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);
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();
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=");
Serial.println(valorServo4); Serial.println(" "); // retardo de tiempo para leer datos comodamente delay(100);
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
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)
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
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
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);
Programa 4: Control robot desde matlab 2018. /* 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
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)
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
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
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=");
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);