Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.
Dependencies: Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed
Diff: OnlineMotion.cpp
- Revision:
- 18:7077bedc37eb
- Parent:
- 17:60de3bfdc70b
- Child:
- 19:c2ec475367aa
diff -r 60de3bfdc70b -r 7077bedc37eb OnlineMotion.cpp --- a/OnlineMotion.cpp Tue Apr 02 04:19:09 2013 +0000 +++ b/OnlineMotion.cpp Wed Apr 03 04:56:13 2013 +0000 @@ -1,20 +1,19 @@ #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), t(0), T(_T), m_NUM_MAX(_size_num - 1), STEP(_TIMESTEP) { - pwm = _pwm; - playing = _playing; + //pwm = _pwm; + //playing = _playing; *playing = true; - t = 0; - T = _T; - m_NUM_MAX = _size_num - 1; - STEP = _TIMESTEP; + //t = 0; + //T = _T; + //m_NUM_MAX = _size_num - 1; + //STEP = _TIMESTEP; } OnlineMotion::~OnlineMotion() @@ -27,8 +26,8 @@ if (t < T) { update(); } else { + *playing = false; tick.detach(); - *playing = false; delete this; } } @@ -93,18 +92,22 @@ 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); + walk(lpos, rpos, 250, 20, 20, 15); invertkinematics(lpos, ltheta); invertkinematics(rpos, rtheta); - pos = (uint16_t*)HOMEPOS; + + //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]; @@ -116,10 +119,41 @@ 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], + HOMEPOS[2] - ltheta[1], + HOMEPOS[3] - ltheta[2], + HOMEPOS[4] + ltheta[3], + HOMEPOS[5] + ltheta[0], + HOMEPOS[6], + HOMEPOS[7], + HOMEPOS[8], + HOMEPOS[9] + rtheta[0], + HOMEPOS[10] + rtheta[1], + HOMEPOS[11] + rtheta[2], + HOMEPOS[12] - rtheta[3], + HOMEPOS[13] + rtheta[0], + 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, (uint32_t)pos[i]); + pwm->SetDuty(i, pos[i]); __enable_irq(); } } \ No newline at end of file