bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: Controls/Kinmatics.cpp
- Revision:
- 16:5b19be27f08a
- Parent:
- 11:711d3c207e8c
diff -r d88f10b3b5f8 -r 5b19be27f08a Controls/Kinmatics.cpp --- a/Controls/Kinmatics.cpp Thu Dec 10 09:46:56 2015 +0000 +++ b/Controls/Kinmatics.cpp Thu Dec 10 10:55:35 2015 +0000 @@ -1,5 +1,6 @@ #include "Kinematics.h" #include <math.h> +#define M_PI 3.14159265358979323846 void getMassMatrix(float A[2][2], volatile float z[4], float p[10]){ @@ -46,6 +47,22 @@ velocity[1] = dth1*(l2*t4+l1*sin(th1))+dth2*l2*t4; } +void getGripperJacobianTranspose(float Jtrans[2][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; + float t3 = cos(t2); + float t4 = l2*t3; + float t5 = sin(t2); + float t6 = l2*t5; + Jtrans[0][0] = t4+l1*cos(th1); + Jtrans[0][1] = t4; + Jtrans[1][0] = t6+l1*sin(th1); + Jtrans[1][1] = t6; +} + float getEnergy(volatile float z[4], float p[10]){ float I1 = p[6]; float I2 = p[7]; @@ -93,4 +110,13 @@ float t2 = sin(th2); output[0] = -c2*dth2*l1*m2*t2*(dth1*2.0+dth2); output[1] = c2*dth1*dth1*l1*m2*t2; +} + +float boundTheta(float theta){ + int numTurns = fix(theta/(2*M_PI)); + return theta-numTurns*2*M_PI; +} + +int fix(float val){//round toward zero + return val > 0 ? floor(val) : ceil(val); } \ No newline at end of file