Diff: Dynamics/dynamics.cpp

Revision:

0:4dd2d995f7d0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Dynamics/dynamics.cpp	Tue May 07 01:56:53 2019 +0000
@@ -0,0 +1,48 @@
+#include "dynamics.h"
+
+         
+void integrate_state(StateStruct * state, float torque, float dt)
+{
+    float s1 = sin(state->q[1]);
+    float c1 = cos(state->q[1]);
+    // Calculate Mass Matrix //
+    state->h <<     J0 + M1*L0*L0 + L1*L1*M1*s1*s1,  L0*L1*M1*c1,
+                    L0*L1*M1*c1,                     J1 + M1*L1*L1;
+    // Calculate Coriolis Matrix //
+    state->c <<     2.0f*L1*L1*M1*s1*c1*state->qd[1] + B0,   -L0*L1*M1*s1*state->qd[1],
+                    -L1*L1*M1*s1*c1*state->qd[0],       B1;
+    // Gravity Vector //
+    state->g <<     0,
+                    -G*L1*M1*sin(state->q[1]);
+    // Torques //
+    state->tau <<  torque,0;
+    
+    // Solve for Accelerations //
+    Vector2f RHS = state->tau- state->g - state->c*state->qd;
+    //state->qdd = state->h.inverse()*RHS;
+    //state->qdd = state->h.colPivHouseholderQr().solve(RHS);
+    Matrix2f h_inv = state->h.inverse();    // For 2x2, inverse is actually faster
+    state->qdd = h_inv*RHS;
+    // Integrate //
+    state->qd = state->qd + state->qdd*dt;
+    state->q = state->q + state->qd*dt;
+    
+}
+
+
+void update_sensors(StateStruct state, SensorStruct * sensors)
+{
+    float PI2 = 2.0f*PI;
+    float off1 = 0;
+    float off2 = 0;
+    if(state.q[0] < 0){off1 = PI2*(((int)(-state.q[0]/PI2)) + 1);}
+    if(state.q[1] < 0){off2 = PI2*(((int)(-state.q[1]/PI2)) + 1);}
+    
+    sensors->encoder_count[0] = (int)((state.q[0]+off1) * (float)B_RES * 0.15915494309f)%B_RES;
+    sensors->encoder_count[1] = (int)((state.q[1]+off2) * (float)P_RES * 0.15915494309f)%P_RES;
+    sensors->q_sensed[0] = state.q[0];
+    sensors->q_sensed[1] = state.q[1];
+    sensors->qd_sensed[0] = state.qd[0];
+    sensors->qd_sensed[1] = state.qd[1];
+    
+}