Programa cinemtica inversa

#include #include

//Declaraciones//Creando objetos servosVarSpeedServo servo1;VarSpeedServo servo2;VarSpeedServo servo3;//pines de servosconst int servoPin1=3;const int servoPin2=5;const int servoPin3=9;//Longitud eslabonesint d=5; //[5 cm]int a2=8; //[8 cm]int a3=5.7; //[5.7 cm]//Variables y constantesint q1;int q2;int q3;double yc=0;double xc=0;double zc=d+a2+a3;double theta1;double theta2;double theta3;double f;double k;double t;float pi=3.1416;

void setup() { // put your setup code here, to run once: servo1.attach(servoPin1); servo3.attach(servoPin3); servo2.attach(servoPin2); // Posiciones iniciales de los servos delay(800); servo1.write(90,30,true); delay(800); servo3.write(90,30,true); delay(800); servo2.write(180,30,true); //Inicializando comunicacin serial Serial.begin(9600);}/*Comentario caso 1: xc=0 yc=0 zc=d+a2+a3 caso2 xc=a2+a3 yc=0 zc=d caso3 xc=0 yc=a2+a3 zc=d

*/

void loop() { // put your main code here, to run repeatedly://Comunicaion serial if (Serial.available()>0){ //Cinemtica inversa del brazo robot //1er GDL theta1=atan2(xc,yc)*180/pi; q1=(int)theta1; //Casting //3er GDL f=zc-d;theta3=acos(pow(f,2)+pow(xc,2)+pow(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); //Seales 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

VarSpeedServo ServoA;VarSpeedServo ServoB;VarSpeedServo ServoC;

char comando;int posicion;

void setup(){ ServoA.attach(3); ServoB.attach(5); ServoC.attach(9); Serial.begin(9600);}void leer_dato(){ if (Serial.available()>0){ comando=Serial.read(); posicion=Serial.parseInt(); }}

void loop(){ leer_dato();

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

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

case 'c': ServoC.write(posicion+90,30,true); comando=' '; break; }}