Diff: OnlineMotion.cpp

Revision:

19:c2ec475367aa

Parent:

18:7077bedc37eb

--- a/OnlineMotion.cpp	Wed Apr 03 04:56:13 2013 +0000
+++ b/OnlineMotion.cpp	Sun Apr 07 06:59:33 2013 +0000
@@ -6,14 +6,7 @@
 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;
     *playing = true;
-    
-    //t = 0;
-    //T = _T;
-    //m_NUM_MAX = _size_num - 1;
-    //STEP = _TIMESTEP;
 }
 
 OnlineMotion::~OnlineMotion()
@@ -42,14 +35,13 @@
 {
     float L1 = 45.0;
     float L2 = 90.0;
-    float L, h;
     
     float x = pos[0];
     float y = pos[1];
     float z = pos[2];
     
-    L = sqrt(z*z + y*y) - 2 * L1;
-    h = sqrt(x*x + L*L);
+    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));
@@ -102,28 +94,6 @@
     invertkinematics(lpos, ltheta);
     invertkinematics(rpos, rtheta);
     
-    
-    //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];
-    pos[4] = HOMEPOS[4] + ltheta[3];
-    pos[5] = HOMEPOS[5] + ltheta[0];
-    
-    pos[9] = HOMEPOS[9] + rtheta[0];
-    pos[10] = HOMEPOS[10] + rtheta[1];
-    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],