puma560_control.m

PUMA560仿真系统,基于MATLAB的robotics toolbox工具箱。

T2=transl(0.6,0.5,0.2);
T3=transl(0.6,0.5,-0.2);
T4=transl(0.6,-0.5,-0.2);
T={T1,T2,T3,T4,T1};
eval('puma560');
for i=1:1,
    for j=1:4,
        start_point=T{j};
        end_point=T{j+1};
        T0=ctraj(start_point,end_point,36);
        qianhe=findobj('Tag','qianhe');
    if isempty(qianhe),
        qianhe=figure('Name','PUMA560机器人仿真演示窗口（三维坐标）','Tag','qianhe');
        figure(qianhe);
        plot3(squeeze(T0(1,4,:)),squeeze(T0(2,4,:)),squeeze(T0(3,4,:)));
        hold on;
    else
        hold on;
        figure(qianhe);
        plot3(squeeze(T0(1,4,:)),squeeze(T0(2,4,:)),squeeze(T0(3,4,:)));
    end
        q=ikine(p560,T0);
        puma=findobj('Tag','puma560_gui');
    if ~isempty(puma),
        puma=guihandles(puma);
        axes(puma.axes1);
        cla;
        plot(p560,q);
    else
        puma=guihandles(puma560_gui);
        axes(puma.axes1);
        cla;
        plot(p560,q);
    end
    end
end

% --- Executes on slider movement.
function slider_q1_Callback(hObject, eventdata, handles)
% hObject    handle to slider_q1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
val=get(hObject,'Value');
set(handles.edit_q1,'String',num2str(val));
drive_robot(handles);


% --- Executes during object creation, after setting all properties.
function slider_q1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to slider_q1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end


% --- Executes on slider movement.
function slider_q2_Callback(hObject, eventdata, handles)
% hObject    handle to slider_q2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
val=get(hObject,'Value');
set(handles.edit_q2,'String',num2str(val));
drive_robot(handles);

% --- Executes during object creation, after setting all properties.
function slider_q2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to slider_q2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end


% --- Executes on slider movement.
function slider_q3_Callback(hObject, eventdata, handles)
% hObject    handle to slider_q3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
val=get(hObject,'Value');
set(handles.edit_q3,'String',num2str(val));
drive_robot(handles);

% --- Executes during object creation, after setting all properties.
function slider_q3_CreateFcn(hObject, eventdata, handles)
% hObject    handle to slider_q3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end


% --- Executes on slider movement.
function slider_q4_Callback(hObject, eventdata, handles)
% hObject    handle to slider_q4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
val=get(hObject,'Value');
set(handles.edit_q4,'String',num2str(val));
drive_robot(handles);

% --- Executes during object creation, after setting all properties.
function slider_q4_CreateFcn(hObject, eventdata, handles)
% hObject    handle to slider_q4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end


% --- Executes on slider movement.
function slider_q5_Callback(hObject, eventdata, handles)
% hObject    handle to slider_q5 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
val=get(hObject,'Value');
set(handles.edit_q5,'String',num2str(val));
drive_robot(handles);

% --- Executes during object creation, after setting all properties.
function slider_q5_CreateFcn(hObject, eventdata, handles)
% hObject    handle to slider_q5 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end


% --- Executes on slider movement.
function slider_q6_Callback(hObject, eventdata, handles)
% hObject    handle to slider_q6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
val=get(hObject,'Value');
set(handles.edit_q6,'String',num2str(val));
drive_robot(handles);

% --- Executes during object creation, after setting all properties.
function slider_q6_CreateFcn(hObject, eventdata, handles)
% hObject    handle to slider_q6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end



function edit_q1_Callback(hObject, eventdata, handles)
% hObject    handle to edit_q1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_q1 as text
%        str2double(get(hObject,'String')) returns contents of edit_q1 as a double
val=str2double(get(hObject,'String'));
slider_min=get(handles.slider_q1,'Min');
slider_max=get(handles.slider_q1,'Max');
if val>=slider_min && val<=slider_max,
    set(handles.slider_q1,'Value',val);
    drive_robot(handles);
else
    uiwait(errordlg(['Your Value must be in ' num2str(slider_min) ' ~ ' num2str(slider_max) '!'],'Error'));
    set(hObject,'String','');
end

