Diff: Motion/OnlineMotion.cpp

Revision:

23:0927e605af4b

Parent:

22:bf5aa20b9df0

--- a/Motion/OnlineMotion.cpp	Fri Sep 06 08:36:21 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,132 +0,0 @@
-#include "OnlineMotion.h"
-#include "HomePosition.h"
-
-//extern Ticker tick;
-extern DigitalOut led1;
-
-const float RADTOVAL = (SRV_MAX_DUTY - SRV_MIN_DUTY) / 3.14159265;
-
-OnlineMotion::OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, Adafruit_PWMServoDriver* _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] + rtheta[0],
-        HOMEPOS[1] + rtheta[1],
-        HOMEPOS[2] + rtheta[2],
-        HOMEPOS[3] - rtheta[3],
-        HOMEPOS[4] + rtheta[0],
-        HOMEPOS[5] + ltheta[0],
-        HOMEPOS[6] - ltheta[1],
-        HOMEPOS[7] - ltheta[2],
-        HOMEPOS[8] + ltheta[3],
-        HOMEPOS[9] + ltheta[0],
-        HOMEPOS[10],
-        HOMEPOS[11],
-        HOMEPOS[12],
-        HOMEPOS[13],
-        HOMEPOS[14],
-        HOMEPOS[15],
-        HOMEPOS[16],
-        HOMEPOS[17],
-        HOMEPOS[18],
-        HOMEPOS[19],
-        HOMEPOS[20],
-        HOMEPOS[21],
-        HOMEPOS[22],
-        HOMEPOS[23]
-    };
-    
-    __disable_irq();
-    for (int i = 0; i < m_NUM_MAX; ++i) {
-        pwm->setDuty(i, pos[i]);
-    }
-    __enable_irq();
-}
\ No newline at end of file