Diff: OnlineMotion.cpp

Revision:

18:7077bedc37eb

Parent:

17:60de3bfdc70b

Child:

19:c2ec475367aa

--- a/OnlineMotion.cpp	Tue Apr 02 04:19:09 2013 +0000
+++ b/OnlineMotion.cpp	Wed Apr 03 04:56:13 2013 +0000
@@ -1,20 +1,19 @@
 #include "OnlineMotion.h"
 
-#include <cmath>
-
 extern Ticker tick;
 extern DigitalOut led1;
 
 OnlineMotion::OnlineMotion(float _T, float _TIMESTEP, unsigned char _size_num, PWM* _pwm, bool* _playing)
+    : pwm(_pwm), playing(_playing), t(0), T(_T), m_NUM_MAX(_size_num - 1), STEP(_TIMESTEP)
 {
-    pwm = _pwm;
-    playing = _playing;
+    //pwm = _pwm;
+    //playing = _playing;
     *playing = true;
     
-    t = 0;
-    T = _T;
-    m_NUM_MAX = _size_num - 1;
-    STEP = _TIMESTEP;
+    //t = 0;
+    //T = _T;
+    //m_NUM_MAX = _size_num - 1;
+    //STEP = _TIMESTEP;
 }
 
 OnlineMotion::~OnlineMotion()
@@ -27,8 +26,8 @@
     if (t < T) {
         update();
     } else {
+        *playing = false;
         tick.detach();
-        *playing = false;
         delete this;
     }
 }
@@ -93,18 +92,22 @@
 
 void OnlineMotion::play_online()
 {
+    
     uint16_t ltheta[5], rtheta[5];
-    uint16_t* pos;
     
     float lpos[3], rpos[3];
     
-    walk(lpos, rpos, 1,1,1,1);
+    walk(lpos, rpos, 250, 20, 20, 15);
     
     invertkinematics(lpos, ltheta);
     invertkinematics(rpos, rtheta);
     
-    pos = (uint16_t*)HOMEPOS;
+    
+    //uint16_t* pos;
     
+    //pos = (uint16_t*)HOMEPOS;
+    
+    /*
     pos[1] = HOMEPOS[1] + ltheta[0];
     pos[2] = HOMEPOS[2] - ltheta[1];
     pos[3] = HOMEPOS[3] - ltheta[2];
@@ -116,10 +119,41 @@
     pos[11] = HOMEPOS[11] + rtheta[2];
     pos[12] = HOMEPOS[12] - rtheta[3];
     pos[13] = HOMEPOS[13] + rtheta[0];
+    */
+    
+    //pos[10] = 1520 + 500 * sin(t * M_PI / T);
+    //uint32_t poo = 1520 + 500 * sin(t * M_PI / T);
+    
+    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[9] + rtheta[0],
+        HOMEPOS[10] + rtheta[1],
+        HOMEPOS[11] + rtheta[2],
+        HOMEPOS[12] - rtheta[3],
+        HOMEPOS[13] + rtheta[0],
+        HOMEPOS[14],
+        HOMEPOS[15],
+        HOMEPOS[16],
+        HOMEPOS[17],
+        HOMEPOS[18],
+        HOMEPOS[19],
+        HOMEPOS[20],
+        HOMEPOS[21],
+        HOMEPOS[22],
+        HOMEPOS[23]
+    };
     
     for (int i = 0; i < m_NUM_MAX; ++i) {
         __disable_irq();
-        pwm->SetDuty(i, (uint32_t)pos[i]);
+        pwm->SetDuty(i, pos[i]);
         __enable_irq();
     }
 }
\ No newline at end of file