![](/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:
- 19:c2ec475367aa
- Parent:
- 18:7077bedc37eb
diff -r 7077bedc37eb -r c2ec475367aa OnlineMotion.cpp --- a/OnlineMotion.cpp Wed Apr 03 04:56:13 2013 +0000 +++ b/OnlineMotion.cpp Sun Apr 07 06:59:33 2013 +0000 @@ -6,14 +6,7 @@ OnlineMotion::OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, PWM* _pwm, bool* _playing) : pwm(_pwm), playing(_playing), t(0), T(_T), m_NUM_MAX(_size_num - 1), STEP(_TIMESTEP) { - //pwm = _pwm; - //playing = _playing; *playing = true; - - //t = 0; - //T = _T; - //m_NUM_MAX = _size_num - 1; - //STEP = _TIMESTEP; } OnlineMotion::~OnlineMotion() @@ -42,14 +35,13 @@ { 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 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)); @@ -102,28 +94,6 @@ invertkinematics(lpos, ltheta); invertkinematics(rpos, rtheta); - - //uint16_t* pos; - - //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]; - */ - - //pos[10] = 1520 + 500 * sin(t * M_PI / T); - //uint32_t poo = 1520 + 500 * sin(t * M_PI / T); - uint32_t pos[] = { HOMEPOS[0], HOMEPOS[1] + ltheta[0],