Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.
Dependencies: Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed
Motion/OnlineMotion.cpp@21:a54bcab078ed, 2013-09-04 (annotated)
- Committer:
- syundo0730
- Date:
- Wed Sep 04 03:59:40 2013 +0000
- Revision:
- 21:a54bcab078ed
- Parent:
- 20:abb7852df747
- Child:
- 22:bf5aa20b9df0
Motion handling was moved to out of motion class
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
syundo0730 | 17:60de3bfdc70b | 1 | #include "OnlineMotion.h" |
syundo0730 | 21:a54bcab078ed | 2 | #include "HomePosition.h" |
syundo0730 | 17:60de3bfdc70b | 3 | |
syundo0730 | 20:abb7852df747 | 4 | //extern Ticker tick; |
syundo0730 | 17:60de3bfdc70b | 5 | extern DigitalOut led1; |
syundo0730 | 17:60de3bfdc70b | 6 | |
syundo0730 | 21:a54bcab078ed | 7 | const float RADTOVAL = (SRV_MAX_DUTY - SRV_MIN_DUTY) / 3.14159265; |
syundo0730 | 21:a54bcab078ed | 8 | |
syundo0730 | 21:a54bcab078ed | 9 | OnlineMotion::OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, Adafruit_PWMServoDriver* _pwm, bool* _playing) |
syundo0730 | 18:7077bedc37eb | 10 | : pwm(_pwm), playing(_playing), t(0), T(_T), m_NUM_MAX(_size_num - 1), STEP(_TIMESTEP) |
syundo0730 | 17:60de3bfdc70b | 11 | { |
syundo0730 | 17:60de3bfdc70b | 12 | *playing = true; |
syundo0730 | 17:60de3bfdc70b | 13 | } |
syundo0730 | 17:60de3bfdc70b | 14 | |
syundo0730 | 17:60de3bfdc70b | 15 | OnlineMotion::~OnlineMotion() |
syundo0730 | 17:60de3bfdc70b | 16 | { |
syundo0730 | 17:60de3bfdc70b | 17 | led1 = !led1; |
syundo0730 | 17:60de3bfdc70b | 18 | } |
syundo0730 | 17:60de3bfdc70b | 19 | |
syundo0730 | 17:60de3bfdc70b | 20 | void OnlineMotion::step() |
syundo0730 | 17:60de3bfdc70b | 21 | { |
syundo0730 | 17:60de3bfdc70b | 22 | if (t < T) { |
syundo0730 | 17:60de3bfdc70b | 23 | update(); |
syundo0730 | 17:60de3bfdc70b | 24 | } else { |
syundo0730 | 18:7077bedc37eb | 25 | *playing = false; |
syundo0730 | 20:abb7852df747 | 26 | //tick.detach(); |
syundo0730 | 20:abb7852df747 | 27 | //delete this; |
syundo0730 | 17:60de3bfdc70b | 28 | } |
syundo0730 | 17:60de3bfdc70b | 29 | } |
syundo0730 | 17:60de3bfdc70b | 30 | |
syundo0730 | 17:60de3bfdc70b | 31 | void OnlineMotion::update() |
syundo0730 | 17:60de3bfdc70b | 32 | { |
syundo0730 | 17:60de3bfdc70b | 33 | play_online(); |
syundo0730 | 17:60de3bfdc70b | 34 | t += STEP; |
syundo0730 | 17:60de3bfdc70b | 35 | } |
syundo0730 | 17:60de3bfdc70b | 36 | |
syundo0730 | 17:60de3bfdc70b | 37 | void OnlineMotion::invertkinematics(float* pos, uint16_t* theta) |
syundo0730 | 17:60de3bfdc70b | 38 | { |
syundo0730 | 17:60de3bfdc70b | 39 | float L1 = 45.0; |
syundo0730 | 17:60de3bfdc70b | 40 | float L2 = 90.0; |
syundo0730 | 17:60de3bfdc70b | 41 | |
syundo0730 | 17:60de3bfdc70b | 42 | float x = pos[0]; |
syundo0730 | 17:60de3bfdc70b | 43 | float y = pos[1]; |
syundo0730 | 17:60de3bfdc70b | 44 | float z = pos[2]; |
syundo0730 | 17:60de3bfdc70b | 45 | |
syundo0730 | 19:c2ec475367aa | 46 | float L = sqrt(z*z + y*y) - 2 * L1; |
syundo0730 | 19:c2ec475367aa | 47 | float h = sqrt(x*x + L*L); |
syundo0730 | 17:60de3bfdc70b | 48 | |
syundo0730 | 17:60de3bfdc70b | 49 | float alpha = atan(y / z); |
syundo0730 | 17:60de3bfdc70b | 50 | float beta = acos(h / (2 * L2)); |
syundo0730 | 17:60de3bfdc70b | 51 | float gunmma = asin(x / h); |
syundo0730 | 17:60de3bfdc70b | 52 | |
syundo0730 | 17:60de3bfdc70b | 53 | theta[0] = alpha * RADTOVAL; |
syundo0730 | 17:60de3bfdc70b | 54 | theta[1] = (beta - gunmma) * RADTOVAL; |
syundo0730 | 17:60de3bfdc70b | 55 | theta[2] = 2 * beta * RADTOVAL; |
syundo0730 | 17:60de3bfdc70b | 56 | theta[3] = (beta + gunmma) * RADTOVAL; |
syundo0730 | 17:60de3bfdc70b | 57 | } |
syundo0730 | 17:60de3bfdc70b | 58 | |
syundo0730 | 17:60de3bfdc70b | 59 | void OnlineMotion::walk(float* lpos, float* rpos, float h, float stride, float side, float up) |
syundo0730 | 17:60de3bfdc70b | 60 | { |
syundo0730 | 17:60de3bfdc70b | 61 | if (t < T / 2) { |
syundo0730 | 17:60de3bfdc70b | 62 | lpos[0] = stride - (4 * stride / T) * t; |
syundo0730 | 17:60de3bfdc70b | 63 | rpos[0] = -stride + (4 * stride / T) * t; |
syundo0730 | 17:60de3bfdc70b | 64 | if (t < T / 6) { |
syundo0730 | 17:60de3bfdc70b | 65 | lpos[2] = h; |
syundo0730 | 17:60de3bfdc70b | 66 | } else if (t < T / 3) { |
syundo0730 | 17:60de3bfdc70b | 67 | lpos[2] = h - up * sin((t-T/6) * M_PI/(T/6)); |
syundo0730 | 17:60de3bfdc70b | 68 | } else { |
syundo0730 | 17:60de3bfdc70b | 69 | lpos[2] = h; |
syundo0730 | 17:60de3bfdc70b | 70 | } |
syundo0730 | 17:60de3bfdc70b | 71 | rpos[2] = h; |
syundo0730 | 17:60de3bfdc70b | 72 | } else { |
syundo0730 | 17:60de3bfdc70b | 73 | lpos[0] = -stride + (4 * stride / T) * (t - T/2); |
syundo0730 | 17:60de3bfdc70b | 74 | rpos[0] = stride - (4 * stride / T) * (t - T/2); |
syundo0730 | 17:60de3bfdc70b | 75 | if (t < 4*T/6) { |
syundo0730 | 17:60de3bfdc70b | 76 | rpos[2] = h; |
syundo0730 | 17:60de3bfdc70b | 77 | } else if (t < 5*T/6) { |
syundo0730 | 17:60de3bfdc70b | 78 | rpos[2] = h -up * sin((t-4*T/6) * M_PI/(T/6)); |
syundo0730 | 17:60de3bfdc70b | 79 | } else { |
syundo0730 | 17:60de3bfdc70b | 80 | rpos[2] = h; |
syundo0730 | 17:60de3bfdc70b | 81 | } |
syundo0730 | 17:60de3bfdc70b | 82 | lpos[2] = h; |
syundo0730 | 17:60de3bfdc70b | 83 | } |
syundo0730 | 17:60de3bfdc70b | 84 | lpos[1] = -side * sin(t * 2 * M_PI/T); |
syundo0730 | 17:60de3bfdc70b | 85 | rpos[1] = lpos[1]; |
syundo0730 | 17:60de3bfdc70b | 86 | } |
syundo0730 | 17:60de3bfdc70b | 87 | |
syundo0730 | 17:60de3bfdc70b | 88 | void OnlineMotion::play_online() |
syundo0730 | 17:60de3bfdc70b | 89 | { |
syundo0730 | 18:7077bedc37eb | 90 | |
syundo0730 | 17:60de3bfdc70b | 91 | uint16_t ltheta[5], rtheta[5]; |
syundo0730 | 17:60de3bfdc70b | 92 | |
syundo0730 | 17:60de3bfdc70b | 93 | float lpos[3], rpos[3]; |
syundo0730 | 17:60de3bfdc70b | 94 | |
syundo0730 | 18:7077bedc37eb | 95 | walk(lpos, rpos, 250, 20, 20, 15); |
syundo0730 | 17:60de3bfdc70b | 96 | |
syundo0730 | 17:60de3bfdc70b | 97 | invertkinematics(lpos, ltheta); |
syundo0730 | 17:60de3bfdc70b | 98 | invertkinematics(rpos, rtheta); |
syundo0730 | 17:60de3bfdc70b | 99 | |
syundo0730 | 18:7077bedc37eb | 100 | uint32_t pos[] = { |
syundo0730 | 21:a54bcab078ed | 101 | HOMEPOS[0] + ltheta[0], |
syundo0730 | 21:a54bcab078ed | 102 | HOMEPOS[1] - ltheta[1], |
syundo0730 | 21:a54bcab078ed | 103 | HOMEPOS[2] - ltheta[2], |
syundo0730 | 21:a54bcab078ed | 104 | HOMEPOS[3] + ltheta[3], |
syundo0730 | 21:a54bcab078ed | 105 | HOMEPOS[4] + ltheta[0], |
syundo0730 | 21:a54bcab078ed | 106 | HOMEPOS[5] + rtheta[0], |
syundo0730 | 21:a54bcab078ed | 107 | HOMEPOS[6] + rtheta[1], |
syundo0730 | 21:a54bcab078ed | 108 | HOMEPOS[7] + rtheta[2], |
syundo0730 | 21:a54bcab078ed | 109 | HOMEPOS[8] - rtheta[3], |
syundo0730 | 18:7077bedc37eb | 110 | HOMEPOS[9] + rtheta[0], |
syundo0730 | 21:a54bcab078ed | 111 | HOMEPOS[10], |
syundo0730 | 21:a54bcab078ed | 112 | HOMEPOS[11], |
syundo0730 | 21:a54bcab078ed | 113 | HOMEPOS[12], |
syundo0730 | 21:a54bcab078ed | 114 | HOMEPOS[13], |
syundo0730 | 18:7077bedc37eb | 115 | HOMEPOS[14], |
syundo0730 | 18:7077bedc37eb | 116 | HOMEPOS[15], |
syundo0730 | 18:7077bedc37eb | 117 | HOMEPOS[16], |
syundo0730 | 18:7077bedc37eb | 118 | HOMEPOS[17], |
syundo0730 | 18:7077bedc37eb | 119 | HOMEPOS[18], |
syundo0730 | 18:7077bedc37eb | 120 | HOMEPOS[19], |
syundo0730 | 18:7077bedc37eb | 121 | HOMEPOS[20], |
syundo0730 | 18:7077bedc37eb | 122 | HOMEPOS[21], |
syundo0730 | 18:7077bedc37eb | 123 | HOMEPOS[22], |
syundo0730 | 18:7077bedc37eb | 124 | HOMEPOS[23] |
syundo0730 | 18:7077bedc37eb | 125 | }; |
syundo0730 | 17:60de3bfdc70b | 126 | |
syundo0730 | 17:60de3bfdc70b | 127 | for (int i = 0; i < m_NUM_MAX; ++i) { |
syundo0730 | 17:60de3bfdc70b | 128 | __disable_irq(); |
syundo0730 | 21:a54bcab078ed | 129 | pwm->setDuty(i, pos[i]); |
syundo0730 | 17:60de3bfdc70b | 130 | __enable_irq(); |
syundo0730 | 17:60de3bfdc70b | 131 | } |
syundo0730 | 17:60de3bfdc70b | 132 | } |