Para controlar nuestro brazo robótico necesitamos realizar un programa en Arduino. Nuestro controlador de servos se conecta a nuestra placa Arduino mediante el puerto I2C, para facilitar su uso y configuracion vamos a usar unas librerias.
Estas librerias estan hechas por Adafruit y se pueden descargar desde aquí.
En este ejemplo tenemos un codigo para controlar todos los servo del brazo de una forma suave. Un ajuste a tener en cuenta son los valores máximos y minimos que pueden tener nuestros servos, estos ajustes dependen del monaje del brazo y de los propios servos. Lo primero que debemos hacer es ajustar dichos parámetro para que el brazo funcione adecuadamente.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 |
////////////////////////////////////////////// // -test movimientos brazo 01- // // Programa de prueba para CompluARM // // En este programa se prueban secuencialmente // los servos del brazo robótico CompluARM // // Complubot - www.prueba.complubot.com - 04/06/2018 /////////////////////////////////////////////// //Incluimos las librerias de "wire" para usar el I2C y //las del controlador de servos de adafruit #include <Wire.h> #include <Adafruit_PWMServoDriver.h> int pos_ant[6] = {0, 0, 0, 0, 0, 0}; //Estos parametros sirven para ajustar las posiciones iniciales y finales del brazo int pos_min[6] = {110, 125, 600, 600, 600, 110}; //dependen de como hayas montado el brazo int pos_max[6] = {460, 410, 140, 140, 140, 220}; //Definimos el controlador de servos Adafruit_PWMServoDriver control_servos = Adafruit_PWMServoDriver(); void setup() { //Iniciamos el controlador control_servos.begin(); //Definimos la frecuencia de refresco de los servos a 60 HZ control_servos.setPWMFreq(60); //ponemos el brazo en la posicion inicial inicio_brazo(); delay(2000); } void loop() { //secuencia de movimientos de prueba mover_servo(0, 180); mover_servo(0, 0); mover_servo(4, 180); mover_servo(4, 0); mover_servo(5, 180); mover_servo(5, 0); while (true); } //funcion para mover el brazo, hace un desplazamiento suave entre dos posiciones //El servo va de 0 a 5 y la posición de 0 180 void mover_servo(int servo, int pos) { //controla el numero de pasos entre un punto y otro de un movimiento //cuanto mayor sea este numero el brazo se movera mas sueve, //pero tardara mas tiempo en realizarlo #define NUM_PASOS 100 if (pos > pos_ant[servo]) { for (int p = pos_ant[servo]; p < pos; p += (pos - pos_ant[servo]) / NUM_PASOS) { control_servos.setPWM(servo, 0, map(p, 0, 180, pos_min[servo], pos_max[servo])); delay(10); } } else { for (int p = pos_ant[servo]; p > pos; p -= abs((pos - pos_ant[servo]) / NUM_PASOS)) { control_servos.setPWM(servo, 0, map(p, 0, 180, pos_min[servo], pos_max[servo])); delay(10); } } control_servos.setPWM(servo, 0, map(pos, 0, 180, pos_min[servo], pos_max[servo])); pos_ant[servo] = pos; } void inicio_brazo() //funcion que pone el brazo en posicion inicial { for (int i = 0; i < 6; i++) { control_servos.setPWM(i, 0, pos_min[i]); delay(200); } } |
