D ı a vào nhng l SFkVª này ta xây d ı ng ng d Ø ng cho bài mô phng chuy ˙Q ng v i các hàm ph Øc vØ nh ng mØFtFKF Ø th˙.
+uQK6 [ gi+i thu5t l5p trnh chuyKQ ang coa robot
~QJ ~QJ Sai .KªL W¥RFiFWKDPVQJKF FºDURERWW¥LWKÆLL˙PW 9` URERWW¥LWKÆLL˙P«X %–WTimer Nút Run Linear/ 5XQ6SOLQH mF'Q 7QJ t+=0.001 TínhWDWKkQYjFiFELˆQNKS 9` URERWW¥LWKÆLL˙PNˆWLˆS t = T (s) ’QJ Sai
Mô ph2•‰¯7ng h1…”‘„‘–¯‹„ 7 hai chân
74
Chuy˙Q ng cº DURERWmc l–p trình theo hai quy lu–t chuy˙Q ng. M t là robot chuy˙Q QJ ¯u WKHRFiFR¥n quß ¥o th•ng n i li¯n KDLL˙m. Hai là robot Emc chuy˙Q QJ ¯u WKHRFiFR¥n quß ¥o th•ng n i li¯n KDLL˙m. Hai là robot Emc
L WKHRmÆng cong spline b–FLTXDFiFL ˙m mm F[iF ˇnh7Um c hˆt, các tham s ng h c cº DURERWmc khª i t¥o t¥i thÆLL˙P «u (t = 0) và tr¥ng thái cºa tham s ng h c cº DURERWmc khª i t¥o t¥i thÆLL˙P «u (t = 0) và tr¥ng thái cºa robot t¥i thÆLL˙m «u s` m c v`.
// Tham số hằng số cấu trúc robot d = 50; a2 = 40; a3 = 320; a4 = 350; a5 = 80; l1 = 55; l2 = 195; Pi=3.145926535897932384626433832795; // Thời gian tại mỗi thời điểm
t = 0; Tc = 0.6; T1 = 0.05; Tm = 0.15; T2 = Tm-T1; // Khởi tạo giá trị tham số của robot đi bộ hai chân s = 400;
alpha = 15*Pi/180; // alpha = 15 độ
// Khởi tạo giá trị biến khớp tại thời điểm đầu m_theta11 = 0.0; m_theta12 = 0.0; m_theta13 = 0.0; m_theta14 = 0.0; m_theta15 = 90; m_theta21 = 0.0; m_theta22 = 0.0; m_theta23 = 0.0; m_theta24 = 0.0; m_theta25 = 90; // Vị trí thân yT0 = s/2; yT1 = yT0+50; yT2 = yT0+200; yT3 = 3*s/2; yT4 = 3*s/2+50; yT5 = 3*s/2+200; yT6 = 5*s/2; zT0 = 655+a2+d; zT1 = 660+a2+d; zT2 = 700+a2+d; zT3 = zT0; zT4 = zT1; zT5 = zT2; zT6 = zT0; // vị trí cổ chân phải yR0 = 0; yR1 = l2*(1-cos(alpha2))+a5*sin(alpha2); yR2 = 200; yR3 = 2*s-l1*(1-cos(alpha1))-a5*sin(alpha1); yR4 = 2*s; yR5 = 2*s; yR6 = 2*s;
Mô ph2•‰¯7ng h1…”‘„‘–¯‹„ 7 hai chân75 75 zR0 = a5; zR1 = l2*sin(alpha2)+a5*cos(alpha2); zR2 = 200; zR3 = l1*sin(alpha1)+a5*cos(alpha1); zR4 = a5; zR5 = a5; zR6 = a5; // vị trí cổ chân trái yL0 = s-l1*(1-cos(alpha1))-a5*sin(alpha1); yL1 = s; yL2 = s; yL3 = s; yL4 = s+l2*(1-cos(alpha2))+a5*sin(alpha2); yL5 = 600; yL6 = 3*s-l1*(1-cos(alpha1))-a5*sin(alpha1); zL0 = l1*sin(alpha1)+a5*cos(alpha1); zL1 = a5; zL2 = a5; zL3 = a5; zL4 = l2*sin(alpha2)+a5*cos(alpha2); zL5 = 200; zL6 = l1*sin(alpha1)+a5*cos(alpha1);
T D thân và các góc khSm c tính toán trong hàm CalculateLinear() và hàm CalculateSpline(). Hàm CalculateLinear() cho phép tính tD thân và các giá trˇ CalculateSpline(). Hàm CalculateLinear() cho phép tính tD thân và các giá trˇ
cºa góc kh p v i quy lu–t chuy˙Q QJ ¯XWKHRFiFR¥n th•ng n LKDLL˙Pm[iFˇnh. Hàm CalulateSpline() cho phép tính tD thân và các giá trˇ góc kh p v i ˇnh. Hàm CalulateSpline() cho phép tính tD thân và các giá trˇ góc kh p v i quy lu–t chuy˙Q QJWKHmÆng cong Spline b–c 3.
void CCTRPANEL::CalculateLinear(void) {
// Đoạn quỹ đạo 1 (0 ≤ t <T1)
if ((t>= 0) && (t< T1)) { m_yThan = yT0+(yT1-yT0)*t/T1; m_zThan = zT0+(zT1-zT0)*t/T1; m_theta13 = p30+(p31-p30)*t/T1; m_theta14 = p40+(p41-p40)*t/T1; m_theta15 = p50+(p51-p50)*t/T1; m_theta23 = q30+(q31-q30)*t/T1; m_theta24 = q40+(q41-q40)*t/T1; m_theta25 = q50+(q51-q50)*t/T1; }
// Đoạn quỹ đạo 2 (T1 ≤ t <Tm)
else if ((t>= T1) && (t< Tm)) { m_yThan = yT1+(yT2-yT1)*(t-T1)/(Tm-T1); m_zThan = zT1+(zT2-zT1)*(t-T1)/(Tm-T1); m_theta13 = p31+(p32-p31)*(t-T1)/(Tm-T1); m_theta14 = p41+(p42-p41)*(t-T1)/(Tm-T1); m_theta15 = p51+(p52-p51)*(t-T1)/(Tm-T1); m_theta23 = q31+(q32-q31)*(t-T1)/(Tm-T1); m_theta24 = q41+(q42-q41)*(t-T1)/(Tm-T1); m_theta25 = q51+(q52-q51)*(t-T1)/(Tm-T1); }
// Đoạn quỹ đạo 3 (Tm ≤ t <Tc/2)
else if ((t>= Tm) && (t< Tc/2))
Mô ph2•‰¯7ng h1…”‘„‘–¯‹„ 7 hai chân76 76 m_zThan = zT2+(zT3-zT2)*(t-Tm)/(Tc/2-Tm); m_theta13 = p32+(p33-p32)*(t-Tm)/(Tc/2-Tm); m_theta14 = p42+(p43-p42)*(t-Tm)/(Tc/2-Tm); m_theta15 = p52+(p53-p52)*(t-Tm)/(Tc/2-Tm); m_theta23 = q32+(q33-q32)*(t-Tm)/(Tc/2-Tm); m_theta24 = q42+(q43-q42)*(t-Tm)/(Tc/2-Tm); m_theta25 = q52+(q53-q52)*(t-Tm)/(Tc/2-Tm); }
// Đoạn quỹ đạo 4 (Tc/2 ≤ t <Tc/2+T1)
else if ((t>= Tc/2) && (t< Tc/2+T1)) { m_yThan = yT3+(yT4-yT3)*(t-Tc/2)/T1; m_zThan = zT3+(zT4-zT3)*(t-Tc/2)/T1; m_theta13 = p33+(p34-p33)*(t-Tc/2)/T1; m_theta14 = p43+(p44-p43)*(t-Tc/2)/T1; m_theta15 = p53+(p54-p53)*(t-Tc/2)/T1; m_theta23 = q33+(q34-q33)*(t-Tc/2)/T1; m_theta24 = q43+(q44-q43)*(t-Tc/2)/T1; m_theta25 = q53+(q54-q53)*(t-Tc/2)/T1; }
// Đoạn quỹ đạo 5 (Tc/2 +T1 ≤ t <Tc/2+Tm)
else if ((t>= Tc/2+T1) && (t< Tc/2+Tm)) { m_yThan = yT4+(yT5-yT4)*(t-Tc/2-T1)/(Tm-T1); m_zThan = zT4+(zT5-zT4)*(t-Tc/2-T1)/(Tm-T1); m_theta13 = p34+(p35-p34)*(t-Tc/2-T1)/(Tm-T1); m_theta14 = p44+(p45-p44)*(t-Tc/2-T1)/(Tm-T1); m_theta15 = p54+(p55-p54)*(t-Tc/2-T1)/(Tm-T1); m_theta23 = q34+(q35-q34)*(t-Tc/2-T1)/(Tm-T1); m_theta24 = q44+(q45-q44)*(t-Tc/2-T1)/(Tm-T1); m_theta25 = q54+(q55-q54)*(t-Tc/2-T1)/(Tm-T1); }
// Đoạn quỹ đạo 6 (Tc/2 +Tm ≤ t <=Tc)
else if ((t>= Tc/2+Tm) && (t<= Tc)) { m_yThan = yT5+(yT6-yT5)*(t-Tc/2-Tm)/(Tc/2-Tm); m_zThan = zT5+(zT6-zT5)*(t-Tc/2-Tm)/(Tc/2-Tm); m_theta13 = p35+(p36-p35)*(t-Tc/2-Tm)/(Tc/2-Tm); m_theta14 = p45+(p46-p45)*(t-Tc/2-Tm)/(Tc/2-Tm); m_theta15 = p55+(p56-p55)*(t-Tc/2-Tm)/(Tc/2-Tm); m_theta23 = q35+(q36-q35)*(t-Tc/2-Tm)/(Tc/2-Tm); m_theta24 = q45+(q46-q45)*(t-Tc/2-Tm)/(Tc/2-Tm); m_theta25 = q55+(q56-q55)*(t-Tc/2-Tm)/(Tc/2-Tm); } }
void CCTRPANEL::CalculateSpline(void) // Đoạn quỹ đạo 1 (0 ≤ t <T1)
if ((t>= 0) && (t< T1)) { m_yThan = yT0+(yT1-yT0)*t/T1; m_zThan = 35044.64287000000330*pow(t,3)+12.3883928700000006*t+745.0000000; m_theta13 = p30+(p31-p30)*t/T1; m_theta14 = p40+(p41-p40)*t/T1; m_theta15 = p50+(p51-p50)*t/T1; m_theta23 = -4427.93194700000004*pow(t,3)+65.1823406400000067*t+6.713079035; m_theta24 = 5765.84843999999976*pow(t,3)-249.756261099999988*t-49.41631140; m_theta25 = q50+(q51-q50)*t/T1; }
// Đoạn quỹ đạo 2 (T1 ≤ t <Tm)
else if ((t>= T1) && (t< Tm)) { m_yThan = yT1+(yT2-yT1)*(t-T1)/(Tm-T1); m_zThan = -40089.2857300000032*pow((t-T1),3)+5256.69642857142844*pow((t- T1),2)+275.223214199999972*t+736.2388393; m_theta13 = p31+(p32-p31)*(t-T1)/(Tm-T1);
Mô ph2•‰¯7ng h1…”‘„‘–¯‹„ 7 hai chân77 77 m_theta14 = p41+(p42-p41)*(t-T1)/(Tm-T1); m_theta15 = p51+(p52-p51)*(t-T1)/(Tm-T1); m_theta23 = 8536.5333900000005*pow((t-T1),3)-664.189791751116104*pow((t- T1),2)+31.9728510600000000*t+7.820062021; m_theta24 = 2147.01539000000002*pow((t-T1),3)+864.877265967633890*pow((t- T1),2)-206.512397800000002*t-50.85777351; m_theta25 = q51+(q52-q51)*(t-T1)/(Tm-T1); }
// Đoạn quỹ đạo 3 (Tm ≤ t <Tc/2)
else if ((t>= Tm) && (t< Tc/2)) { m_yThan = yT2+(yT3-yT2)*(t-Tm)/(Tc/2-Tm); m_zThan = 26294.6428699999997*pow((t-Tm),3)-6770.08928571428532*pow((t- Tm),2)+123.883928699999998*t+771.4174107; m_theta13 = p32+(p33-p32)*(t-Tm)/(Tc/2-Tm); m_theta14 = p42+(p43-p42)*(t-Tm)/(Tc/2-Tm); m_theta15 = p52+(p53-p52)*(t-Tm)/(Tc/2-Tm); m_theta23 = -10123.5669199999993*pow((t-Tm),3)+1896.77022525334814*pow((t- Tm),2)+155.230894400000012*t-8.77400901; m_theta24 = -6707.27701700000034*pow((t-Tm),3)+1508.98188309709803*pow((t- Tm),2)+30.8735171000000008*t-75.66987270; m_theta25 = q52+(q53-q52)*(t-Tm)/(Tc/2-Tm); }
// Đoạn quỹ đạo 4 (Tc/2 ≤ t <Tc/2+T1)
else if ((t>= Tc/2) && (t< Tc/2+T1)) { m_yThan = yT3+(yT4-yT3)*(t-Tc/2)/T1; m_zThan = -8348.21428699999888*pow((t-Tc/2),3)+5062.4999999999991*pow((t- Tc/2),2)-132.254464299999995*t+784.6763393; m_theta13 = -12107.7614699999995*pow((t-Tc/2),3)+1238.4366386874996*pow((t- Tc/2),2)+28.3475717200000013*t-2.753030057; m_theta14 = 13758.852559999999*pow((t-Tc/2),3)-1285.94509369791650*pow((t- Tc/2),2)-190.099876700000010*t+9.28850214; m_theta15 = p53+(p54-p53)*(t-Tc/2)/T1; m_theta23 = q33+(q34-q33)*(t-Tc/2)/T1; m_theta24 = q43+(q44-q43)*(t-Tc/2)/T1; m_theta25 = q53+(q54-q53)*(t-Tc/2)/T1; }
// Đoạn quỹ đạo 5 (Tc/2 +T1 ≤ t <Tc/2+Tm)
else if ((t>= Tc/2+T1) && (t< Tc/2+Tm)) { m_yThan = yT4+(yT5-yT4)*(t-Tc/2-T1)/(Tm-T1); m_zThan = -29241.0714300000000*pow((t-T1-Tc/2),3)+3810.26785714285688*pow((t- T1-Tc/2),2)+311.383928599999990*t+641.0156250; m_theta13 = 5398.35703699999976*pow((t-T1-Tc/2),3)- 577.727580694196376*pow((t-T1-Tc/2),2)+61.3830246099999997*t-12.73281715; m_theta14 = 1474.08702500000004*pow((t-T1- Tc/2),3)+777.882790374628030*pow((t-T1-Tc/2),2)-215.502991900000012*t+16.68458629; m_theta15 = p54+(p55-p54)*(t-Tc/2-T1)/(Tm-T1); m_theta23 = q34+(q35-q34)*(t-Tc/2-T1)/(Tm-T1); m_theta24 = q44+(q45-q44)*(t-Tc/2-T1)/(Tm-T1); m_theta25 = q54+(q55-q54)*(t-Tc/2-T1)/(Tm-T1); }
// Đoạn quỹ đạo 6 (Tc/2 +Tm ≤ t <=Tc)
else if ((t>= Tc/2+Tm) && (t<= Tc)) { m_yThan = yT5+(yT6-yT5)*(t-Tc/2-Tm)/(Tc/2-Tm); m_zThan = 11026.7857100000001*pow((t-Tc/2-Tm),3)-4962.05357142857066*pow((t- Tc/2-Tm),2)+196.205356999999992*t+701.7075894; m_theta13 = -2315.06562200000008*pow((t-Tc/2- Tm),3)+1041.7795297388393*pow((t-Tc/2-Tm),2)+107.788219499999996*t-33.99407363;
Mô ph2•‰¯7ng h1…”‘„‘–¯‹„ 7 hai chân78 78 m_theta14 = -2711.35310699999991*pow((t-Tc/2- Tm),3)+1220.10889772507426*pow((t-Tc/2-Tm),2)-15.7038230999999994*t-63.97212474; m_theta15 = p55+(p56-p55)*(t-Tc/2-Tm)/(Tc/2-Tm); m_theta23 = q35+(q36-q35)*(t-Tc/2-Tm)/(Tc/2-Tm); m_theta24 = q45+(q46-q45)*(t-Tc/2-Tm)/(Tc/2-Tm); m_theta25 = q55+(q56-q55)*(t-Tc/2-Tm)/(Tc/2-Tm); } }
Trong hàm OnTimer(), cKmkQJWUuQKV` g i các hàm tính các tham s ng h c cºa robot CalculateLinear() ho»c CalculateSpline() và c–p nh–t tr¥ng thái cºa robot cºa robot CalculateLinear() ho»c CalculateSpline() và c–p nh–t tr¥ng thái cºa robot