Diff: isr.cpp

Revision:

4:f18f6bc5e1fd

Parent:

3:86ccde39f61b

--- a/isr.cpp	Wed Feb 25 04:44:05 2015 +0000
+++ b/isr.cpp	Thu Feb 26 04:49:21 2015 +0000
@@ -75,34 +75,38 @@
 }
 
 void pos_update() {
-    float ascaled = 2 *(((float) analoga - 0.143f)/(0.618f - 0.143f) - 0.5f);
-    float bscaled = 2 *(((float) analogb - 0.202f)/(0.542f - 0.202f) - 0.5f);
-   
+    float ascaled = 2*(((float)analoga-0.256f)/(0.484f-0.256f)-0.5f);
+    float bscaled = 2*(((float)analogb-0.254f)/(0.474f-0.254f)-0.5f);
+    
     float x = bscaled / ascaled;
     
     unsigned int index = (abs(x) / ATAN_UPPER_BOUND)* ATAN_TABLE_SIZE;
     
     if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1;
-    
-    if(bscaled < 0){
-        if(ascaled < 0) motor->angle = arctan[index];
-        if(ascaled > 0) motor->angle = 180.0f - arctan[index];
-    } 
-    
-    if(bscaled > 0){
-        if(ascaled > 0) motor->angle = 180.0f + arctan[index];
-        if(ascaled < 0) motor->angle = 360.0f - arctan[index];
-    }
+
+    if(ascaled<0){
+           if(bscaled<0) motor->angle = 90 - arctan[index];
+           if(bscaled>0) motor->angle = 90 + arctan[index];
+   } 
+       
+   if(ascaled>0){
+           if(bscaled>0) motor->angle = 270 - arctan[index];
+           if(bscaled<0) motor->angle = 270 + arctan[index];
+   }
     
     if(motor->angle > 360.0f) motor->angle = 360.0f;
     if(motor->angle < 0) motor->angle = 0;
     
 #ifdef __DEBUG
-    if (!motor->halt) {
-        fbuffer[bufidx] = motor->angle;
-        bufidx++;
+    skipidx++;
+    if (skipidx == SKIP) {
+        skipidx = 0;
+        if (!motor->halt) {
+            fbuffer[bufidx] = motor->angle;
+            bufidx++;
+        }
     }
-    if (bufidx == 10000) {
+    if (bufidx == DBG_BUF_SZ) {
         motor->debug_stop = 1;
     }
 #endif