joy_inverso.m

是一个在matlab下的机器人运动仿真的软件

function Joy_Inverso(hObject, eventdata, handles)

global flag_joy Px Py Pz
global teta1 teta4 teta5 teta6 tetap tetaf d2 d3 l1 d4 x_joyantesPx y_joyantesPy z_joyantesPz
warning off;

%ai=analoginput('joy',1); % the number is an ID number of installed joystick (see Controller IDs in Control panel/Gaming options

%addchannel(ai,[1 2 3 4 5 6]);
%channels 
% 1 - X axis
% 2 - Y axis
% 3 - Z axis
% 4 - R axis
% 5 - buttons pressed (quasy binary data - see below)
% 6 - no of buttons pressed at the time
% Set up the display

flag_joy=1;
while flag_joy==1 
    pause(0.05);
    samples = jst;
    samples(1) = -samples(1)*100;  %Y data
    samples(2) = samples(2)*100;   %X data
    samples(3) = -samples(3)*100;  %Z data
    % decoding the data from dll. You may play with the dll. Please see
    % JoyIn.cpp
    ButtonsPressed = samples(5);
    BP = ButtonsPressed;
        
    if BP==32         %BP==32 =>R1 controlar eixos dos XX
     % pause(0.01);
      x_joy=samples(2);    %%varia entre -100 e 100
     
      x_joy=x_joy+x_joyantesPx;
      x_joyantesPx=x_joy;
      Pxmax=sqrt((282^2)-(Py^2));
      Pxmin=sqrt((112.75^2)-(Py^2));
      if(x_joy>=0)
          Px=((x_joy*(Pxmax-Pxmin)/100)+Pxmin);
      else
          Px=((x_joy*(Pxmax-Pxmin)/100)-Pxmin);
      end
      
      if(x_joy>100)
          Px=Pxmax;
      end    
      if(x_joy<-100)
          Px=-Pxmax;
      end    

      teta1=atan2(Py,Px)+atan2(sqrt(Px^2+Py^2-l1^2),l1);
      d2=Pz;
      d3=sin(teta1)*Px-cos(teta1)*Py-d4;
      teta4=atan2(1,0);
      teta5=(tetap*pi)/180;
      tetaf=(tetaf*pi)/180;
      teta6=tetaf-teta4;
    
      Pxstr=num2str(Px);
      Pystr=num2str(Py);
      Pzstr=num2str(Pz);
      set(handles.textpz,'String',Pzstr);
      set(handles.textpx,'String',Pxstr);
      set(handles.textpy,'String',Pystr);
      
      SimuladorCs('Desenha_robo',hObject, eventdata,handles); %tem q estar dentro senao o cpu est