Diff: Motion/OnlineMotion.cpp

Revision:

20:abb7852df747

Parent:

19:c2ec475367aa

Child:

21:a54bcab078ed

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion/OnlineMotion.cpp	Mon Aug 19 08:10:58 2013 +0000
@@ -0,0 +1,129 @@
+#include "OnlineMotion.h"
+
+//extern Ticker tick;
+extern DigitalOut led1;
+
+OnlineMotion::OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, PWM* _pwm, bool* _playing)
+    : pwm(_pwm), playing(_playing), t(0), T(_T), m_NUM_MAX(_size_num - 1), STEP(_TIMESTEP)
+{
+    *playing = true;
+}
+
+OnlineMotion::~OnlineMotion()
+{
+    led1 = !led1;
+}
+
+void OnlineMotion::step()
+{
+    if (t < T) {
+        update();
+    } else {
+        *playing = false;
+        //tick.detach();
+        //delete this;
+    }
+}
+
+void OnlineMotion::update()
+{
+    play_online();
+    t += STEP;
+}
+
+void OnlineMotion::invertkinematics(float* pos, uint16_t* theta)
+{
+    float L1 = 45.0;
+    float L2 = 90.0;
+    
+    float x = pos[0];
+    float y = pos[1];
+    float z = pos[2];
+    
+    float L = sqrt(z*z + y*y) - 2 * L1;
+    float h = sqrt(x*x + L*L);
+    
+    float alpha = atan(y / z);
+    float beta = acos(h / (2 * L2));
+    float gunmma = asin(x / h);
+    
+    theta[0] = alpha * RADTOVAL;
+    theta[1] = (beta - gunmma) * RADTOVAL;
+    theta[2] = 2 * beta * RADTOVAL;
+    theta[3] = (beta + gunmma) * RADTOVAL;
+}
+
+void OnlineMotion::walk(float* lpos, float* rpos, float h, float stride, float side, float up)
+{
+    if (t < T / 2) {
+        lpos[0] = stride - (4 * stride / T) * t;
+        rpos[0] = -stride + (4 * stride / T) * t;
+        if (t < T / 6) {
+            lpos[2] = h;
+        } else if (t < T / 3) {
+            lpos[2] = h - up * sin((t-T/6) * M_PI/(T/6));
+        } else {
+            lpos[2] = h;
+        }
+        rpos[2] = h;
+    } else {
+        lpos[0] = -stride + (4 * stride / T) * (t - T/2);
+        rpos[0] = stride - (4 * stride / T) * (t - T/2);
+        if (t < 4*T/6) {
+            rpos[2] = h;
+        } else if (t < 5*T/6) {
+            rpos[2] = h -up * sin((t-4*T/6) * M_PI/(T/6));
+        } else {
+            rpos[2] = h;
+        }
+        lpos[2] = h;
+    }
+    lpos[1] = -side * sin(t * 2 * M_PI/T);
+    rpos[1] = lpos[1];
+}
+
+void OnlineMotion::play_online()
+{
+    
+    uint16_t ltheta[5], rtheta[5];
+    
+    float lpos[3], rpos[3];
+    
+    walk(lpos, rpos, 250, 20, 20, 15);
+    
+    invertkinematics(lpos, ltheta);
+    invertkinematics(rpos, rtheta);
+    
+    uint32_t pos[] = {
+        HOMEPOS[0],
+        HOMEPOS[1] + ltheta[0],
+        HOMEPOS[2] - ltheta[1],
+        HOMEPOS[3] - ltheta[2],
+        HOMEPOS[4] + ltheta[3],
+        HOMEPOS[5] + ltheta[0],
+        HOMEPOS[6],
+        HOMEPOS[7],
+        HOMEPOS[8],
+        HOMEPOS[9] + rtheta[0],
+        HOMEPOS[10] + rtheta[1],
+        HOMEPOS[11] + rtheta[2],
+        HOMEPOS[12] - rtheta[3],
+        HOMEPOS[13] + rtheta[0],
+        HOMEPOS[14],
+        HOMEPOS[15],
+        HOMEPOS[16],
+        HOMEPOS[17],
+        HOMEPOS[18],
+        HOMEPOS[19],
+        HOMEPOS[20],
+        HOMEPOS[21],
+        HOMEPOS[22],
+        HOMEPOS[23]
+    };
+    
+    for (int i = 0; i < m_NUM_MAX; ++i) {
+        __disable_irq();
+        pwm->SetDuty(i, pos[i]);
+        __enable_irq();
+    }
+}
\ No newline at end of file