Revision 21:a54bcab078ed, committed 2013-09-04

Comitter:

syundo0730

Date:

Wed Sep 04 03:59:40 2013 +0000

Parent:

20:abb7852df747

Child:

22:bf5aa20b9df0

Commit message:

Motion handling was moved to out of motion class

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit-PWM-Servo-Driver.lib	Wed Sep 04 03:59:40 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/syundo0730/code/Adafruit-PWM-Servo-Driver/#88bdd5c4e77b

--- a/Controlor.cpp	Mon Aug 19 08:10:58 2013 +0000
+++ b/Controlor.cpp	Wed Sep 04 03:59:40 2013 +0000
@@ -5,11 +5,19 @@
 
 Ticker tick;
 
+const float TIMESTEP = 0.01;
+
+
 //detach may be better to controled from Controlor class
 
 Controlor::Controlor(uint16_t* data) : playing(false), attached(false)
 {
-    pwm = new PWM();
+    //pwm = new PWM();
+    pwm = new Adafruit_PWMServoDriver(p9, p10);
+    pwm->begin();
+    pwm->setPrescale(64);    //This value is decided for 10ms interval.
+    pwm->setI2Cfreq(400000); //400kHz
+    
     //comu = new SCI(p28, p27);
     comu = new SCI(USBTX, USBRX);
     
@@ -77,7 +85,7 @@
         attached = true;
         offline = new OfflineMotion(motions[id], pose_size[id], servo_size, pwm, &playing);
         tick.attach(offline, &OfflineMotion::step, TIMESTEP);
-        //online = new OnlineMotion(3.0, TIMESTEP, servo_size, pwm, &playing);
+        //online = new OnlineMotion(1.0, TIMESTEP, servo_size, pwm, &playing);
         //tick.attach(online, &OnlineMotion::step, TIMESTEP);
     //}
 }

--- a/Controlor.h	Mon Aug 19 08:10:58 2013 +0000
+++ b/Controlor.h	Wed Sep 04 03:59:40 2013 +0000
@@ -9,13 +9,12 @@
 #include "OnlineMotion.h"
 
 #include "SCI.h"
-#include "PWM.h"
+//#include "PWM.h"
+#include "Adafruit_PWMServoDriver.h"
 
 #include "CSV.h"
 #include "readMotion.h"
 
-const float TIMESTEP = 0.02;
-
 class Controlor
 {
   public:
@@ -48,7 +47,8 @@
     bool playing;
     bool attached;
     SCI* comu;
-    PWM* pwm;
+    //PWM* pwm;
+    Adafruit_PWMServoDriver* pwm;
 };
 
 #endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HomePosition.h	Wed Sep 04 03:59:40 2013 +0000
@@ -0,0 +1,26 @@
+const uint16_t HOMEPOS[] = {
+1570,
+1490,
+1500,
+1820,
+1515,
+1500,
+0,
+0,
+1500,
+1500,
+1500,
+1220,
+1500,
+1500,
+0,
+0,
+1520,
+1520,
+1520,
+1520,
+1520,
+1520,
+1520,
+1520
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.lib	Wed Sep 04 03:59:40 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/garfieldsg/code/MPU6050/#1e0baaf91e96

--- a/Motion/OfflineMotion.cpp	Mon Aug 19 08:10:58 2013 +0000
+++ b/Motion/OfflineMotion.cpp	Wed Sep 04 03:59:40 2013 +0000
@@ -3,7 +3,7 @@
 //extern Ticker tick;
 extern DigitalOut led1;
 
-OfflineMotion::OfflineMotion(uint16_t** data, unsigned short int size_idx, unsigned char size_num, PWM* _pwm, bool* _playing)
+OfflineMotion::OfflineMotion(uint16_t** data, unsigned short int size_idx, unsigned char size_num, Adafruit_PWMServoDriver* _pwm, bool* _playing)
 {
     pwm = _pwm;
     
@@ -103,7 +103,7 @@
         m_dd_buf[i] = 0.0;
         
         __disable_irq();
-        pwm->SetDuty(i, (uint32_t)m_buf[i]);
+        pwm->setDuty(i, (uint32_t)m_buf[i]);
         __enable_irq();
     }
     m_brake_flg = 0.0;
@@ -119,7 +119,7 @@
         m_buf[i] += m_d_buf[i];
         
         __disable_irq();
-        pwm->SetDuty(i, (uint32_t)m_buf[i]);
+        pwm->setDuty(i, (uint32_t)m_buf[i]);
         __enable_irq();
     }
 }
\ No newline at end of file

--- a/Motion/OfflineMotion.h	Mon Aug 19 08:10:58 2013 +0000
+++ b/Motion/OfflineMotion.h	Wed Sep 04 03:59:40 2013 +0000
@@ -3,13 +3,14 @@
 
 #include "LPC17xx.h"
 #include "mbed.h"
-#include "PWM.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, PWM* _pwm, bool* playing);
+    OfflineMotion(uint16_t** data, unsigned short int size_idx, unsigned char size_num, Adafruit_PWMServoDriver* _pwm, bool* _playing);
     ~OfflineMotion();
   public:
     void step();
@@ -20,7 +21,8 @@
     void step_inter();
   
   private:
-    PWM* pwm;
+    //PWM* pwm;
+    Adafruit_PWMServoDriver* pwm;
     uint16_t** m_data;
     unsigned short int m_IDX_MAX;
     unsigned char m_NUM_MAX;

--- a/Motion/OnlineMotion.cpp	Mon Aug 19 08:10:58 2013 +0000
+++ b/Motion/OnlineMotion.cpp	Wed Sep 04 03:59:40 2013 +0000
@@ -1,9 +1,12 @@
 #include "OnlineMotion.h"
