Robotarm

Jag har problem med b-delen av uppgiften (se uppgiften längre ner). Jag har löst ut a1, a2,b1 och b2 så att theta beror på tiden, MEN del B som jag trodde skulle vara enkel sitter jag helt fast på. Det jag skulle vilja ha hjälp med är att hitta information/litteratur i hur jag får armen att röra sig enl då tiden har intervallet 0:0.1:4 (alt 0:4), men än mer vill jag veta hur jag lyckas plotta själva armen. Jag tänker att armen borde vara en vektor och har använt mig av 

syms x y
L_1=solve(abs(sqrt(y^2+x^2))==4)

för att jag tänkte att då får jag ju en ekvation för längden 4, men det fungerar inte i sammanhanget så jag tog bort det. Sen är ju frågan om jag nu ska följa ekvationen så borde det egentligen bara vara att sätta L1=4 och L2=3 vilket jag gjort nu, men det fungerade inte heller när jag skulle köra det. Gör jag något fel med funktionsfilerna?

Här kommer funktionsfiler: 

function [Q] =theta_1(t)

Q=10+0.3*t^3+0.4*t^4;

end

function [M] = theta_2(t)

M=20+0.3*t^3+(-0.7)*t^4;

end

Här kommer programmet:

clear all, close all, clc

L_1=4;
L_2=3;

[THETA_1,THETA_2] = meshgrid(@(t)theta_1,@(t)theta_2);
% genererar ett rutnät med värden för theta_1 och theta_2

x=L_1* cos(theta_1)+L_2* cos(theta_1+theta_2) %beräknar x coordinater

y=L_1*sin(theta_1)+L_2*sin(theta_1+theta_2) %beräknar y coordinater

data1 = [x(0:20) y(0:20) THETA_1(0:pi/2)]; % create x-y-theta1 dataset
data2 = [x(0:20) y(0:20) THETA_2(0:pi)]; % create x-y-theta2 dataset

plot(x(0:20),y(0:20),'r.');
axis equal;
xlabel('x','fontsize',10)
ylabel('y','fontsize',10)
title('X-Y coordinates generated for all theta1 and theta2 combinations using forward kinematics formula','fontsize',10)

Uppgift:

Figuren visar en robotarm med två länkar. Ledvinklarna ges av theta1 och theta2. Koordinaterna för
robothanden blir
x = L1 cos (theta1) + L2 cos (theta1 + theta2)

y = L1 sin (theta1) + L2 sin (theta1 + theta2)
där L1 och L2 är länklängderna.
Vinklarna theta1 och theta2 som bestämmer robothandens rörelse kontrolleras tidsmässigt av följande
polynomuttryck:
theta1(t) = theta1(0) + a1t3 + a2t4
theta2(t) = theta2(0) + b1t3 + b2t4
där theta1(0) och theta2(0) är startvärden för vinklarna vid tiden t = 0. Vinklarna anges i enheten grader
och tiden i sekunder.
Robothand

a )

Ställ upp ett linjärt ekvationssystem för bestämning av parametrarna a1, a2, b1 och b2, givet
startvärdena theta1(0) = 10, theta2(0) = 20 samt vinklarnas värden då t = 3: theta1(3) = 50,5, theta2(3) = -28,6
och då t = 4: theta1(4) = 131,6, theta2(4) = -140,0. Lös ekvationssystemet i MATLAB.

b)

Använd dina resultat i a) för att plotta robothandens svep när tiden t går från 0 till 4 sekunder,
med värdena L1 = 4 m och L2 = 3 m.