% --- Executes during object creation, after setting all properties.
function edit_q1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_q1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit_q2_Callback(hObject, eventdata, handles)
% hObject    handle to edit_q2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_q2 as text
%        str2double(get(hObject,'String')) returns contents of edit_q2 as a double
val=str2double(get(hObject,'String'));
slider_min=get(handles.slider_q2,'Min');
slider_max=get(handles.slider_q2,'Max');
if val>=slider_min && val<=slider_max,
    set(handles.slider_q2,'Value',val);
    drive_robot(handles);
else
    uiwait(errordlg(['Your Value must be in ' num2str(slider_min) ' ~ ' num2str(slider_max) '!'],'Error'));
    set(hObject,'String','');
end

% --- Executes during object creation, after setting all properties.
function edit_q2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_q2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit_q3_Callback(hObject, eventdata, handles)
% hObject    handle to edit_q3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_q3 as text
%        str2double(get(hObject,'String')) returns contents of edit_q3 as a double
val=str2double(get(hObject,'String'));
slider_min=get(handles.slider_q3,'Min');
slider_max=get(handles.slider_q3,'Max');
if val>=slider_min && val<=slider_max,
    set(handles.slider_q3,'Value',val);
    drive_robot(handles);
else
    uiwait(errordlg(['Your Value must be in ' num2str(slider_min) ' ~ ' num2str(slider_max) '!'],'Error'));
    set(hObject,'String','');
end

% --- Executes during object creation, after setting all properties.
function edit_q3_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_q3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit_q4_Callback(hObject, eventdata, handles)
% hObject    handle to edit_q4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_q4 as text
%        str2double(get(hObject,'String')) returns contents of edit_q4 as a double
val=str2double(get(hObject,'String'));
slider_min=get(handles.slider_q4,'Min');
slider_max=get(handles.slider_q4,'Max');
if val>=slider_min && val<=slider_max,
    set(handles.slider_q4,'Value',val);
    drive_robot(handles);
else
    uiwait(errordlg(['Your Value must be in ' num2str(slider_min) ' ~ ' num2str(slider_max) '!'],'Error'));
    set(hObject,'String','');
end

% --- Executes during object creation, after setting all properties.
function edit_q4_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_q4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit_q5_Callback(hObject, eventdata, handles)
% hObject    handle to edit_q5 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_q5 as text
%        str2double(get(hObject,'String')) returns contents of edit_q5 as a double
val=str2double(get(hObject,'String'));
slider_min=get(handles.slider_q5,'Min');
slider_max=get(handles.slider_q5,'Max');
if val>=slider_min && val<=slider_max,
    set(handles.slider_q5,'Value',val);
    drive_robot(handles);
else
    uiwait(errordlg(['Your Value must be in ' num2str(slider_min) ' ~ ' num2str(slider_max) '!'],'Error'));
    set(hObject,'String','');
end

% --- Executes during object creation, after setting all properties.
function edit_q5_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_q5 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit_q6_Callback(hObject, eventdata, handles)
% hObject    handle to edit_q6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit_q6 as text
%        str2double(get(hObject,'String')) returns contents of edit_q6 as a double
val=str2double(get(hObject,'String'));
slider_min=get(handles.slider_q6,'Min');
slider_max=get(handles.slider_q6,'Max');
if val>=slider_min && val<=slider_max,
    set(handles.slider_q6,'Value',val);
    drive_robot(handles);
else
    uiwait(errordlg(['Your Value must be in ' num2str(slider_min) ' ~ ' num2str(slider_max) '!'],'Error'));
    set(hObject,'String','');
end

% --- Executes during object creation, after setting all properties.
function edit_q6_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit_q6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


function drive_robot(handles)
theta1=get(handles.slider_q1,'Value');
theta2=get(handles.slider_q2,'Value');
theta3=get(handles.slider_q3,'Value');
theta4=get(handles.slider_q4,'Value');
theta5=get(handles.slider_q5,'Value');
theta6=get(handles.slider_q6,'Value');
puma=findobj('Tag','puma560_gui');
if ~isempty(puma),
    puma=guihandles(puma);
    axes(puma.axes1);
    cla;
    q=[theta1 theta2 theta3 theta4 theta5 theta6];
    eval('puma560');
    plot(p560,q);
else
    puma=guihandles(puma560_gui());
    axes(puma.axes1);
    cla;
    q=[theta1 theta2 theta3 theta4 theta5 theta6];
    eval('puma560');
    plot(p560,q);
end