File content as of revision 21:a54bcab078ed:

#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] + ltheta[0],
        HOMEPOS[1] - ltheta[1],
        HOMEPOS[2] - ltheta[2],
        HOMEPOS[3] + ltheta[3],
        HOMEPOS[4] + ltheta[0],
        HOMEPOS[5] + rtheta[0],
        HOMEPOS[6] + rtheta[1],
        HOMEPOS[7] + rtheta[2],
        HOMEPOS[8] - rtheta[3],
        HOMEPOS[9] + rtheta[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]
    };
    
    for (int i = 0; i < m_NUM_MAX; ++i) {
        __disable_irq();
        pwm->setDuty(i, pos[i]);
        __enable_irq();
    }
}