Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.
Dependencies: Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed
Diff: Motion/OnlineMotion.cpp
- Revision:
- 23:0927e605af4b
- Parent:
- 22:bf5aa20b9df0
diff -r bf5aa20b9df0 -r 0927e605af4b Motion/OnlineMotion.cpp --- 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