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@22:bf5aa20b9df0, 2013-09-06 (annotated)

Committer:
syundo0730
Date:
Fri Sep 06 08:36:21 2013 +0000
Revision:
22:bf5aa20b9df0
Parent:
21:a54bcab078ed 
Subtile change.; Home position was changed.; SCI class was renamed to Console class and function was modified.

Who changed what in which revision?

UserRevisionLine numberNew 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 22:bf5aa20b9df0 101 HOMEPOS[0] + rtheta[0],
syundo0730 22:bf5aa20b9df0 102 HOMEPOS[1] + rtheta[1],
syundo0730 22:bf5aa20b9df0 103 HOMEPOS[2] + rtheta[2],
syundo0730 22:bf5aa20b9df0 104 HOMEPOS[3] - rtheta[3],
syundo0730 22:bf5aa20b9df0 105 HOMEPOS[4] + rtheta[0],
syundo0730 22:bf5aa20b9df0 106 HOMEPOS[5] + ltheta[0],
syundo0730 22:bf5aa20b9df0 107 HOMEPOS[6] - ltheta[1],
syundo0730 22:bf5aa20b9df0 108 HOMEPOS[7] - ltheta[2],
syundo0730 22:bf5aa20b9df0 109 HOMEPOS[8] + ltheta[3],
syundo0730 22:bf5aa20b9df0 110 HOMEPOS[9] + ltheta[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 22:bf5aa20b9df0 127 __disable_irq();
syundo0730 17:60de3bfdc70b 128 for (int i = 0; i < m_NUM_MAX; ++i) {
syundo0730 21:a54bcab078ed 129 pwm->setDuty(i, pos[i]);
syundo0730 17:60de3bfdc70b 130 }
syundo0730 22:bf5aa20b9df0 131 __enable_irq();
syundo0730 17:60de3bfdc70b 132 }