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

Motion/OnlineMotion.cpp@20:abb7852df747, 2013-08-19 (annotated)

Committer:
syundo0730
Date:
Mon Aug 19 08:10:58 2013 +0000
Revision:
20:abb7852df747
Parent:
OnlineMotion.cpp@19:c2ec475367aa
Child:
21:a54bcab078ed
delete Motion instance from Control class

Who changed what in which revision?

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