![](/media/cache/profiles/e5815151957be36ad2085b7a1a02c5cc.jpg.50x50_q85.jpg)
Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.
Dependencies: Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed
OnlineMotion.cpp
- Committer:
- syundo0730
- Date:
- 2013-04-02
- Revision:
- 17:60de3bfdc70b
- Child:
- 18:7077bedc37eb
File content as of revision 17:60de3bfdc70b:
#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(); } }