Ben Katz / Mbed 2 deprecated Pendulum_demo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Eigen

Dynamics/dynamics.cpp@0:4dd2d995f7d0, 2019-05-07 (annotated)

Committer:
benkatz
Date:
Tue May 07 01:56:53 2019 +0000
Revision:
0:4dd2d995f7d0
works with the python side control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:4dd2d995f7d0 1 #include "dynamics.h"
benkatz 0:4dd2d995f7d0 2
benkatz 0:4dd2d995f7d0 3
benkatz 0:4dd2d995f7d0 4 void integrate_state(StateStruct * state, float torque, float dt)
benkatz 0:4dd2d995f7d0 5 {
benkatz 0:4dd2d995f7d0 6 float s1 = sin(state->q[1]);
benkatz 0:4dd2d995f7d0 7 float c1 = cos(state->q[1]);
benkatz 0:4dd2d995f7d0 8 // Calculate Mass Matrix //
benkatz 0:4dd2d995f7d0 9 state->h << J0 + M1*L0*L0 + L1*L1*M1*s1*s1, L0*L1*M1*c1,
benkatz 0:4dd2d995f7d0 10 L0*L1*M1*c1, J1 + M1*L1*L1;
benkatz 0:4dd2d995f7d0 11 // Calculate Coriolis Matrix //
benkatz 0:4dd2d995f7d0 12 state->c << 2.0f*L1*L1*M1*s1*c1*state->qd[1] + B0, -L0*L1*M1*s1*state->qd[1],
benkatz 0:4dd2d995f7d0 13 -L1*L1*M1*s1*c1*state->qd[0], B1;
benkatz 0:4dd2d995f7d0 14 // Gravity Vector //
benkatz 0:4dd2d995f7d0 15 state->g << 0,
benkatz 0:4dd2d995f7d0 16 -G*L1*M1*sin(state->q[1]);
benkatz 0:4dd2d995f7d0 17 // Torques //
benkatz 0:4dd2d995f7d0 18 state->tau << torque,0;
benkatz 0:4dd2d995f7d0 19
benkatz 0:4dd2d995f7d0 20 // Solve for Accelerations //
benkatz 0:4dd2d995f7d0 21 Vector2f RHS = state->tau- state->g - state->c*state->qd;
benkatz 0:4dd2d995f7d0 22 //state->qdd = state->h.inverse()*RHS;
benkatz 0:4dd2d995f7d0 23 //state->qdd = state->h.colPivHouseholderQr().solve(RHS);
benkatz 0:4dd2d995f7d0 24 Matrix2f h_inv = state->h.inverse(); // For 2x2, inverse is actually faster
benkatz 0:4dd2d995f7d0 25 state->qdd = h_inv*RHS;
benkatz 0:4dd2d995f7d0 26 // Integrate //
benkatz 0:4dd2d995f7d0 27 state->qd = state->qd + state->qdd*dt;
benkatz 0:4dd2d995f7d0 28 state->q = state->q + state->qd*dt;
benkatz 0:4dd2d995f7d0 29
benkatz 0:4dd2d995f7d0 30 }
benkatz 0:4dd2d995f7d0 31
benkatz 0:4dd2d995f7d0 32
benkatz 0:4dd2d995f7d0 33 void update_sensors(StateStruct state, SensorStruct * sensors)
benkatz 0:4dd2d995f7d0 34 {
benkatz 0:4dd2d995f7d0 35 float PI2 = 2.0f*PI;
benkatz 0:4dd2d995f7d0 36 float off1 = 0;
benkatz 0:4dd2d995f7d0 37 float off2 = 0;
benkatz 0:4dd2d995f7d0 38 if(state.q[0] < 0){off1 = PI2*(((int)(-state.q[0]/PI2)) + 1);}
benkatz 0:4dd2d995f7d0 39 if(state.q[1] < 0){off2 = PI2*(((int)(-state.q[1]/PI2)) + 1);}
benkatz 0:4dd2d995f7d0 40
benkatz 0:4dd2d995f7d0 41 sensors->encoder_count[0] = (int)((state.q[0]+off1) * (float)B_RES * 0.15915494309f)%B_RES;
benkatz 0:4dd2d995f7d0 42 sensors->encoder_count[1] = (int)((state.q[1]+off2) * (float)P_RES * 0.15915494309f)%P_RES;
benkatz 0:4dd2d995f7d0 43 sensors->q_sensed[0] = state.q[0];
benkatz 0:4dd2d995f7d0 44 sensors->q_sensed[1] = state.q[1];
benkatz 0:4dd2d995f7d0 45 sensors->qd_sensed[0] = state.qd[0];
benkatz 0:4dd2d995f7d0 46 sensors->qd_sensed[1] = state.qd[1];
benkatz 0:4dd2d995f7d0 47
benkatz 0:4dd2d995f7d0 48 }