![](/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
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