ARMAR ROBOT.

PRIMER CODIGO PARA CALCULAR POSICION DEL EFECTOR FINAL:

% PROGRAMA P92; AQU ES PARA PONER EL ANGULO DE JIRO
% EN q1=pi/2=90, aqu jira 90, pi/4= 45
clear; close all; clc
L1=16;
q1=pi/2; % aqui se ingresa el angulo degiro del servo
angq1=q1*180/pi; % angulo de q1
disp('ang deseado'), disp(angq1)
% CINEMATICA DIRECTA
X=L1*cos(q1);
Y=L1*sin(q1);
parXY=[X,Y]; % solo pares de X i Y
disp('(X,Y)'), disp(parXY)
% cinematica Inversa
q1=atan2(Y,X);
angq1ci=q1*180/pi;
disp('ang IKINE'), disp(angq1ci)
PROGRAMA ES PARA CONECTAR AL ARDUINO Y EL MOTOR SE CONECTA AL PIN 12
clear; close

all; clc

% CODIGO ARDUINO EN BASE A COMANDOS

% PARA QUE SEA CONVERTIDO A SIMULINK ARDUINOIO
% NOTA: el programa trabaja solo e independiente
% al simulink (debe estar cerrado el programa)
% no se puede correr ambos; solo funciona el codigo matlab o simulink
delete(instrfind({'Port'},{'COM4'}));
path(path,'C:\arduinoIO')
% Path para ubicar arduino, dentro de
comillas simple va
% la ubicacion donde se encuentra el arduinoIO
% siguiente es para la libreria
a=arduino('COM4','uno');
s1=servo(a,12);
% 12 es el prueto
% Posicion inicial (x,y)
x1=16;
y1=0;
% posicion final

x2=0.0000; % aqui se copia los valores obtenidos de P92

y2=16.0000;
while(true)
% primer movimiento
qa1=robot1a(x1,y1);
writePosition(s1,qa1/180);
disp('qi'), disp(qa1)
pause(2); % pausa de 2 segundos
% Segundo movimiento
qb1=robot1a(x2,y2);
writePosition(s1,qb1/180);
disp('qf'), disp(qb1)
pause(2); % pausa de 2 segundos
end