Revision 23:0927e605af4b, committed 2013-11-22

Comitter:

syundo0730

Date:

Fri Nov 22 00:30:42 2013 +0000

Parent:

22:bf5aa20b9df0

Commit message:

first commit

Changed in this revision

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
+}