Diff: OnlineMotion.cpp

Revision:

17:60de3bfdc70b

Child:

18:7077bedc37eb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/OnlineMotion.cpp	Tue Apr 02 04:19:09 2013 +0000
@@ -0,0 +1,125 @@
+#include "OnlineMotion.h"
+
+#include <cmath>
+
+extern Ticker tick;
+extern DigitalOut led1;
+
+OnlineMotion::OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, PWM* _pwm, bool* _playing)
+{
+    pwm = _pwm;
+    playing = _playing;
+    *playing = true;
+    
+    t = 0;
+    T = _T;
+    m_NUM_MAX = _size_num - 1;
+    STEP = _TIMESTEP;
+}
+
+OnlineMotion::~OnlineMotion()
+{
+    led1 = !led1;
+}
+
+void OnlineMotion::step()
+{
+    if (t < T) {
+        update();
+    } else {
+        tick.detach();
+        *playing = false;
+        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 L, h;
+    
+    float x = pos[0];
+    float y = pos[1];
+    float z = pos[2];
+    
+    L = sqrt(z*z + y*y) - 2 * L1;
+    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];
+    uint16_t* pos;
+    
+    float lpos[3], rpos[3];
+    
+    walk(lpos, rpos, 1,1,1,1);
+    
+    invertkinematics(lpos, ltheta);
+    invertkinematics(rpos, rtheta);
+    
+    pos = (uint16_t*)HOMEPOS;
+    
+    pos[1] = HOMEPOS[1] + ltheta[0];
+    pos[2] = HOMEPOS[2] - ltheta[1];
+    pos[3] = HOMEPOS[3] - ltheta[2];
+    pos[4] = HOMEPOS[4] + ltheta[3];
+    pos[5] = HOMEPOS[5] + ltheta[0];
+    
+    pos[9] = HOMEPOS[9] + rtheta[0];
+    pos[10] = HOMEPOS[10] + rtheta[1];
+    pos[11] = HOMEPOS[11] + rtheta[2];
+    pos[12] = HOMEPOS[12] - rtheta[3];
+    pos[13] = HOMEPOS[13] + rtheta[0];
+    
+    for (int i = 0; i < m_NUM_MAX; ++i) {
+        __disable_irq();
+        pwm->SetDuty(i, (uint32_t)pos[i]);
+        __enable_irq();
+    }
+}
\ No newline at end of file