• Author / Uploaded
• Daniel Arevalo

Citation preview

Programa cinemática inversa

double theta2; double theta3;

#include

double f;

#include

double k; double t;

//Declaraciones

float pi=3.1416;

//Creando objetos servos VarSpeedServo servo1;

void setup() {

VarSpeedServo servo2;

// put your setup code here, to run once:

VarSpeedServo servo3; //pines de servos const int servoPin1=3; const int servoPin2=5; const int servoPin3=9; //Longitud eslabones

servo1.attach(servoPin1); servo3.attach(servoPin3); servo2.attach(servoPin2); // Posiciones iniciales de los servos delay(800);

int d=5; //[5 cm] int a2=8; //[8 cm] int a3=5.7; //[5.7 cm]

servo1.write(90,30,true); delay(800);

//Variables y constantes int q1; int q2;

servo3.write(90,30,true); delay(800);

int q3; double yc=0;

servo2.write(180,30,true);

double xc=0; double zc=d+a2+a3; double theta1;

//Inicializando comunicación serial Serial.begin(9600);

} /*Comentario caso 1: xc=0 yc=0 zc=d+a2+a3

void loop() { // put your main code here, to run repeatedly: //Comunicaion serial if (Serial.available()>0){ //Cinemática inversa del brazo robot

caso2 xc=a2+a3

//1er GDL

yc=0

theta1=atan2(xc,yc)*180/pi;

zc=d

q1=(int)theta1; //Casting

caso3

//3er GDL

xc=0

f=zc-d;

yc=a2+a3 zc=d

theta3=acos(pow(f,2)+pow(xc,2)+p ow(yc,2)-pow(a2,2)pow(a3,2)/2*a2*a3)*180/pi; q2=(int)theta3; //Casting

*/ //2do GDL t=sqrt(pow(xc,2)+pow(yc,2)); theta2=(atan2(t,(zc-d)) +pi/2)*180/pi; q3=(int)theta2; //Casting

delay(800); //Impresion en monitor serial

Serial.println("Theta1: "); Serial.println(q1,DEC); Serial.println("Theta2: "); Serial.println(q3,DEC); Serial.println("Theta3: "); Serial.println(q2,DEC);

//Señales de control de ángulos

delay(800); servo1.write(q1,30,true); delay(800); servo2.write(q3,30,true); delay(800); servo3.write(q2+90,30,true);

} }

}

Programa para control directo de ángulos #include

void loop(){ leer_dato();

VarSpeedServo ServoA; VarSpeedServo ServoB; VarSpeedServo ServoC;

switch(comando){ case 'a': ServoA.write(posicion,30,true);

char comando;

comando=' ';

int posicion;

break;

void setup(){

case 'b': ServoB.write(posicion,30,true);

ServoA.attach(3);

comando=' ';

ServoB.attach(5);

break;

ServoC.attach(9); Serial.begin(9600);

case 'c':

} void leer_dato(){

ServoC.write(posicion+90,30,true);

if (Serial.available()>0){

comando=' ';

comando=Serial.read();

break;

posicion=Serial.parseInt(); }

} }