Diff: Motion/OnlineMotion.cpp

Revision:

21:a54bcab078ed

Parent:

20:abb7852df747

Child:

22:bf5aa20b9df0

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