Anke Post / Mbed 2 deprecated RKI

Matrices

Dependencies:   mbed FXOS8700Q

main.cpp

Committer:
AnkePost
Date:
2019-10-30
Revision:
1:b832c2220d45
Parent:
0:fb18afff5c9d
Child:
2:30749226b8bd

File content as of revision 1:b832c2220d45:


Joint_velocity_x[0][0] = (vx*(cos(q1+3.141592653589793/6.0)*-8.5E2-sin(q1)*4.25E2+cos(q1)*cos(q2)*2.25E2+cos(q1)*sin(q2)*6.77E2+cos(q2)*sin(q1)*6.77E2-sin(q1)*sin(q2)*2.25E2+sqrt(3.0)*cos(q1)*4.25E2-sqrt(3.0)*cos(q1)*cos(q2)*4.25E2+sqrt(3.0)*sin(q1)*sin(q2)*4.25E2)*(4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
Joint_velocity_x[1][0] = (vx*(sin(q1)*-4.25E2+cos(q1)*cos(q2)*2.25E2+cos(q1)*sin(q2)*6.77E2+cos(q2)*sin(q1)*6.77E2-sin(q1)*sin(q2)*2.25E2+sqrt(3.0)*cos(q1)*4.25E2-sqrt(3.0)*cos(q1)*cos(q2)*4.25E2+sqrt(3.0)*sin(q1)*sin(q2)*4.25E2)*(-4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);

Joint_velocity_y[0][0] = (vy*(cos(q1)*4.25E2-sin(q1+3.141592653589793/6.0)*8.5E2-cos(q1)*cos(q2)*6.77E2+cos(q1)*sin(q2)*2.25E2+cos(q2)*sin(q1)*2.25E2+sin(q1)*sin(q2)*6.77E2+sqrt(3.0)*sin(q1)*4.25E2-sqrt(3.0)*cos(q1)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*sin(q1)*4.25E2)*(4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);
Joint_velocity_y[1][0] = (vy*(cos(q1)*4.25E2-cos(q1)*cos(q2)*6.77E2+cos(q1)*sin(q2)*2.25E2+cos(q2)*sin(q1)*2.25E2+sin(q1)*sin(q2)*6.77E2+sqrt(3.0)*sin(q1)*4.25E2-sqrt(3.0)*cos(q1)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*sin(q1)*4.25E2)*(-4.0E1/1.7E1))/(cos(q1)*cos(q1+3.141592653589793/6.0)*4.25E2+sin(q1)*sin(q1+3.141592653589793/6.0)*4.25E2-cos(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*6.77E2-cos(q2)*sin(q1)*sin(q1+3.141592653589793/6.0)*6.77E2+cos(q1+3.141592653589793/6.0)*sin(q1)*sin(q2)*6.77E2+sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*2.25E2-sqrt(3.0)*cos(q1)*sin(q1+3.141592653589793/6.0)*4.25E2+sqrt(3.0)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-cos(q1)*cos(q2)*cos(q1+3.141592653589793/6.0)*6.77E2-cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*2.25E2+cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*2.25E2+cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*2.25E2+sqrt(3.0)*cos(q1)*cos(q2)*sin(q1+3.141592653589793/6.0)*4.25E2-sqrt(3.0)*cos(q1)*cos(q1+3.141592653589793/6.0)*sin(q2)*4.25E2-sqrt(3.0)*cos(q2)*cos(q1+3.141592653589793/6.0)*sin(q1)*4.25E2-sqrt(3.0)*sin(q1)*sin(q2)*sin(q1+3.141592653589793/6.0)*4.25E2);