bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: Controls/Kinmatics.cpp
- Revision:
- 11:711d3c207e8c
- Parent:
- 10:769cc457c3a4
- Child:
- 16:5b19be27f08a
--- a/Controls/Kinmatics.cpp Sat Dec 05 09:04:23 2015 +0000 +++ b/Controls/Kinmatics.cpp Tue Dec 08 22:52:46 2015 +0000 @@ -11,19 +11,39 @@ float m1 = p[4]; float m2 = p[5]; float th2 = z[1]; - float t2 = pow(c2,2); + float t2 = c2*c2; float t3 = m2*t2; float t4 = cos(th2); float t5 = c2*l1*m2*t4; float t6 = t3+t5; - A[0][0] = I1+I2+t3+pow(c1,2)*m1+pow(l1,2)*m2+c2*l1*m2*t4*2.0; + A[0][0] = I1+I2+t3+c1*c1*m1+l1*l1*m2+c2*l1*m2*t4*2.0; A[0][1] = t6; A[1][0] = t6; A[1][1] = I2+t3; } -float getPositionOfFoot(){ - return 0; +void getGripperPosition(float position[2], volatile float z[4], float p[10]){ + float l1 = p[0]; + float l2 = p[1]; + float th1 = z[0]; + float th2 = z[1]; + float t2 = th1+th2; + position[0] = l2*sin(t2)+l1*sin(th1); + position[1] = -l2*cos(t2)-l1*cos(th1); +} + +void getGripperVelocity(float velocity[2], volatile float z[4], float p[10]){ + float dth1 = z[2]; + float dth2 = z[3]; + float l1 = p[0]; + float l2 = p[1]; + float th1 = z[0]; + float th2 = z[1]; + float t2 = th1+th2; + float t3 = cos(t2); + float t4 = sin(t2); + velocity[0] = dth1*(l2*t3+l1*cos(th1))+dth2*l2*t3; + velocity[1] = dth1*(l2*t4+l1*sin(th1))+dth2*l2*t4; } float getEnergy(volatile float z[4], float p[10]){ @@ -39,36 +59,38 @@ float m2 = p[5]; float th1 = z[0]; float th2 = z[1]; - float t2 = pow(dth1,2); - float t3 = pow(c2,2); - float t4 = pow(dth2,2); + float t2 = dth1*dth1; + float t3 = c2*c2; + float t4 = dth2*dth2; float t5 = cos(th1); float t6 = cos(th2); - return I1*t2*(1.0/2.0)+I2*t2*(1.0/2.0)+I2*t4*(1.0/2.0)+pow(c1,2)*m1*t2*(1.0/2.0)+pow(l1,2)*m2*t2*(1.0/2.0)-g*m2*(l1*t5+c2*cos(th1+th2))+m2*t2*t3*(1.0/2.0)+m2*t3*t4*(1.0/2.0)+dth1*dth2*m2*t3-c1*g*m1*t5+c2*l1*m2*t2*t6+c2*dth1*dth2*l1*m2*t6; + return I1*t2*(1.0/2.0)+I2*t2*(1.0/2.0)+I2*t4*(1.0/2.0)+c1*c1*m1*t2*(1.0/2.0)+l1*l1*m2*t2*(1.0/2.0)-g*m2*(l1*t5+c2*cos(th1+th2))+m2*t2*t3*(1.0/2.0)+m2*t3*t4*(1.0/2.0)+dth1*dth2*m2*t3-c1*g*m1*t5+c2*l1*m2*t2*t6+c2*dth1*dth2*l1*m2*t6; } -float getGravity(volatile float z[4], float p[10]){ -// c1 = p(3,:); +void getGravity(float output[2], volatile float z[4], float p[10]){ + float c1 = p[2]; float c2 = p[3]; float g = p[8]; -// l1 = p(1,:); -// m1 = p(5,:); + float l1 = p[0]; + float m1 = p[4]; float m2 = p[5]; float th1 = z[0]; float th2 = z[1]; -// t2 = sin(th1); + float t2 = sin(th1); float t3 = th1+th2; float t4 = sin(t3); - return c2*g*m2*t4; + output[0] = g*m2*(c2*t4+l1*t2)+c1*g*m1*t2; + output[1] = c2*g*m2*t4; } -float getCoriolis(volatile float z[4], float p[10]){ +void getCoriolisCentrip(float output[2], volatile float z[4], float p[10]){ float c2 = p[3]; float dth1 = z[2]; -// float dth2 = z[3]; + float dth2 = z[3]; float l1 = p[0]; float m2 = p[5]; float th2 = z[1]; float t2 = sin(th2); - return c2*pow(dth1,2)*l1*m2*t2; + output[0] = -c2*dth2*l1*m2*t2*(dth1*2.0+dth2); + output[1] = c2*dth1*dth1*l1*m2*t2; } \ No newline at end of file