Đang tải... (xem toàn văn)
Báo cáo lập trình mô phỏng robot và hệ cơ điện tử ĐH Bách Khoa Hà Nội Soruce CODE ở cuối tài liệu Báo cáo bao gồm tính toán động học, động lực học và tĩnh học Ví dụ code tính toán ma trận M và động lực học syms ϴ1 ϴ2 ϴ3 assume(ϴ1,real); assume(ϴ2,real); assume(ϴ3,real); ϴ = ϴ1;ϴ2;ϴ3; qϴ=diff(ϴ); q2ϴ=diff(qϴ); %Tham so khoi luong m2= 217.55 ; m3= 73.49 ; g=9.8; a2= 83.1 ; a3= 60.76 ; m1= 336.08; %Tinh toan ma tran khoi luong T1 = cos(ϴ1) 0 sin(ϴ1) 30cos(ϴ1) ; sin(ϴ1) 0 cos(ϴ1) 30sin(ϴ1); 0 1 0 150 ; 0 0 0 1 ; T2 = cos(ϴ2) sin(ϴ2) 0 125cos(ϴ2); sin(ϴ2) cos(ϴ2) 0 125sin(ϴ2); 0 0 1 0 ; 0 0 0 1 ; T3 = cos(ϴ3) sin(ϴ3) 0 75cos(ϴ3); sin(ϴ3) cos(ϴ3) 0 75sin(ϴ3); 0 0 1 0 ; 0 0 0 1 ; T02=T1T2; T03=T1T2T3; %cac ma tran cosin chi huong A01=T1(1:3,1:3) A02=T02(1:3,1:3) A03=T03(1:3,1:3) %ma tran vi tri R1=T1(1:3,4) R2=T02(1:3,4)
Báo cáo tập lớn Học phần: Lập trình mơ robot hệ điện tử Đề tài: Xe Robot gắp vật Giáo viên hướng dẫn: TS.Nguyễn Thành Hùng Nhóm sinh viên thực hiện: Phạm Nguyễn Trung Hiếu MSSV: 20166113 Lớp: CN-Cơ điện tử 03 – K61 Hà Mạnh Tú MSSV: 20166948 Lớp: CN-Cơ điện tử 03 – K61 Mã lớp học: 104678 Bảng phân công nhiệm vụ: MSSV 20166113 20166948 Họ tên Phạm Nguyễn Trung Hiếu Nhiệm vụ Mơ Matlab Hà Mạnh Tú Tính tốn lý thuyết Thiết kế 3D Tổng hợp báo cáo NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN Hà Nội, ngày năm 2018 tháng 06 Giáo viên hướng dẫn (Ký, ghi rõ họ tên) Mục lục I Tổng quan đề tài Đặt vấn đề Lý chọn đề tài .5 I Thiết kế mơ hình 3D solidwork I Phần tính tốn động học .11 Thiết lập ma trận truyền 11 1.1 Bảng Denavit – Hatenberg 11 1.2 Thiết lập ma trận trạng thái khâu thao tác theo tọa độ thao tác .12 Giải toán động học thuận 12 2.1 Xác định tọa độ điểm thao tác 12 2.2 Xác định vận tốc, gia tốc điểm thao tác vận tốc khâu 13 Giải toán động học ngược 14 3.1 Động học ngược vị trí 15 3.2 Động học ngược vận tốc gia tốc 16 Bài toán tĩnh học .17 4.1 Cơ sở lý thuyết .17 4.2 Giải toán cụ thể 18 4.2.1 Lực dẫn động khâu 19 4.2.2 Lực dẫn động khâu 20 4.2.3 Lực dẫn động khâu 22 Bài toán động học 22 5.1 Khâu 24 5.2 Khâu 24 5.3 Khâu 25 II Sơ đồ mô Matlab – Simulink 27 III Giao diện điều khiển .28 IV Phần code 29 Lời mở đầu Nền khoa học kĩ thuật phát triển mạnh mẽ, dẫn tới thay đổi lớn lao sản xuất Đó thay đổi lực lượng sản xuất ngành nghề việc thay sức lao động người máy móc, nhằm đảm bảo tăng suất lao động, sản lượng chất lượng sản phẩm Do việc sử dụng tay máy hay gọi Robot cơng nghiệp vào sản xuất ưa chuộng chúng đáp ứng yêu cầu Như biết Robot có nhiều ưu điểm đặc biệt chất lượng độ xác, ngồi phải kể đến hiệu kinh tế cao Có thể làm việc môi trường độc hại mà người làm được, công việc yêu cầu cẩn thận không nhầm lẫn, thao tác nhẹ nhàng tinh tế đòi hỏi trình độ thợ bậc cao, quan trọng Robot không bị căng thẳng người nên làm việc ngày Việc tìm hiểu nghiên cứu Robot khuôn khổ môn học Lập trình mơ Robot hệ điện tử sở để nhóm em tính tốn, thiết kế mơ loại robot phục vụ lợi ích người Cụ thể nhóm em đề xuất đề tài:” Xe Robot gắp vật” Chúng em xin chân thành gửi lời cảm ơn tới thầy TS Nguyễn Thành Hùng Cảm ơn thầy đóng góp qua giảng hướng dẫn q trình hồn thành tập lớn Những góp ý, sửa chữa thầy giúp chúng em có thêm kinh nghiệm cho q trình học tâp làm việc sau Mặc dù có chuẩn bị kiến thức chúng em đề cập đến thuyết minh thiếu sót Chúng em mong có bổ sung, sửa chữa để thuyết minh hoàn thiện Chúng em xin chân thành cảm ơn chúc Thầy sức khỏe! I Tổng quan đề tài 1.Đặt vấn đề Ngày nay, Robot khơng q xa lạ với người Chúng giới kỹ thuật hình dung máy đặc biệt, người tác theo cấu tạo hoạt động mình, dùng để thay số cơng việc xác định Để hồn thành nhiệm vụ đó, Robot cần có khả cảm nhận thông số trạng thái môi trường thực động tác tương tự người Khả hoạt động Robot đảm bảo hệ thống khí, gồm cấu vận động để lại cấu hành động để làm việc Việc thiết kế chế tạo hệ thống thuộc lĩnh vực khoa học cấu truyền động, chấp hành vật liệu khí Chức cảm nhận, gồm thu nhận tín hiệu trạng thái mơi trường trạng thái thân hệ thống, cảm biến (sensor) thiết bị liên quan thực Hệ thống gọi hệ thống thu nhận xử lý tín hiệu, hay đơn giản hệ thống cảm biến Một cách đơn giản, Robotics hiểu ngành khoa học có nhiệm vụ nghiên cứu thiết kế, chế tạo Robot ứng dụng chúng lĩnh vực hoạt động khác xã hội loài người, nghiên cứu khoa học - kỹ thuật, kinh tế, quốc phòng dân sinh Ngày nay, khái niệm Robot mở rộng khái niệm nguyên thủy nhiều Sự tác kết cấu, chức năng, dáng vẻ người cần thiết khơng ngự trị kỹ thuật Robot Kết cấu nhiều Robot khác xa với kết cấu phận thể người chúng thực việc vượt xa khả người 2.Lý chọn đề tài Việc bốc xếp hàng kho việc vận chuyển khối hàng kho sau đóng gói Đa phần hàng hóa thường đựng khối hình hộp chữ nhật Hình 1.1 Hàng hóa kho, xưởng Hình 1.2 Robot hỗ trợ bốc, xếp hàng hóa Hiện nay, hầu hết doanh nghiệp cần có cơng đoạn bốc xếp hàng hóa Cơng việc đòi hỏi hành động trung gian vận chuyển khối hàng từ nơi sang nơi khác, có khối hàng to lớn có khối lượng nặng gây khó khăn cho người cơng nhân q trình vận chuyển Vì việc bốc xếp hàng cần tới trợ giúp robot để giúp đỡ người nhằm giảm chi phí nhân cơng tăng năng suất lao động I Thiết kế mơ hình 3D solidwork Sử dụng phần mềm Solidwork ta thiết kế đưuọc mơ hình 3D sản phẩm Hình 2.1 Mơ hình 3D sản phẩm Mơ hình bao gốm phần: + Phần khung xe: Phần bao gồm bánh xe, thân xe nắp xe Hình 2.1 Phần khung xe + Phần khâu thứ nhất: Các thông số kích thước khâu: Cao 150 – Rộng 30 10 sind(theta3) cosd(theta3) 75*sind(theta3); 0 0; 0 1]; T=T1*T2*T3; px=T(1,4); py=T(2,4); pz=T(3,4); %set(handles.edit_endX,'string',num2str(px)); set(handles.edit_endY,'string',num2str(py)); %set(handles.edit_endZ,'string',num2str(pz)); end; % - Executes on button press in set_link3 function set_link3_Callback(hObject, eventdata, handles) % hObject handle to set_link3 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) ModelName = 'xeth'; btstt_3 = get(handles.set_link3,'value'); if(btstt_3) theta1 = get(handles.edit_link1,'value'); theta2 = get(handles.edit_link2,'value'); theta3 = get(handles.edit_link3,'value'); set(handles.slider9,'value',theta3); set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta3)); T1 = [ cosd(theta1) 30*cosd(theta1); sind(theta1) 30*sind(theta1); 0 sind(theta1) -cosd(theta1) 0 150; 1]; 57 T2 = [ cosd(theta2) 125*cosd(theta2); sind(theta2) 125*sind(theta2); 0 -sind(theta2) cosd(theta2) 0 0; 1]; T3 = [ cosd(theta3) -sind(theta3) 75*cosd(theta3); sind(theta3) cosd(theta3) 75*sind(theta3); 0 0; 0 1]; T=T1*T2*T3; px=T(1,4); py=T(2,4); pz=T(3,4); %set(handles.edit_endX,'string',num2str(px)); %set(handles.edit_endY,'string',num2str(py)); set(handles.edit_endZ,'string',num2str(pz)); end; % - Executes on button press in set_link4 function set_link4_Callback(hObject, eventdata, handles) % hObject handle to set_link4 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) ModelName = 'xeth'; btstt_4 = get(handles.set_link4,'value'); if(btstt_4) theta_gripper = get(handles.edit_gripper,'value'); set(handles.slider10,'value',theta_gripper); set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta_gripper)); set_param([ModelName '/Slider Gain4'],'Gain',num2str(-theta_gripper)); 58 end; function editxa_Callback(hObject, eventdata, handles) % hObject handle to editxa (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 editxa as text % str2double(get(hObject,'String')) returns contents of editxa as a double % - Executes during object creation, after setting all properties function editxa_CreateFcn(hObject, eventdata, handles) % hObject handle to editxa (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 editya_Callback(hObject, eventdata, handles) % hObject handle to editya (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB 59 % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editya as text % str2double(get(hObject,'String')) returns contents of editya as a double % - Executes during object creation, after setting all properties function editya_CreateFcn(hObject, eventdata, handles) % hObject handle to editya (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 editza_Callback(hObject, eventdata, handles) % hObject handle to editza (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 editza as text % str2double(get(hObject,'String')) returns contents of editza as a double 60 % - Executes during object creation, after setting all properties function editza_CreateFcn(hObject, eventdata, handles) % hObject handle to editza (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 editxb_Callback(hObject, eventdata, handles) % hObject handle to editxb (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 editxb as text % str2double(get(hObject,'String')) returns contents of editxb as a double % - Executes during object creation, after setting all properties function editxb_CreateFcn(hObject, eventdata, handles) % hObject handle to editxb (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called 61 % 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 edityb_Callback(hObject, eventdata, handles) % hObject handle to edityb (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 edityb as text % str2double(get(hObject,'String')) returns contents of edityb as a double % - Executes during object creation, after setting all properties function edityb_CreateFcn(hObject, eventdata, handles) % hObject handle to edityb (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 62 function editzb_Callback(hObject, eventdata, handles) % hObject handle to editzb (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 editzb as text % str2double(get(hObject,'String')) returns contents of editzb as a double % - Executes during object creation, after setting all properties function editzb_CreateFcn(hObject, eventdata, handles) % hObject handle to editzb (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 % - Executes on button press in pushbutton_circle function pushbutton_circle_Callback(hObject, eventdata, handles) % hObject handle to pushbutton_circle (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) global theta1; global theta2; global theta3; 63 global t; ModelName = 'xeth'; button_circle = get(handles.pushbutton_circle,'value'); if (button_circle) px = str2num(get(handles.editxa,'string')); py = str2num(get(handles.editya,'string')); pz = str2num(get(handles.editza,'string')); time = str2num(get(handles.edit_time,'string')); rot = str2num(get(handles.edit_goc,'string')); %tinh cac goc khop theta1 = atan(py/px)*(360/(2*pi)); N1 = pz-150; N2 = px*cosd(theta1)+py*sind(theta1)-30; theta3 = acos((N1*N1+N2*N2-125*125-75*75)/ (2*125*75))*(360/(2*pi)); tempX=theta3; N3 = N2 - (75*N1*sind(theta3))/ (125+75*cosd(theta3)); N4 = (125^2+75^2+2*75*125*cosd(theta3))/ (125+75*cosd(theta3)); theta2 = asin(N3/N4)*(360/(2*pi))-90; tempY=theta2; %update len giao dien va simulink set_param([ModelName '/Slider Gain'],'Gain',num2str(theta1)); set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta2)); set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta3)); for t = 1:time theta1 = (rot/time)*t; set_param([ModelName '/Slider Gain'],'Gain',num2str(theta1)); end for t = 1:time t=t+1; theta1 = (rot/time)*t; theta2 = tempY; theta3 = tempX; 64 T1 = [ cosd(theta1) 30*cosd(theta1); sind(theta1) 30*sind(theta1); 0 sind(theta1) -cosd(theta1) T2 = [ cosd(theta2) 125*cosd(theta2); sind(theta2) 125*sind(theta2); 0 -sind(theta2) cosd(theta2) 0 0 T3 = [ cosd(theta3) -sind(theta3) 75*cosd(theta3); sind(theta3) cosd(theta3) 75*sind(theta3); 0 0 T=T1*T2*T3; x(t)=T(1,4); y(t)=T(2,4); z(t) = pz; axes(handles.axes5); plot3(x(:),y(:),z(:),'.r'); xlabel('x'); ylabel('y'); zlabel('z'); title('End point orbit'); end end 150; 1]; 0; 1]; 0 0; 1]; % - Executes on button press in pushbutton25 function pushbutton25_Callback(hObject, eventdata, handles) % hObject handle to pushbutton25 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) global theta1; 65 global theta2; global theta3; global t; ModelName = 'xeth'; button_line = get(handles.pushbutton25,'value'); if (button_line) xa = str2num(get(handles.editxa,'string')); ya = str2num(get(handles.editya,'string')); za = str2num(get(handles.editza,'string')); xb = str2num(get(handles.editxb,'string')); yb = str2num(get(handles.edityb,'string')); zb = str2num(get(handles.editzb,'string')); time = str2num(get(handles.edit_time,'string')); px = ((xb-xa)/time)*t+xa; py = ((yb-ya)/time)*t+ya; pz = ((zb-za)/time)*t+za; for t = 1:time px = ((xb-xa)/time)*t+xa; py = ((yb-ya)/time)*t+ya; pz = ((zb-za)/time)*t+za; theta1 = atan(py/px)*(360/(2*pi)); N1 = pz-150; N2 = px*cosd(theta1)+py*sind(theta1)-30; theta3 = acos((N1*N1+N2*N2-125*125-75*75)/ (2*125*75))*(360/(2*pi)); N3 = N2 - (75*N1*sind(theta3))/ (125+75*cosd(theta3)); N4 = (125^2+75^2+2*75*125*cosd(theta3))/ (125+75*cosd(theta3)); theta2 = asin(N3/N4)*(360/(2*pi))-90; %update len giao dien va simulink set_param([ModelName '/Slider Gain'],'Gain',num2str(theta1)); set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta2)); set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta3)); 66 end end for t = 1:time t=t+1; x(t) = ((xb-xa)/time)*t+xa; y(t) = ((yb-ya)/time)*t+ya; z(t) = ((zb-za)/time)*t+za; axes(handles.axes5); plot3(x(:),y(:),z(:),'.r'); xlabel('x'); ylabel('y'); zlabel('z'); title('End point orbit'); end function edit_time_Callback(hObject, eventdata, handles) % hObject handle to edit_time (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_time as text % str2double(get(hObject,'String')) returns contents of edit_time as a double % - Executes during object creation, after setting all properties function edit_time_CreateFcn(hObject, eventdata, handles) % hObject handle to edit_time (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 67 % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit_goc_Callback(hObject, eventdata, handles) % hObject handle to edit_goc (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: edit_goc % contents get(hObject,'String') returns contents of as text str2double(get(hObject,'String')) returns of edit_goc as a double % - Executes during object creation, after setting all properties function edit_goc_CreateFcn(hObject, eventdata, handles) % hObject handle to edit_goc (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 % - Executes during object creation, after setting all properties 68 function axes5_CreateFcn(hObject, eventdata, handles) % hObject handle to axes5 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: place code in OpeningFcn to populate axes5 % - Executes on button press in pushbutton_force function pushbutton_force_Callback(hObject, eventdata, handles) % hObject handle to pushbutton_force (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) global theta1a; global theta2a; global theta3a; global theta1b; global theta2b; global theta3b; global t; ModelName = 'xeth'; button_force = get(handles.pushbutton_force,'value'); if (button_force) xa = str2num(get(handles.editxa,'string')); ya = str2num(get(handles.editya,'string')); za = str2num(get(handles.editza,'string')); xb = str2num(get(handles.editxb,'string')); yb = str2num(get(handles.edityb,'string')); zb = str2num(get(handles.editzb,'string')); time = str2num(get(handles.edit_time,'string')); theta1a = atan(ya/xa)*(360/(2*pi)); N1a = za-150; N2a = xa*cosd(theta1a)+ya*sind(theta1a)-30; theta3a = acos((N1a*N1a+N2a*N2a-125*125-75*75)/ (2*125*75))*(360/(2*pi)); 69 N3a = N2a - (75*N1a*sind(theta3a))/ (125+75*cosd(theta3a)); N4a = (125^2+75^2+2*75*125*cosd(theta3a))/ (125+75*cosd(theta3a)); theta2a = asin(N3a/N4a)*(360/(2*pi))-90; theta1b = atan(yb/xb)*(360/(2*pi)); N1b = zb-150; N2b = xb*cosd(theta1b)+yb*sind(theta1b)-30; theta3b = acos((N1b*N1b+N2b*N2b-125*125-75*75)/ (2*125*75))*(360/(2*pi)); N3b = N2b - (75*N1b*sind(theta3b))/ (125+75*cosd(theta3b)); N4b = (125^2+75^2+2*75*125*cosd(theta3b))/ (125+75*cosd(theta3b)); theta2b = asin(N3b/N4b)*(360/(2*pi))-90; theta1 = theta1b-theta1a; theta2 = theta2b-theta2a; theta3 = theta3b-theta3a; t = [1:0.01:time] theta1 = ((theta1b-theta1a)/time)*t; theta2 = ((theta1b-theta1a)/time)*t; theta3 = ((theta1b-theta1a)/time)*t; axes(handles.axes3); y = (19291125*sin(2*((theta1b-theta1a)/time)*t + ((theta1b-theta1a)/time)*t))/4 + 15916250*sin(2*((theta1b-theta1a)/time)*t) + (11574675*sin(2*((theta1b-theta1a)/time)*t+ 2*((theta1b-theta1a)/time)*t))/8 + (2314935*sin(((theta1b-theta1a)/time)*t + ((theta1btheta1a)/time)*t))/2 + (4590351421219537*cos(theta2))/17179869184 + 7639800*sin(((theta1b-theta1a)/time)*t); plot(t,y,'.r'); z = (19291125*sin(2*((theta1b-theta1a)/time)*t + ((theta1b-theta1a)/time)*t))/8 + (11574675*sin(2*((theta1b-theta1a)/time)*t + 2*((theta1b-theta1a)/time)*t))/8 + (2314935*sin(((theta1b-theta1a)/time)*t + ((theta1btheta1a)/time)*t))/2 + (93972753829289*cos(((theta1b- 70 theta1a)/time)*t))/2147483648 + (19291125*sin(((theta1b-theta1a)/time)*t))/8; hold on; plot(t,z,'.g'); hold off; ylabel('M(Nmm)'); xlabel('t(s)'); end 71