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

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

Files at this revision

API Documentation at this revision

Comitter:
syundo0730
Date:
Fri Nov 22 00:30:42 2013 +0000
Parent:
22:bf5aa20b9df0
Commit message:
first commit

Changed in this revision

Console.cpp Show annotated file Show diff for this revision Revisions of this file
Console.h Show annotated file Show diff for this revision Revisions of this file
Controlor.cpp Show annotated file Show diff for this revision Revisions of this file
Controlor.h Show annotated file Show diff for this revision Revisions of this file
HomePosition.h Show annotated file Show diff for this revision Revisions of this file
Motion.lib Show annotated file Show diff for this revision Revisions of this file
Motion/Motion.h Show diff for this revision Revisions of this file
Motion/OfflineMotion.cpp Show diff for this revision Revisions of this file
Motion/OfflineMotion.h Show diff for this revision Revisions of this file
Motion/OnlineMotion.cpp Show diff for this revision Revisions of this file
Motion/OnlineMotion.h Show diff for this revision Revisions of this file
RS300.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r bf5aa20b9df0 -r 0927e605af4b Console.cpp
--- a/Console.cpp	Fri Sep 06 08:36:21 2013 +0000
+++ b/Console.cpp	Fri Nov 22 00:30:42 2013 +0000
@@ -51,6 +51,25 @@
     return val;
 }
 
+float Console::get_float_cr()
+{
+    stringstream sstr;
+    serial->printf("Enter the float parameter. And push Enter.: \r\n");
+    while (1) {
+        uint8_t tmp = getc_wait();
+        serial->printf("%c", tmp);
+        if (tmp == '\r') {
+            serial->printf("\r\n");
+            break;
+            }
+        sstr << tmp;
+    }
+    float val;
+    sstr >> val;
+    serial->printf("parameter:%f \r\n", val);
+    return val;
+}
+
 uint8_t Console::getc_wait()
 {
     while (!serial->readable());
@@ -72,6 +91,14 @@
     serial->printf(str);
 }
 
+void Console::showOnline(OnlineMotion* on) {
+	serial->printf("T:%f\r\n", on->T);
+	serial->printf("h:%f\r\n", on->h);
+	serial->printf("side:%f\r\n", on->side);
+	serial->printf("stride:%f\r\n", on->stride);
+	serial->printf("up:%f\r\n", on->up);
+}
+
 /*uint16_t Console::readint()
 {
     uint8_t buff[2];
@@ -89,4 +116,4 @@
     buff[1] = (uint8_t)(val >> 8);
     pc.putc(buff[0]);
     pc.putc(buff[1]);
-}*/
\ No newline at end of file
+}*/
diff -r bf5aa20b9df0 -r 0927e605af4b Console.h
--- a/Console.h	Fri Sep 06 08:36:21 2013 +0000
+++ b/Console.h	Fri Nov 22 00:30:42 2013 +0000
@@ -3,6 +3,8 @@
 
 #include "mbed.h"
 
+#include "OnlineMotion.h"
+
 class Console
 {
   public:
@@ -13,7 +15,9 @@
     int getid();
     uint8_t getheader();
     uint16_t get_int_cr();
+    float get_float_cr();
     void printf(char* str);
+    void showOnline(OnlineMotion* on);
     
   private:
     uint8_t getc_wait();
@@ -23,4 +27,4 @@
     Serial* serial;
 };
 
-#endif
\ No newline at end of file
+#endif
diff -r bf5aa20b9df0 -r 0927e605af4b Controlor.cpp
--- a/Controlor.cpp	Fri Sep 06 08:36:21 2013 +0000
+++ b/Controlor.cpp	Fri Nov 22 00:30:42 2013 +0000
@@ -4,13 +4,11 @@
 #include "Controlor.h"
 #include "HomePosition.h"
 
-Ticker tick;
-
-const float TIMESTEP = 0.005;
+const float TIMESTEP = 0.01;
 
 //detach may be better to controled from Controlor class
 
