Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.
Dependencies: Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed
Diff: Motion/OnlineMotion.cpp
- Revision:
- 21:a54bcab078ed
- Parent:
- 20:abb7852df747
- Child:
- 22:bf5aa20b9df0
diff -r abb7852df747 -r a54bcab078ed Motion/OnlineMotion.cpp --- a/Motion/OnlineMotion.cpp Mon Aug 19 08:10:58 2013 +0000 +++ b/Motion/OnlineMotion.cpp Wed Sep 04 03:59:40 2013 +0000 @@ -1,9 +1,12 @@ #include "OnlineMotion.h" +#include "HomePosition.h" //extern Ticker tick; extern DigitalOut led1; -OnlineMotion::OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, PWM* _pwm, bool* _playing) +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; @@ -95,20 +98,20 @@ invertkinematics(rpos, rtheta); 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[0] + ltheta[0], + HOMEPOS[1] - ltheta[1], + HOMEPOS[2] - ltheta[2], + HOMEPOS[3] + ltheta[3], + HOMEPOS[4] + ltheta[0], + HOMEPOS[5] + rtheta[0], + HOMEPOS[6] + rtheta[1], + HOMEPOS[7] + rtheta[2], + HOMEPOS[8] - rtheta[3], HOMEPOS[9] + rtheta[0], - HOMEPOS[10] + rtheta[1], - HOMEPOS[11] + rtheta[2], - HOMEPOS[12] - rtheta[3], - HOMEPOS[13] + rtheta[0], + HOMEPOS[10], + HOMEPOS[11], + HOMEPOS[12], + HOMEPOS[13], HOMEPOS[14], HOMEPOS[15], HOMEPOS[16], @@ -123,7 +126,7 @@ for (int i = 0; i < m_NUM_MAX; ++i) { __disable_irq(); - pwm->SetDuty(i, pos[i]); + pwm->setDuty(i, pos[i]); __enable_irq(); } } \ No newline at end of file