File content as of revision 0:4dd2d995f7d0:

#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];
    
}