-Controlor::Controlor(uint16_t* data) : playing(false), attached(false)
+Controlor::Controlor(uint16_t* data) : playing(false), attached(false), servo_size(10)
 {
     //pwm = new PWM();
     pwm = new Adafruit_PWMServoDriver(p9, p10);
@@ -18,20 +16,19 @@
     pwm->setPrescale(64);    //This value is decided for 10ms interval.
     pwm->setI2Cfreq(400000); //400kHz
     
-    for (int i = 0; i < 16; ++i) {
-        pwm->setDuty(i, HOMEPOS[i]);
-    }
+    rs300 = new RS300(p28, p27);
+    rs300->on_all_servo();
     
-    //comu = new SCI(p28, p27);
-    comu = new Console(USBTX, USBRX);
+    home();
     
-    LocalFileSystem* local = new LocalFileSystem("local");
+    //comu = new Console(p13, p14);
+    comu = new Console(USBTX, USBRX);
+
+    mode = 0;
     
-    read("/local/motion.csv", data);
-    set(data);
-    
-    // Motion 0 is Home position
-    //setmotion(0);
+    online = new OnlineMotion(0.7, TIMESTEP, servo_size, pwm, rs300, &playing);
+    cpg = new CPG(TIMESTEP, pwm, &playing);
+    playing = false;
 }
 
 Controlor::~Controlor()
@@ -45,12 +42,25 @@
     delete pwm;
 }
 
+void Controlor::home() {
+	int pwm_servo_num = 10, serial_servo_num = 10;
+
+	for (int i = 0; i < pwm_servo_num; ++i) {
+		pwm->setDuty(i, HOMEPOS[i]);
+	}
+	//std::vector<uint16_t> buf(&HOMEPOS[pwm_servo_num], &HOMEPOS[pwm_servo_num + serial_servo_num]);
+	std::vector<uint16_t> buf;
+	for (int i = pwm_servo_num; i < pwm_servo_num + serial_servo_num; ++i) {
+		buf.push_back(HOMEPOS[i]);
+	}
+	rs300->send_servo_pos(1, buf);
+}
+
 void Controlor::read(const string& filename, uint16_t* data)
 {
     CSV csv;
     pose_size = new int;//<-This code is suspicious
     csv.read(filename, data, &servo_size, &motion_size, pose_size);
-    //readMotion(filename, data, servo_size, motion_size, pose_size); // not so good at speed and has bug in handling float motion value.
 }
 
 void Controlor::set(uint16_t* data)
@@ -86,38 +96,47 @@
     //if (!motion.playing) {
         //motion = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
         //tick.attach(motion, &Motion::step, TIMESTEP);
-        attached = true;
+        //attached = true;
         //offline = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
         //tick.attach(offline, &OfflineMotion::step, TIMESTEP);
-        online = new OnlineMotion(0.5, TIMESTEP, servo_size, pwm, &playing);
-        tick.attach(online, &OnlineMotion::step, TIMESTEP);
+        //online = new OnlineMotion(0.5, TIMESTEP, servo_size, pwm, &playing);
+        //tick.attach(online, &OnlineMotion::step, TIMESTEP);
+        //cpg = new CPG(TIMESTEP, pwm, &playing);
+        //tick.attach(cpg, &CPG::step, TIMESTEP);
     //}
 }
 
 void Controlor::control()
 {
-    /*if (!playing) {
-        if (attached) {
-            tick.detach();
-            delete online;
-            attached = false;
-        } else {
-            setmotion(1);
-        }
-    }*/
-    
-    //setmotion(1);
+    if (playing) {
+        //online->step();
+        cpg->step();
+    }
     char head = comu->getheader();
     if (head == 'a') {
         int id = comu->getid();
         if (checkid(id)) {
-            //setmotion(id);
+            setmotion(id);
         }
     } else if (head == 'b') {
         int id = comu->getid();
         uint16_t val = comu->get_int_cr();
-        //wait(1);
         pwm->setDuty(id, val);
-    }
-    
-}
\ No newline at end of file
+    } else if (head == 'c') {
+        cpg->change_param(0.1, 0.1, 0.1);
+    } else if (head == 'd') {
+        playing = true;
+    } else if (head == 'e') {
+    	playing = false;
+    } else if (head == 'e') {
+        comu->printf("Omega:\r\n");
+        float omega = comu->get_float_cr();
+        comu->printf("K:\r\n");
+        float k = comu->get_float_cr();
+        cpg->change_param(omega, k, 0);
+    } else if (head == 'f') {
+    	home();
+    } else if (head == 'p') {
+    	comu->showOnline(online);
+    } 
+}
diff -r bf5aa20b9df0 -r 0927e605af4b Controlor.h
--- a/Controlor.h	Fri Sep 06 08:36:21 2013 +0000
+++ b/Controlor.h	Fri Nov 22 00:30:42 2013 +0000
@@ -3,14 +3,17 @@
 
 #include "mbed.h"
 #include <string>