+#include "HomePosition.h"
 
 //extern Ticker tick;
 extern DigitalOut led1;
 
-OnlineMotion::OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, PWM* _pwm, bool* _playing)
+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;
@@ -95,20 +98,20 @@
     invertkinematics(rpos, rtheta);
     
     uint32_t pos[] = {
-        HOMEPOS[0],
-        HOMEPOS[1] + ltheta[0],
-        HOMEPOS[2] - ltheta[1],
-        HOMEPOS[3] - ltheta[2],
-        HOMEPOS[4] + ltheta[3],
-        HOMEPOS[5] + ltheta[0],
-        HOMEPOS[6],
-        HOMEPOS[7],
-        HOMEPOS[8],
+        HOMEPOS[0] + ltheta[0],
+        HOMEPOS[1] - ltheta[1],
+        HOMEPOS[2] - ltheta[2],
+        HOMEPOS[3] + ltheta[3],
+        HOMEPOS[4] + ltheta[0],
+        HOMEPOS[5] + rtheta[0],
+        HOMEPOS[6] + rtheta[1],
+        HOMEPOS[7] + rtheta[2],
+        HOMEPOS[8] - rtheta[3],
         HOMEPOS[9] + rtheta[0],
-        HOMEPOS[10] + rtheta[1],
-        HOMEPOS[11] + rtheta[2],
-        HOMEPOS[12] - rtheta[3],
-        HOMEPOS[13] + rtheta[0],
+        HOMEPOS[10],
+        HOMEPOS[11],
+        HOMEPOS[12],
+        HOMEPOS[13],
         HOMEPOS[14],
         HOMEPOS[15],
         HOMEPOS[16],
@@ -123,7 +126,7 @@
     
     for (int i = 0; i < m_NUM_MAX; ++i) {
         __disable_irq();
-        pwm->SetDuty(i, pos[i]);
+        pwm->setDuty(i, pos[i]);
         __enable_irq();
     }
 }
\ No newline at end of file

--- a/Motion/OnlineMotion.h	Mon Aug 19 08:10:58 2013 +0000
+++ b/Motion/OnlineMotion.h	Wed Sep 04 03:59:40 2013 +0000
@@ -6,16 +6,16 @@
 #include "LPC17xx.h"
 #include "mbed.h"
 #include "PWM.h"
+#include "Adafruit_PWMServoDriver.h"
 //#include "Motion.h"
 
-const float RADTOVAL = (SRV_MAX_DUTY - SRV_MIN_DUTY) / 3.14159265;
 
 const float M_PI = 3.14159265;
 
 class OnlineMotion //: public Motion
 {
   public:
-    OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, PWM* _pwm, bool* _playing);
+    OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, Adafruit_PWMServoDriver* _pwm, bool* _playing);
     ~OnlineMotion();
   public:
     void step();
@@ -26,7 +26,8 @@
     void walk(float* lpos, float* rpos, float h, float stride, float side, float up);
     
   private:
-    PWM* pwm;
+    Adafruit_PWMServoDriver* pwm;
+    //PWM* pwm;
     unsigned char m_NUM_MAX;
     bool* playing;
     volatile float t;

--- a/PWM.cpp	Mon Aug 19 08:10:58 2013 +0000
+++ b/PWM.cpp	Wed Sep 04 03:59:40 2013 +0000
@@ -1,4 +1,5 @@
 #include "PWM.h"
+#include "HomePosition.h"
 
 //static menber variable
 volatile uint8_t PWM::SRV_Idx = 0;
@@ -7,7 +8,7 @@
 PWM::PWM()
 {
     for (int i = 0; i < SRV_CH_NUM; i++) {
-        SetDuty(i, HOMEPOS[i]);
+        setDuty(i, HOMEPOS[i]);
     }
     InitPWM();
 }
@@ -39,7 +40,7 @@
     NVIC_EnableIRQ(PWM1_IRQn);//enable
 }
 
-void PWM::SetDuty(uint8_t ch, uint32_t duty)
+void PWM::setDuty(uint8_t ch, uint32_t duty)
 {
     if (ch >= SRV_CH_NUM) {
         return;

--- a/PWM.h	Mon Aug 19 08:10:58 2013 +0000
+++ b/PWM.h	Wed Sep 04 03:59:40 2013 +0000
@@ -20,39 +20,12 @@
     {0,0,0,0,SRV_PERIOD,SRV_PERIOD,SRV_PERIOD,SRV_PERIOD}
     };
 
-const uint16_t HOMEPOS[] = {
-1570,
-1490,
-1500,
-1820,
-1515,
-1500,
-0,
-0,
-1500,
-1500,
-1500,
-1220,
-1500,
-1500,
-0,
-0,
-1520,
-1520,
-1520,
-1520,
-1520,
-1520,
-1520,
-1520
-};
-
 class PWM {
   public:
     PWM();
   public:
     void InitPWM();
-    void SetDuty(uint8_t ch, uint32_t duty);
+    void setDuty(uint8_t ch, uint32_t duty);
   public:
     static volatile uint8_t SRV_Idx;
     static volatile uint32_t SRV_dutyTable[SRV_BANK_NUM][SRV_IDX_NUM];

--- a/main.cpp	Mon Aug 19 08:10:58 2013 +0000
+++ b/main.cpp	Wed Sep 04 03:59:40 2013 +0000
@@ -13,6 +13,6 @@
     
     while (1) {
         controlor.control();
-        wait(0.02);
+        wait(0.01);
     }
 }
\ No newline at end of file