Diff: AX12.cpp

Revision:

4:56c593ae9a45

Parent:

3:7b970151bf19

Child:

5:bae6dc62dfb4

--- a/AX12.cpp	Wed Dec 23 18:32:37 2015 +0000
+++ b/AX12.cpp	Mon Dec 28 17:19:02 2015 +0000
@@ -28,7 +28,7 @@
 StatusCode AX12::SetGoal(float degrees)
 {
     _LastError = statusValid;
-    short goal = (short)((1024.0f * degrees) / 300.0f);
+    short goal = (short)((1024.0f * (degrees - 30.0f)) / 300.0f);
 
     CommBuffer data;
     data.push_back( goal & 0xff );
@@ -67,12 +67,21 @@
     if( s == statusValid )
     {
         int16_t value = data[0] | (data[1] << 8);
-        float degrees = (float)value * 300.0f / 1024.0f;
+        float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
         _LastPosition = degrees;
         return degrees;
     }
     else
     {
+        // try again one time
+        s = _pbus->Read(_ID, ctPresentPositionL, 2, data); 
+        if( s == statusValid )
+        {
+            int16_t value = data[0] | (data[1] << 8);
+            float degrees = 30.0f + ((float)value * 300.0f / 1024.0f);
+            _LastPosition = degrees;
+            return degrees;
+        }
         _LastError = s;
         return 0.0f;
     }
@@ -92,8 +101,15 @@
     }
     else
     {
+        // try again one time
+        s = _pbus->Read(_ID, ctPresentTemperature, 1, data); 
+        if( s == statusValid )
+        {
+            _LastTemperature = (int)data[0];
+            return (int)data[0];
+        }
+
         _LastError = s;
-        _LastTemperature = -1;
         return 0;
     }
 }
@@ -113,8 +129,15 @@
     }
     else
     {
+        // try again one time
+        s = _pbus->Read(_ID, ctPresentVoltage, 1, data); 
+        if( s == statusValid )
+        {
+            float volts = (float)data[0] / 10.0f;
+            _LastVoltage = volts;
+            return volts;
+        }
         _LastError = s;
-        _LastVoltage = -1;
         return 0.0f;
     }
 }