+#include <fstream>
 
 #include "Motion.h"
 #include "OfflineMotion.h"
 #include "OnlineMotion.h"
+#include "CPG.h"
 
 #include "Console.h"
 //#include "PWM.h"
 #include "Adafruit_PWMServoDriver.h"
+#include "RS300.h"
 
 #include "CSV.h"
 #include "readMotion.h"
@@ -25,6 +28,7 @@
     void control();
     
   private:
+    void home();
     void read(const string& filename, uint16_t* data);
     void set(uint16_t* data);
     bool checkid(int id);
@@ -41,7 +45,10 @@
     
     OfflineMotion* offline;
     OnlineMotion* online;
+    CPG* cpg;
     Motion* motion;
+    //std::ofstream log;
+    int mode;
     
   private:
     bool playing;
@@ -49,6 +56,7 @@
     Console* comu;
     //PWM* pwm;
     Adafruit_PWMServoDriver* pwm;
+    RS300* rs300;
 };
 
-#endif
\ No newline at end of file
+#endif
diff -r bf5aa20b9df0 -r 0927e605af4b HomePosition.h
--- a/HomePosition.h	Fri Sep 06 08:36:21 2013 +0000
+++ b/HomePosition.h	Fri Nov 22 00:30:42 2013 +0000
@@ -1,15 +1,15 @@
 const uint16_t HOMEPOS[] = {
+1540,
 1525,
-1510,
 1240,
-1520,
+1505,
+1525,
+
 1510,
-
-1515,
-1520,
+1505,
 1830,
-1530,
-1500,
+1545,
+1495,
 
 0,
 0,
@@ -25,4 +25,4 @@
 0,
 0,
 0
-};
\ No newline at end of file
+};
diff -r bf5aa20b9df0 -r 0927e605af4b Motion.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion.lib	Fri Nov 22 00:30:42 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/syundo0730/code/Motion/#80f4ef6d8e2c
diff -r bf5aa20b9df0 -r 0927e605af4b Motion/Motion.h
--- a/Motion/Motion.h	Fri Sep 06 08:36:21 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,13 +0,0 @@
-#ifndef MOTION_H_2013_04_7_
-#define MOTION_H_2013_04_7_
-
-#include "mbed.h"
-#include "PWM.h"
-
-class Motion
-{
-  public:
-    virtual void step() = 0;
-};
-
-#endif
\ No newline at end of file
diff -r bf5aa20b9df0 -r 0927e605af4b Motion/OfflineMotion.cpp
--- a/Motion/OfflineMotion.cpp	Fri Sep 06 08:36:21 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,125 +0,0 @@
-#include "OfflineMotion.h"
-
-//extern Ticker tick;
-extern DigitalOut led1;
-
-OfflineMotion::OfflineMotion(uint16_t** data, unsigned short int size_idx, unsigned char size_num, Adafruit_PWMServoDriver* _pwm, bool* _playing)
-{
-    pwm = _pwm;
-    
-    playing = _playing;
-    *playing = true;
-  
-    m_data = data;
-    
-    m_IDX_MAX = size_idx;
-    m_NUM_MAX = size_num - 1;
- 
-    for(int i = 0; i < m_IDX_MAX; ++i) {
-        m_data_num[i] = data[i][m_NUM_MAX];
-    }
-    
-    m_mode = 1;
-    
-    m_idx = 0;
-    m_play = 0;
-    
-    for (int i = 0; i < m_NUM_MAX; ++i) {
-        m_buf[i] = 0.0;
-        m_d_buf[i] = 0.0;
-        m_dd_buf[i] = 0.0;
-    }
-    m_brake_flg = 0.0;
-    
-}
-
-OfflineMotion::~OfflineMotion()
-{
-    led1 = !led1;
-}
-
-void OfflineMotion::step()
-{
-    if (m_idx < m_IDX_MAX - 1) {
-        update();
-    } else {
-        m_idx = 0;
-        m_play = 0;
-        *playing = false;
-        //tick.detach();
-        //delete this;
-    }
-}
-
-void OfflineMotion::update()
-{
-    if (m_play == m_data_num[m_idx]) {  //edge(end)
-        ++m_idx;
-        init_inter();
-        m_play = 1;
-    } else if (m_play > 1) {
-        step_inter();                        //interpolate
-        ++m_play;
-    } else if (m_play == 1) {                //inter para
-        set_inter();
-        step_inter();
-        ++m_play;
-    } else if (m_play == 0) {                //start(called onece)
-        init_inter();
-        ++m_play;
-    } else {
-        return;
-    }
-}
-
-void OfflineMotion::set_inter()
-{
-    m_brake_pnt = m_data_num[m_idx] / 2.0;
-    
-    if (m_mode == 0){           //liner
-        for (int i = 0; i < m_NUM_MAX; ++i) {
-            m_d_buf[i] = m_data[m_idx + 1][i] - m_data[m_idx][i];
-            m_d_buf[i] /= m_data_num[m_idx];
-            m_dd_buf[i] = 0.0;
-            m_buf[i] = m_data[m_idx][i];
-        }
-        m_brake_flg = 1.0;
-    } else {                    //accel
-        for (int i = 0; i < m_NUM_MAX; ++i) {
-            m_dd_buf[i] = m_data[m_idx + 1][i] - m_data[m_idx][i];
-            m_dd_buf[i] = m_dd_buf[i] / m_data_num[m_idx] / m_data_num[m_idx] * 4.0;
-            m_d_buf[i] = 0.0;
-            m_buf[i] = m_data[m_idx][i];
-        }
-        m_brake_flg = 1.0;
-    }
-}
-
-void OfflineMotion::init_inter()
-{
-    for (int i = 0; i < m_NUM_MAX; ++i) {
-        m_buf[i] = m_data[m_idx][i];
-        m_d_buf[i] = 0.0;
-        m_dd_buf[i] = 0.0;
-        
-        __disable_irq();
-        pwm->setDuty(i, (uint32_t)m_buf[i]);
-        __enable_irq();
-    }
-    m_brake_flg = 0.0;
-}
-
-void OfflineMotion::step_inter()
-{
-    if (m_play > m_brake_pnt) {
-        m_brake_flg = -1.0;
-    }
-    for (int i = 0; i < m_NUM_MAX; ++i) {
-        m_d_buf[i] += (m_dd_buf[i]) * m_brake_flg;
-        m_buf[i] += m_d_buf[i];
-        
-        __disable_irq();
-        pwm->setDuty(i, (uint32_t)m_buf[i]);
-        __enable_irq();
-    }
-}
\ No newline at end of file
diff -r bf5aa20b9df0 -r 0927e605af4b Motion/OfflineMotion.h
--- a/Motion/OfflineMotion.h	Fri Sep 06 08:36:21 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,44 +0,0 @@
-#ifndef OFFLINEMOTION_H_2012_09_21_
-#define OFFLINEMOTION_H_2012_09_21_
-
-#include "LPC17xx.h"
-#include "mbed.h"
-//#include "PWM.h"
-#include "Adafruit_PWMServoDriver.h"
-//#include "Motion.h"
-
-class OfflineMotion //: public Motion
-{
-  public:
-    OfflineMotion(uint16_t** data, unsigned short int size_idx, unsigned char size_num, Adafruit_PWMServoDriver* _pwm, bool* _playing);
-    ~OfflineMotion();
-  public:
-    void step();
-  private:
-    void update();
-    void set_inter();
-    void init_inter();
-    void step_inter();
-  
-  private:
-    //PWM* pwm;
-    Adafruit_PWMServoDriver* pwm;
-    uint16_t** m_data;
-    unsigned short int m_IDX_MAX;
-    unsigned char m_NUM_MAX;
-    unsigned char m_data_num[500];  //inter
-    unsigned char m_mode;
-    bool* playing;
-  
-  private:
-    unsigned short int m_idx;
-    unsigned char m_play;
-  private:  
-    double m_buf[25];              //buf
-    double m_d_buf[25];            //dbuf
-    double m_dd_buf[25];           //ddbuf
-    double m_brake_flg;
-    unsigned char m_brake_pnt;
-};
-
-#endif
\ No newline at end of file
diff -r bf5aa20b9df0 -r 0927e605af4b Motion/OnlineMotion.cpp
--- a/Motion/OnlineMotion.cpp	Fri Sep 06 08:36:21 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,132 +0,0 @@
-#include "OnlineMotion.h"
-#include "HomePosition.h"
-
-//extern Ticker tick;
-extern DigitalOut led1;
-
-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;
-}
-
-OnlineMotion::~OnlineMotion()
-{
-    led1 = !led1;
-}
-
-void OnlineMotion::step()
-{
-    if (t < T) {
-        update();
-    } else {
-        *playing = false;
-        //tick.detach();
-        //delete this;
-    }
-}
-
-void OnlineMotion::update()
-{
-    play_online();
-    t += STEP;
-}
-
-void OnlineMotion::invertkinematics(float* pos, uint16_t* theta)
-{
-    float L1 = 45.0;
-    float L2 = 90.0;
-    
-    float x = pos[0];
-    float y = pos[1];
-    float z = pos[2];
-    
-    float L = sqrt(z*z + y*y) - 2 * L1;
-    float h = sqrt(x*x + L*L);
-    
-    float alpha = atan(y / z);
-    float beta = acos(h / (2 * L2));
-    float gunmma = asin(x / h);
-    
-    theta[0] = alpha * RADTOVAL;
-    theta[1] = (beta - gunmma) * RADTOVAL;
-    theta[2] = 2 * beta * RADTOVAL;
-    theta[3] = (beta + gunmma) * RADTOVAL;
-}
-
-void OnlineMotion::walk(float* lpos, float* rpos, float h, float stride, float side, float up)
-{
-    if (t < T / 2) {
-        lpos[0] = stride - (4 * stride / T) * t;
-        rpos[0] = -stride + (4 * stride / T) * t;
-        if (t < T / 6) {
-            lpos[2] = h;
-        } else if (t < T / 3) {
-            lpos[2] = h - up * sin((t-T/6) * M_PI/(T/6));
-        } else {
-            lpos[2] = h;
-        }
-        rpos[2] = h;
-    } else {
-        lpos[0] = -stride + (4 * stride / T) * (t - T/2);
-        rpos[0] = stride - (4 * stride / T) * (t - T/2);
-        if (t < 4*T/6) {
-            rpos[2] = h;
-        } else if (t < 5*T/6) {
-            rpos[2] = h -up * sin((t-4*T/6) * M_PI/(T/6));
-        } else {
-            rpos[2] = h;
-        }
-        lpos[2] = h;
-    }
-    lpos[1] = -side * sin(t * 2 * M_PI/T);
-    rpos[1] = lpos[1];
-}
-
-void OnlineMotion::play_online()
-{
-    
-    uint16_t ltheta[5], rtheta[5];
-    
-    float lpos[3], rpos[3];
-    
-    walk(lpos, rpos, 250, 20, 20, 15);
-    
-    invertkinematics(lpos, ltheta);
-    invertkinematics(rpos, rtheta);
-    
-    uint32_t pos[] = {
-        HOMEPOS[0] + rtheta[0],
-        HOMEPOS[1] + rtheta[1],
-        HOMEPOS[2] + rtheta[2],
-        HOMEPOS[3] - rtheta[3],
-        HOMEPOS[4] + rtheta[0],
-        HOMEPOS[5] + ltheta[0],
-        HOMEPOS[6] - ltheta[1],
-        HOMEPOS[7] - ltheta[2],
-        HOMEPOS[8] + ltheta[3],
-        HOMEPOS[9] + ltheta[0],
-        HOMEPOS[10],
-        HOMEPOS[11],
-        HOMEPOS[12],
-        HOMEPOS[13],
-        HOMEPOS[14],
-        HOMEPOS[15],
-        HOMEPOS[16],
-        HOMEPOS[17],
-        HOMEPOS[18],
-        HOMEPOS[19],
-        HOMEPOS[20],
-        HOMEPOS[21],
-        HOMEPOS[22],
-        HOMEPOS[23]
-    };
-    
-    __disable_irq();
-    for (int i = 0; i < m_NUM_MAX; ++i) {
-        pwm->setDuty(i, pos[i]);
-    }
-    __enable_irq();
-}
\ No newline at end of file
diff -r bf5aa20b9df0 -r 0927e605af4b Motion/OnlineMotion.h
--- a/Motion/OnlineMotion.h	Fri Sep 06 08:36:21 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,37 +0,0 @@
-#ifndef ONLINEMOTION_H_2013_02_04_
-#define ONLINEMOTION_H_2013_02_04_
-
-#include <cmath>
-
-#include "LPC17xx.h"
-#include "mbed.h"
-#include "PWM.h"
-#include "Adafruit_PWMServoDriver.h"
-//#include "Motion.h"
-
-
-const float M_PI = 3.14159265;
-
-class OnlineMotion //: public Motion
-{
-  public:
-    OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, Adafruit_PWMServoDriver* _pwm, bool* _playing);
-    ~OnlineMotion();
-  public:
-    void step();
-  private:
-    void update();
-    void play_online();
-    void invertkinematics(float* pos, uint16_t* theta);
-    void walk(float* lpos, float* rpos, float h, float stride, float side, float up);
-    
-  private:
-    Adafruit_PWMServoDriver* pwm;
-    //PWM* pwm;
-    unsigned char m_NUM_MAX;
-    bool* playing;
-    volatile float t;
-    float T ,STEP;
-};
-
-#endif
\ No newline at end of file
diff -r bf5aa20b9df0 -r 0927e605af4b RS300.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RS300.lib	Fri Nov 22 00:30:42 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/syundo0730/code/RS300/#1ab1adf0915c
diff -r bf5aa20b9df0 -r 0927e605af4b main.cpp
--- a/main.cpp	Fri Sep 06 08:36:21 2013 +0000
+++ b/main.cpp	Fri Nov 22 00:30:42 2013 +0000
@@ -2,17 +2,24 @@
 #include "mbed.h"
 #include "Controlor.h"
 #include "PWM.h"
+//#include "rtos.h"
 
 uint16_t data[0x2000] __attribute__((section("AHBSRAM1"))); //motion data area
 
 DigitalOut led1(LED1);
 
+Ticker tick;
+
 int main(void)
 {
     Controlor controlor(data);
     
+    tick.attach(&controlor, &Controlor::control, 0.01);
+    
     while (1) {
-        controlor.control();
-        wait(0.005);
+        //controlor.control();
+        //wait(0.01);
+        //Thread::wait(5);
+        //led1 = !led1;
     }
-}
\ No newline at end of file
+}