Shundo Kishi / Mbed 2 deprecated Hobby_Humanoid_controlor

Controlor for Humanoid. Walking trajectory generator, sensor reflection etc.

Dependencies:   Adafruit-PWM-Servo-Driver MPU6050 RS300 mbed

OnlineMotion.cpp@17:60de3bfdc70b, 2013-04-02 (annotated)

Committer:
syundo0730
Date:
Tue Apr 02 04:19:09 2013 +0000
Revision:
17:60de3bfdc70b
Child:
18:7077bedc37eb
+realtime module(not move well)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syundo0730 17:60de3bfdc70b 1 #include "OnlineMotion.h"
syundo0730 17:60de3bfdc70b 2
syundo0730 17:60de3bfdc70b 3 #include <cmath>
syundo0730 17:60de3bfdc70b 4
syundo0730 17:60de3bfdc70b 5 extern Ticker tick;
syundo0730 17:60de3bfdc70b 6 extern DigitalOut led1;
syundo0730 17:60de3bfdc70b 7
syundo0730 17:60de3bfdc70b 8 OnlineMotion::OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, PWM* _pwm, bool* _playing)
syundo0730 17:60de3bfdc70b 9 {
syundo0730 17:60de3bfdc70b 10 pwm = _pwm;
syundo0730 17:60de3bfdc70b 11 playing = _playing;
syundo0730 17:60de3bfdc70b 12 *playing = true;
syundo0730 17:60de3bfdc70b 13
syundo0730 17:60de3bfdc70b 14 t = 0;
syundo0730 17:60de3bfdc70b 15 T = _T;
syundo0730 17:60de3bfdc70b 16 m_NUM_MAX = _size_num - 1;
syundo0730 17:60de3bfdc70b 17 STEP = _TIMESTEP;
syundo0730 17:60de3bfdc70b 18 }
syundo0730 17:60de3bfdc70b 19
syundo0730 17:60de3bfdc70b 20 OnlineMotion::~OnlineMotion()
syundo0730 17:60de3bfdc70b 21 {
syundo0730 17:60de3bfdc70b 22 led1 = !led1;
syundo0730 17:60de3bfdc70b 23 }
syundo0730 17:60de3bfdc70b 24
syundo0730 17:60de3bfdc70b 25 void OnlineMotion::step()
syundo0730 17:60de3bfdc70b 26 {
syundo0730 17:60de3bfdc70b 27 if (t < T) {
syundo0730 17:60de3bfdc70b 28 update();
syundo0730 17:60de3bfdc70b 29 } else {
syundo0730 17:60de3bfdc70b 30 tick.detach();
syundo0730 17:60de3bfdc70b 31 *playing = false;
syundo0730 17:60de3bfdc70b 32 delete this;
syundo0730 17:60de3bfdc70b 33 }
syundo0730 17:60de3bfdc70b 34 }
syundo0730 17:60de3bfdc70b 35
syundo0730 17:60de3bfdc70b 36 void OnlineMotion::update()
syundo0730 17:60de3bfdc70b 37 {
syundo0730 17:60de3bfdc70b 38 play_online();
syundo0730 17:60de3bfdc70b 39 t += STEP;
syundo0730 17:60de3bfdc70b 40 }
syundo0730 17:60de3bfdc70b 41
syundo0730 17:60de3bfdc70b 42 void OnlineMotion::invertkinematics(float* pos, uint16_t* theta)
syundo0730 17:60de3bfdc70b 43 {
syundo0730 17:60de3bfdc70b 44 float L1 = 45.0;
syundo0730 17:60de3bfdc70b 45 float L2 = 90.0;
syundo0730 17:60de3bfdc70b 46 float L, h;
syundo0730 17:60de3bfdc70b 47
syundo0730 17:60de3bfdc70b 48 float x = pos[0];
syundo0730 17:60de3bfdc70b 49 float y = pos[1];
syundo0730 17:60de3bfdc70b 50 float z = pos[2];
syundo0730 17:60de3bfdc70b 51
syundo0730 17:60de3bfdc70b 52 L = sqrt(z*z + y*y) - 2 * L1;
syundo0730 17:60de3bfdc70b 53 h = sqrt(x*x + L*L);
syundo0730 17:60de3bfdc70b 54
syundo0730 17:60de3bfdc70b 55 float alpha = atan(y / z);
syundo0730 17:60de3bfdc70b 56 float beta = acos(h / (2 * L2));
syundo0730 17:60de3bfdc70b 57 float gunmma = asin(x / h);
syundo0730 17:60de3bfdc70b 58
syundo0730 17:60de3bfdc70b 59 theta[0] = alpha * RADTOVAL;
syundo0730 17:60de3bfdc70b 60 theta[1] = (beta - gunmma) * RADTOVAL;
syundo0730 17:60de3bfdc70b 61 theta[2] = 2 * beta * RADTOVAL;
syundo0730 17:60de3bfdc70b 62 theta[3] = (beta + gunmma) * RADTOVAL;
syundo0730 17:60de3bfdc70b 63 }
syundo0730 17:60de3bfdc70b 64
syundo0730 17:60de3bfdc70b 65 void OnlineMotion::walk(float* lpos, float* rpos, float h, float stride, float side, float up)
syundo0730 17:60de3bfdc70b 66 {
syundo0730 17:60de3bfdc70b 67 if (t < T / 2) {
syundo0730 17:60de3bfdc70b 68 lpos[0] = stride - (4 * stride / T) * t;
syundo0730 17:60de3bfdc70b 69 rpos[0] = -stride + (4 * stride / T) * t;
syundo0730 17:60de3bfdc70b 70 if (t < T / 6) {
syundo0730 17:60de3bfdc70b 71 lpos[2] = h;
syundo0730 17:60de3bfdc70b 72 } else if (t < T / 3) {
syundo0730 17:60de3bfdc70b 73 lpos[2] = h - up * sin((t-T/6) * M_PI/(T/6));
syundo0730 17:60de3bfdc70b 74 } else {
syundo0730 17:60de3bfdc70b 75 lpos[2] = h;
syundo0730 17:60de3bfdc70b 76 }
syundo0730 17:60de3bfdc70b 77 rpos[2] = h;
syundo0730 17:60de3bfdc70b 78 } else {
syundo0730 17:60de3bfdc70b 79 lpos[0] = -stride + (4 * stride / T) * (t - T/2);
syundo0730 17:60de3bfdc70b 80 rpos[0] = stride - (4 * stride / T) * (t - T/2);
syundo0730 17:60de3bfdc70b 81 if (t < 4*T/6) {
syundo0730 17:60de3bfdc70b 82 rpos[2] = h;
syundo0730 17:60de3bfdc70b 83 } else if (t < 5*T/6) {
syundo0730 17:60de3bfdc70b 84 rpos[2] = h -up * sin((t-4*T/6) * M_PI/(T/6));
syundo0730 17:60de3bfdc70b 85 } else {
syundo0730 17:60de3bfdc70b 86 rpos[2] = h;
syundo0730 17:60de3bfdc70b 87 }
syundo0730 17:60de3bfdc70b 88 lpos[2] = h;
syundo0730 17:60de3bfdc70b 89 }
syundo0730 17:60de3bfdc70b 90 lpos[1] = -side * sin(t * 2 * M_PI/T);
syundo0730 17:60de3bfdc70b 91 rpos[1] = lpos[1];
syundo0730 17:60de3bfdc70b 92 }
syundo0730 17:60de3bfdc70b 93
syundo0730 17:60de3bfdc70b 94 void OnlineMotion::play_online()
syundo0730 17:60de3bfdc70b 95 {
syundo0730 17:60de3bfdc70b 96 uint16_t ltheta[5], rtheta[5];
syundo0730 17:60de3bfdc70b 97 uint16_t* pos;
syundo0730 17:60de3bfdc70b 98
syundo0730 17:60de3bfdc70b 99 float lpos[3], rpos[3];
syundo0730 17:60de3bfdc70b 100
syundo0730 17:60de3bfdc70b 101 walk(lpos, rpos, 1,1,1,1);
syundo0730 17:60de3bfdc70b 102
syundo0730 17:60de3bfdc70b 103 invertkinematics(lpos, ltheta);
syundo0730 17:60de3bfdc70b 104 invertkinematics(rpos, rtheta);
syundo0730 17:60de3bfdc70b 105
syundo0730 17:60de3bfdc70b 106 pos = (uint16_t*)HOMEPOS;
syundo0730 17:60de3bfdc70b 107
syundo0730 17:60de3bfdc70b 108 pos[1] = HOMEPOS[1] + ltheta[0];
syundo0730 17:60de3bfdc70b 109 pos[2] = HOMEPOS[2] - ltheta[1];
syundo0730 17:60de3bfdc70b 110 pos[3] = HOMEPOS[3] - ltheta[2];
syundo0730 17:60de3bfdc70b 111 pos[4] = HOMEPOS[4] + ltheta[3];
syundo0730 17:60de3bfdc70b 112 pos[5] = HOMEPOS[5] + ltheta[0];
syundo0730 17:60de3bfdc70b 113
syundo0730 17:60de3bfdc70b 114 pos[9] = HOMEPOS[9] + rtheta[0];
syundo0730 17:60de3bfdc70b 115 pos[10] = HOMEPOS[10] + rtheta[1];
syundo0730 17:60de3bfdc70b 116 pos[11] = HOMEPOS[11] + rtheta[2];
syundo0730 17:60de3bfdc70b 117 pos[12] = HOMEPOS[12] - rtheta[3];
syundo0730 17:60de3bfdc70b 118 pos[13] = HOMEPOS[13] + rtheta[0];
syundo0730 17:60de3bfdc70b 119
syundo0730 17:60de3bfdc70b 120 for (int i = 0; i < m_NUM_MAX; ++i) {
syundo0730 17:60de3bfdc70b 121 __disable_irq();
syundo0730 17:60de3bfdc70b 122 pwm->SetDuty(i, (uint32_t)pos[i]);
syundo0730 17:60de3bfdc70b 123 __enable_irq();
syundo0730 17:60de3bfdc70b 124 }
syundo0730 17:60de3bfdc70b 125 }