Diff: Motion/OnlineMotion.cpp

Revision:

22:bf5aa20b9df0

Parent:

21:a54bcab078ed

--- a/Motion/OnlineMotion.cpp	Wed Sep 04 03:59:40 2013 +0000
+++ b/Motion/OnlineMotion.cpp	Fri Sep 06 08:36:21 2013 +0000
@@ -98,16 +98,16 @@
     invertkinematics(rpos, rtheta);
     
     uint32_t pos[] = {
-        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[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],
@@ -124,9 +124,9 @@
         HOMEPOS[23]
     };
     
+    __disable_irq();
     for (int i = 0; i < m_NUM_MAX; ++i) {
-        __disable_irq();
         pwm->setDuty(i, pos[i]);
-        __enable_irq();
     }
+    __enable_irq();
 }
\ No newline at end of file