BeginPackage["Driver`"]Driver::usage = "Driver[xA,yA,AB,phi,omega,alpha] computes the driver link position, velocity and acceleration vectors."Begin["`Private`"]Driver[xA_,yA_,AB_,phi_,omega_,alpha_]:=Block[ { xB, yB ,vBx, vBy, aBx, aBy },xB = xA + AB Cos[phi] ;yB = yA + AB Sin[phi] ;vBx = - AB omega Sin[phi] ;vBy = AB omega Cos[phi] ;aBx = - AB omega^2 Cos[phi] - AB alpha Sin[phi] ;aBy = - AB omega^2 Sin[phi] + AB alpha Cos[phi] ;Return[ { xB, yB, vBx, vBy, aBx, aBy } ] ;]End[ ] EndPackage[ ]Driver`Driver@xA,yA,AB,phi,omega,alphaD computesthe driver link position, velocity and acceleration vectors.Driver`Private`Driver`Private`Program 1: Driver.m 1(* Driver mechanism *)Apply[Clear,Names["Global`*"]] ;Off[General::spell];Off[General::spell1];<<Driver.m ; (* Input data *)AB = 0.20 ; (* m *)phi = Pi/6 ; (* rad *)omega = 5. ; (* rad/s *)alpha = 0. ; (* rad/s^2 *)(* Position of joint A *)xA = yA = 0 ;(* Position of joint B *)xB = Driver[xA,yA,AB,phi,omega,alpha][[1]] ;yB = Driver[xA,yA,AB,phi,omega,alpha][[2]] ;(* Velocity of joint B *)vBx = Driver[xA,yA,AB,phi,omega,alpha][[3]] ;vBy = Driver[xA,yA,AB,phi,omega,alpha][[4]] ;(* Acceleration of joint B *)aBx = Driver[xA,yA,AB,phi,omega,alpha][[5]] ;aBy = Driver[xA,yA,AB,phi,omega,alpha][[6]] ;Print["rB = ",{xB,yB,0}," [m]"];Print["vB = ",{vBx,vBy,0}," [m/s]"];Print["aB = ",{aBx,aBy,0}," [m/s^2]"];rB = 80.173205, 0.1, 0< @mDvB = 8-0.5, 0.866025, 0< @mêsDaB = 8-4.33013, -2.5, 0< @mês^2DProgram2.nb 1BeginPackage["Position`"]PosRRR::usage = "PosRRR[xM,yM,xN,yN,MP,NP] computes the position vectors for RRR dyad"Begin["`Private`"]PosRRR[xM_,yM_,xN_,yN_,MP_,NP_]:=Block[ { xPSol, yPSol, xP1, yP1, xP2, yP2, eqRRR1, eqRRR2, solRRR },eqRRR1 = (xM-xPSol)^2 + (yM-yPSol)^2 == MP^2 ;eqRRR2 = (xN-xPSol)^2 + (yN-yPSol)^2 == NP^2 ;solRRR = Solve[ { eqRRR1 , eqRRR2 }, { xPSol, yPSol } ] ;xP1 = xPSol/.solRRR[[1]] ;yP1 = yPSol/.solRRR[[1]] ;xP2 = xPSol/.solRRR[[2]] ;yP2 = yPSol/.solRRR[[2]] ;Return[ { xP1, yP1, xP2, yP2 } ] ;]End[ ] (* PosRRR *)PosRRT::usage = "PosRRT[xM,yM,xN,yN,MP,theta] computes the position vectors for RRT \dyad"Begin["`Private`"]PosRRT[xM_,yM_,xN_,yN_,MP_,theta_]:=Block[ { xPSol, yPSol, xP1, yP1, xP2, yP2, eqRRT, solRRT, eqRRT1, eqRRT2 },If[ (theta==Pi/2)||(theta==3*Pi/2), xP1 = xP2 = xN ; eqRRT = (xM-xN)^2 + (yM-yPSol)^2 == MP^2 ; solRRT = Solve[ eqRRT, yPSol ] ; yP1 = yPSol/.solRRT[[1]] ; yP2 = yPSol/.solRRT[[2]] , eqRRT1 = (xM-xPSol)^2 + (yM-yPSol)^2 == MP^2 ; eqRRT2 = Tan[theta] == (yPSol-yN)/(xPSol-xN) ; solRRT = Solve[ { eqRRT1 , eqRRT2 }, { xPSol , yPSol } ] ; xP1 = xPSol/.solRRT[[1]] ; yP1 = yPSol/.solRRT[[1]] ; xP2 = xPSol/.solRRT[[2]] ; yP2 = yPSol/.solRRT[[2]] ;] ;Return[ { xP1, yP1, xP2, yP2 } ] ;]End[ ] (* PosRRT *)EndPackage[ ]Program 3: Position.m 1BeginPackage["Position`"]PosRRR::usage = "PosRRR[xM,yM,xN,yN,MP,NP] computes the position vectors for RRR dyad"Begin["`Private`"]PosRRR[xM_,yM_,xN_,yN_,MP_,NP_]:=Block[ { xPSol, yPSol, xP1, yP1, xP2, yP2, eqRRR1, eqRRR2, solRRR },eqRRR1 = (xM-xPSol)^2 + (yM-yPSol)^2 == MP^2 ;eqRRR2 = (xN-xPSol)^2 + (yN-yPSol)^2 == NP^2 ;solRRR = Solve[ { eqRRR1 , eqRRR2 }, { xPSol, yPSol } ] ;xP1 = xPSol/.solRRR[[1]] ;yP1 = yPSol/.solRRR[[1]] ;xP2 = xPSol/.solRRR[[2]] ;yP2 = yPSol/.solRRR[[2]] ;Return[ { xP1, yP1, xP2, yP2 } ] ;]End[ ] (* PosRRR *)PosRRT::usage = "PosRRT[xM,yM,xN,yN,MP,theta] computes the position vectors for RRT \dyad"Begin["`Private`"]PosRRT[xM_,yM_,xN_,yN_,MP_,theta_]:=Block[ { xPSol, yPSol, xP1, yP1, xP2, yP2, eqRRT, solRRT, eqRRT1, eqRRT2 },If[ (theta==Pi/2)||(theta==3*Pi/2), xP1 = xP2 = xN ; eqRRT = (xM-xN)^2 + (yM-yPSol)^2 == MP^2 ; solRRT = Solve[ eqRRT, yPSol ] ; yP1 = yPSol/.solRRT[[1]] ; yP2 = yPSol/.solRRT[[2]] , eqRRT1 = (xM-xPSol)^2 + (yM-yPSol)^2 == MP^2 ; eqRRT2 = Tan[theta] == (yPSol-yN)/(xPSol-xN) ; solRRT = Solve[ { eqRRT1 , eqRRT2 }, { xPSol , yPSol } ] ; xP1 = xPSol/.solRRT[[1]] ; yP1 = yPSol/.solRRT[[1]] ; xP2 = xPSol/.solRRT[[2]] ; yP2 = yPSol/.solRRT[[2]] ;] ;Return[ { xP1, yP1, xP2, yP2 } ] ;]End[ ] (* PosRRT *)EndPackage[ ]Position`PosRRR@xM,yM,xN,yN,MP,NPD computes the position vectors for RRR dyadPosition`Private`Position`Private`PosRRT@xM,yM,xN,yN,MP,thetaD computes the position vectors for RRT dyadPosition`Private`Position`Private`Program 3: Position.m 2(* R-RTR-RRT mechanism *)Apply[Clear,Names["Global`*"]] ;Off[General::spell];Off[General::spell1];<<Driver.m ;<<Position.m ;(* Input data *)AB = 0.20 ; (* m *)AD = 0.40 ; (* m *)CD = 0.70 ; (* m *)CE = 0.30 ; (* m *)yE = 0.35 ; (* m *)phi = Pi/4 ; (* rad *)omega = 5. ; (* rad/s *)alpha = 0. ; (* rad/s^2 *)(* Position Vectors *)xA = yA = 0 ;xB = Driver[xA,yA,AB,phi,omega,alpha][[1]] ;yB = Driver[xA,yA,AB,phi,omega,alpha][[2]] ;xD = 0 ;yD = -AD ;phi3 = ArcTan[(yB-yD)/(xB-xD)] ;xC = xD + CD Cos[phi3] ;yC = yD + CD Sin[phi3] ;phi5 = Pi ;(* xP=0; yP=yE; *)xE1 = PosRRT[xC,yC,0,yE,CE,phi5][[1]] ;xE2 = PosRRT[xC,yC,0,yE,CE,phi5][[3]] ;(* Choose the correct solution *)If[ (xE1<xC), xE=xE1, xE=xE2 ] ;Print["rB = ",{xB,yB,0}," [m]" ];Print["phi3 = ",phi3*180/N[Pi]," [deg]"];Print["rC = ",{xC,yC,0}," [m]" ];Print["rE = ",{xE,yE,0}," [m]" ];rB = 80.141421, 0.141421, 0< @mDphi3 = 75.3612 @degDrC = 80.176907, 0.277277, 0< @mDrE = 8-0.114145, 0.35, 0< @mDProgram4.nb 1In[220]:=(* R-RRR-RRT mechanism *)Apply [ Clear , Names [ "Global`*" ] ] ;Off[General::spell];Off[General::spell1];<<Driver.m ;<<Position.m ;(* Input data *)AB = 0.15 ; (* m *)BC = 0.40 ; (* m *)CD = 0.37 ; (* m *)CE = 0.23 ; (* m *)EF = CE ; (* m *)La = 0.30 ; (* m *)Lb = 0.45 ; (* m *)Lc = CD ; (* m *)phi = Pi/4 ; (* rad *)omega = N[Pi] ; (* rad/s *)alpha = 0. ; (* rad/s^2 *)(* Position Vectors *)xA = yA = 0 ;xB = Driver[xA,yA,AB,phi,omega,alpha][[1]] ;yB = Driver[xA,yA,AB,phi,omega,alpha][[2]] ;xD = La ;yD = Lb ;xC1 = PosRRR[xB,yB,xD,yD,BC,CD][[1]] ;yC1 = PosRRR[xB,yB,xD,yD,BC,CD][[2]] ;xC2 = PosRRR[xB,yB,xD,yD,BC,CD][[3]] ;yC2 = PosRRR[xB,yB,xD,yD,BC,CD][[4]] ;(* Choose the correct solution *)If[ (yC1>yB), xC=xC1;yC=yC1, xC=xC2;yC=yC2 ] ;phi3 = ArcTan[(yC-yD)/(xC-xD)] + Pi ;xE = xC + CE Cos[phi3] ;yE = yC + CE Sin[phi3] ;xF = - Lc ;phi5 = Pi/2 ;yF1 = PosRRT[xE,yE,-Lc,0,EF,phi5][[2]] ;yF2 = PosRRT[xE,yE,-Lc,0,EF,phi5][[4]] ;(* Choose the correct solution *)If[ (yF1<yE), yF=yF1, yF=yF2 ] ;Print["Positions"];Print["rB = ",{xB,yB,0}," [m]" ];Print["rC = ",{xC,yC,0}," [m]" ];Print["rE = ",{xE,yE,0}," [m]" ];Print["rF = ",{xF,yF,0}," [m]" ];phi2 =
View Full Document